Metis_xctg/AutoAnalysis/FocalPlanes3D.cs

using StageController;
using System;
using System.Collections.Generic;

namespace Metis.AutoAnalysis
{
    public class FocalPlanes3D
    {
        private BasePointEnum basePoint;

        private double A, B, C, D;

        private Point3D center;
        private List<double> As = new List<double>();
        private List<double> Bs = new List<double>();
        private List<double> Cs = new List<double>();
        private List<double> Ds = new List<double>();
        private List<Vector> vectors = new List<Vector>();

        public FocalPlanes3D(Point3D p1, Point3D p2, Point3D p3)
            : this(p1.X, p1.Y, p1.Z, p2.X, p2.Y, p2.Z, p3.X, p3.Y, p3.Z) { }

        public FocalPlanes3D(double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3)
        {
            if (x1 == x2 && x2 == x3 || y1 == y2 && y2 == y3)
            {
                throw new ArgumentException("Three points were in a same straight line.");
            }

            basePoint = BasePointEnum.Threee;

            A = (y2 - y1) * (z3 - z1) - (y3 - y1) * (z2 - z1);
            B = (z2 - z1) * (x3 - x1) - (z3 - z1) * (x2 - x1);
            C = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
            D = -A * x1 - B * y1 - C * z1;
        }

        public FocalPlanes3D(Point3D center, List<Point3D> points)
        {
            if (center == null || points == null || points.Count < 3)
            {
                throw new ArgumentNullException("Argument is null, the number of argumentor is Lless than four.");
            }

            basePoint = BasePointEnum.Multi;

            this.center = center;
            for (int i = 0; i < points.Count; i++)
            {
                var next = i + 1 == points.Count ? 0 : i + 1;
                if (center.X == points[i].X && center.X == points[next].X || center.Y == points[i].Y && center.Y == points[next].Y)
                {
                    throw new ArgumentException("Three points were in a same straight line.");
                }

                vectors.Add(new Vector(center.X - points[i].X, center.Y - points[i].Y, 0));
                As.Add((points[i].Y - center.Y) * (points[next].Z - center.Z) - (points[next].Y - center.Y) * (points[i].Z - center.Z));
                Bs.Add((points[i].Z - center.Z) * (points[next].X - center.X) - (points[next].Z - center.Z) * (points[i].X - center.X));
                Cs.Add((points[i].X - center.X) * (points[next].Y - center.Y) - (points[next].X - center.X) * (points[i].Y - center.Y));
                Ds.Add(-As[i] * center.X - Bs[i] * center.Y - Cs[i] * center.Z);
            }

            //Logs.WriteFocus($"\tCenter=({center.X}, {center.Y}, {center.Z})");
            //for (int i=0;i<points.Count; i++)
            //{
            //    Logs.WriteFocus($"\ti={i+1},\t({points[i].X}, {points[i].Y}, {points[i].Z})");
            //}
        }

        public double GetFocusValue(double x, double y)
        {
            if (basePoint == BasePointEnum.Threee)
            {
                var z = (-D - A * x - B * y) / C;
                //Logs.WriteFocus($"\ti={0},\tx={x}, y={y}, \tz={z}");
                return z;
            }
            else
            {
                var vector = new Vector(center.X - x, center.Y - y, 0);
                for (int i = 0; i < vectors.Count; i++)
                {
                    var next = i + 1 == vectors.Count ? 0 : i + 1;
                    var angle = vectors[i] * vectors[next] / (vectors[i].Model * vectors[next].Model);
                    var angleHalf1 = vectors[i] * vector / (vectors[i].Model * vector.Model);
                    var angleHalf2 = vector * vectors[next] / (vector.Model * vectors[next].Model);
                    if (Math.Abs(Math.Acos(angle) - Math.Acos(angleHalf1) - Math.Acos(angleHalf2)) <= 1e-6)
                    {
                        var z = (-Ds[i] - As[i] * x - Bs[i] * y) / Cs[i];
                        //Logs.WriteFocus($"\tNo={i+1}, \tx={x}, \ty={y}, \tz={z}");
                        return z;
                    }
                }
                throw new Exception("Space sector not closed.");
            }
        }

        public int GetAreaNo(double x, double y)
        {
            if (basePoint == BasePointEnum.Threee)
            {
                return -1;
            }
            else
            {
                var vector = new Vector(center.X - x, center.Y - y, 0);
                for (int i = 0; i < vectors.Count; i++)
                {
                    var next = i + 1 == vectors.Count ? 0 : i + 1;
                    var angle = vectors[i] * vectors[next] / (vectors[i].Model * vectors[next].Model);
                    var angleHalf1 = vectors[i] * vector / (vectors[i].Model * vector.Model);
                    var angleHalf2 = vector * vectors[next] / (vector.Model * vectors[next].Model);
                    if (Math.Abs(Math.Acos(angle) - Math.Acos(angleHalf1) - Math.Acos(angleHalf2)) <= 1e-6)
                    {
                        return i + 1;
                    }
                }
                throw new Exception("Space sector not closed.");
            }
        }
    }

    public enum BasePointEnum
    {
        Threee,

        Multi,
    }

    public class Point3D
    {
        public double X { get; set; }

        public double Y { get; set; }

        public double Z { get; set; }

        public Point3D(double x, double y, double z)
        {
            X = x; Y = y; Z = z;
        }
    }

    public class Vector
    {
        private double x, y, z;

        public Vector(double x, double y, double z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public double Model => Math.Sqrt(x * x + y * y + z * z);

        public static Vector operator +(Vector AB, Vector AC)
        {
            return new Vector(AB.x + AC.x, AB.y + AC.y, AB.z + AC.z);
        }

        public static Vector operator -(Vector AB, Vector AC)
        {
            return new Vector(AB.x - AC.x, AB.y - AC.y, AB.z - AC.z);
        }

        public static Vector operator ^(Vector AB, Vector AC)
        {
            return new Vector(AB.y * AC.z - AC.y * AB.z, AC.x * AB.z - AB.x * AC.z, AB.x * AC.y - AC.x * AB.y);
        }

        public static double operator *(Vector AB, Vector AC)
        {
            return AB.x * AC.x + AB.y * AC.y + AB.z * AC.z;
        }
    }
}