LGES-OC5-PtoP-Welder/AuroraVision/VisionCommon/Common/MathUtil.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace VisionCommon {
    public class MathUtil {
        private const double DELTA_EQUALITY = 0.00000001;

        public static double ConvertDegreeToRadian(double angle) {
            return angle * (Math.PI / 180.0);
        }

        public static double ConvertRadianToDegree(double angle) {
            return angle * (180.0 / Math.PI);
        }

        public static double CalculateDistance(DoublePoint p1, DoublePoint p2) {
            var diff = p1 - p2;
            return Math.Sqrt(Math.Pow(diff.X, 2) + Math.Pow(diff.Y, 2));
        }

        public static DoublePoint CalculateCenter(DoublePoint p1, DoublePoint p2) {
            return (p1 + p2) / 2;
        }

        public static double ConvertUnitSecToMilli(double value) {
            return value * 1000;
        }

        public static double ConvertUnitMilliToSec(double value) {
            return value / 1000;
        }

        public static double ConvertUnitMilliToMicro(double value) {
            return value * 1000;
        }

        public static double ConvertUnitMicroToMilli(double value) {
            return value / 1000;
        }

        public static DoublePoint TranslateRotateScale(
            DoublePoint point,
            Angle angle, DoublePoint origin,
            double scale,
            DoublePoint translation) {
            var translatedPoint = point + (translation / scale);
            var rotatedPoint = Rotate(translatedPoint, angle, origin);
            var scaledPoint = rotatedPoint * scale;

            return scaledPoint;
        }

        public static DoublePoint TranslateRotateScale(
            DoublePoint point,
            Angle angle,
            double scale,
            DoublePoint translation) {
            var translatedPoint = point + (translation / scale);
            var rotatedPoint = Rotate(translatedPoint, angle);
            var scaledPoint = rotatedPoint * scale;

            return scaledPoint;
        }

        public static DoublePoint RotateTranslateScale(
            DoublePoint point,
            Angle angle, DoublePoint origin,
            double scale,
            DoublePoint translation) {
            var rotatedPoint = Rotate(point, angle, origin);
            var translatedPoint = rotatedPoint + (translation / scale);
            var scaledPoint = translatedPoint * scale;

            return scaledPoint;
        }

        public static DoublePoint ScaleRotate(
            DoublePoint point,
            Angle angle,
            double scaleX,
            double scaleY) {
            var scaledPoint = DoublePoint.Transform(
                point,
                new TransformScaleTranslation() {
                    ScaleX = scaleX,
                    ScaleY = scaleY,
                });
            var rotatedPoint = Rotate(scaledPoint, angle);

            return rotatedPoint;
        }

        public static List<DoublePoint> RotatePoints(List<DoublePoint> points, Angle angle, DoublePoint thetaOrigin) {
            var rotatedPoints = new List<DoublePoint>();

            foreach (var p in points) {
                rotatedPoints.Add(Rotate(p, angle, thetaOrigin));
            }

            return rotatedPoints;
        }

        public static DoublePoint Rotate(DoublePoint point, Angle angle, DoublePoint origin) {
            var rotatedPoint = RotateImpl(point - origin, angle);

            return rotatedPoint + origin;
        }

        public static DoublePoint Rotate(DoublePoint point, Angle angle) {
            return RotateImpl(point, angle);
        }

        public static DoublePoint RotateFlip(DoublePoint point, Angle angle, eFlipY flipY) {
            var rotatedPoint = RotateImpl(point, angle);
            rotatedPoint.Y *= (int)flipY;
            return rotatedPoint;
        }

        private static DoublePoint RotateImpl(DoublePoint point, Angle angle) {
            // Rotate point in Image Coordinate System
            double cosVal = Math.Cos(angle.Radian);
            double sinVal = Math.Sin(angle.Radian);

            var rotatedPoint = point.Clone();
            rotatedPoint.X = (point.X * cosVal) - (point.Y * sinVal);
            rotatedPoint.Y = (point.X * sinVal) + (point.Y * cosVal);

            return rotatedPoint;
        }

        public static Angle GetLineAngle(DoublePoint p1, DoublePoint p2) {
            var diff = p2 - p1;
            double angleRad = Math.Atan2(diff.Y, diff.X);

            return Angle.CreateWithRadian(angleRad);
        }

        public static bool Equals(double d1, double d2, double deltaEquality = DELTA_EQUALITY) {
            if (Math.Abs(d1 - d2) > deltaEquality) {
                return false;
            }

            return true;
        }
    }
}