ActiveProtect/Models/Common.cs

using System;
using System.Numerics;

namespace ActiveProtect.Models
{
    /// <summary>
    /// 表示三维空间中的向量
    /// </summary>
    public class Vector3D
    {
        // 添加静态单位向量属性
        public static Vector3D UnitX => new Vector3D(1, 0, 0);
        public static Vector3D UnitY => new Vector3D(0, 1, 0);
        public static Vector3D UnitZ => new Vector3D(0, 0, 1);
        public static Vector3D Zero => new Vector3D(0, 0, 0);

        /// <summary>
        /// X 坐标
        /// </summary>
        public double X { get; set; }

        /// <summary>
        /// Y 坐标
        /// </summary>
        public double Y { get; set; }

        /// <summary>
        /// Z 坐标
        /// </summary>
        public double Z { get; set; }

        /// <summary>
        /// 构造函数
        /// </summary>
        public Vector3D(double x, double y, double z)
        {
            X = x;
            Y = y;
            Z = z;
        }

        /// <summary>
        /// 计算两个向量之间的距离
        /// </summary>
        /// <param name="v1">第一个向量</param>
        /// <param name="v2">第二个向量</param>
        /// <returns>两个向量之间的距离</returns>
        public static double Distance(Vector3D v1, Vector3D v2)
        {
            return Math.Sqrt(Math.Pow(v1.X - v2.X, 2) + Math.Pow(v1.Y - v2.Y, 2) + Math.Pow(v1.Z - v2.Z, 2));
        }


        /// <summary>
        /// 将向量转换为字符串表示
        /// </summary>
        /// <returns>向量的字符串表示</returns>
        public override string ToString()
        {
            return $"({X:F2}, {Y:F2}, {Z:F2})";
        }

        /// <summary>
        /// 向量减法运算符重载
        /// </summary>
        public static Vector3D operator -(Vector3D a, Vector3D b)
        {
            return new Vector3D(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
        }

        /// <summary>
        /// 向量加法运算符重载
        /// </summary>
        public static Vector3D operator +(Vector3D a, Vector3D b)
        {
            return new Vector3D(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
        }

        /// <summary>
        /// 向量与标量乘法运算符重载
        /// </summary>
        public static Vector3D operator *(Vector3D a, double scalar)
        {
            return new Vector3D(a.X * scalar, a.Y * scalar, a.Z * scalar);
        }

        /// <summary>
        /// 向量与标量除法运算符重载
        /// </summary>
        public static Vector3D operator /(Vector3D a, double scalar)
        {
            return new Vector3D(a.X / scalar, a.Y / scalar, a.Z / scalar);
        }

        /// <summary>
        /// 向量反向
        /// </summary>
        public static Vector3D operator -(Vector3D a)
        {
            return new Vector3D(-a.X, -a.Y, -a.Z);
        }

        /// <summary>
        /// 向量相等运算符重载
        /// </summary>
        public static bool operator ==(Vector3D left, Vector3D right)
        {
            return left.X == right.X && left.Y == right.Y && left.Z == right.Z;
        }

        /// <summary>
        /// 向量不相等运算符重载
        /// </summary>
        public static bool operator !=(Vector3D left, Vector3D right)
        {
            return !(left == right);
        }

        /// <summary>
        /// 判断两个向量是否相等
        /// </summary>
        public override bool Equals(object? obj)
        {
            if (obj is Vector3D other)
            {
                return this == other;
            }
            return false;
        }

        /// <summary>
        /// 获取向量的哈希码
        /// </summary>
        /// <returns>向量的哈希码</returns>
        public override int GetHashCode()
        {
            return HashCode.Combine(X, Y, Z);
        }

        /// <summary>
        /// 计算向量的模长
        /// </summary>
        /// <returns>向量的模长</returns>
        public double Magnitude()
        {
            return Math.Sqrt(X * X + Y * Y + Z * Z);
        }

        /// <summary>
        /// 计算向量模长的平方
        /// </summary>
        /// <returns>向量模长的平方</returns>
        public double MagnitudeSquared()
        {
            return X * X + Y * Y + Z * Z;
        }

        /// <summary>
        /// 向量归一化
        /// </summary>
        /// <returns>归一化后的向量</returns>
        public Vector3D Normalize()
        {
            double mag = Magnitude();
            if (mag > 0)
            {
                return new Vector3D(X / mag, Y / mag, Z / mag);
            }
            return new Vector3D(0, 0, 0);
        }

        /// <summary>
        /// 计算两个向量的叉积
        /// </summary>
        /// <param name="a">第一个向量</param>
        /// <param name="b">第二个向量</param>
        /// <returns>叉积结果</returns>
        public static Vector3D CrossProduct(Vector3D a, Vector3D b)
        {
            return new Vector3D(
                a.Y * b.Z - a.Z * b.Y,
                a.Z * b.X - a.X * b.Z,
                a.X * b.Y - a.Y * b.X
            );
        }

        /// <summary>
        /// 计算两个向量的点积
        /// </summary>
        /// <param name="a">第一个向量</param>
        /// <param name="b">第二个向量</param>
        /// <returns>点积结果</returns>
        public static double DotProduct(Vector3D a, Vector3D b)
        {
            return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
        }

        /// <summary>
        /// 向量取反
        /// </summary>
        /// <param name="a">输入向量</param>
        /// <returns>取反后的向量</returns>
        public static Vector3D Negate(Vector3D a)
        {
            return new Vector3D(-a.X, -a.Y, -a.Z);
        }

        /// <summary>
        /// 计算AB两点之间距离 A 点一定距离的点的坐标
        /// </summary>
        /// <param name="a">直线起点</param>
        /// <param name="b">直线终点</param>
        /// <param name="distance">距离起点距离</param>
        /// <returns>直线上的点</returns>
        public static Vector3D PointOnLine(Vector3D a, Vector3D b, double distance)
        {
            Vector3D direction = b - a;
            Vector3D unitDirection = direction.Normalize();
            return a + unitDirection * distance;
        }

        internal Vector3D Rotate(Orientation orientation, double misalignmentAngle)
        {
            // 首先，根据方向和失调角计算出旋转矩阵
            Matrix4x4 rotationMatrix = CalculateRotationMatrix(orientation, misalignmentAngle);

            // 然后，使用旋转矩阵对当前向量进行旋转
            Vector3D rotatedVector = ApplyRotationMatrix(this, rotationMatrix);
            return rotatedVector;
        }

        private static Matrix4x4 CalculateRotationMatrix(Orientation orientation, double misalignmentAngle)
        {
            // 创建旋转矩阵
            Matrix4x4 yawRotation = Matrix4x4.CreateRotationY((float)orientation.Yaw);
            Matrix4x4 pitchRotation = Matrix4x4.CreateRotationX((float)orientation.Pitch);
            Matrix4x4 rollRotation = Matrix4x4.CreateRotationZ((float)orientation.Roll);
            Matrix4x4 misalignmentRotation = Matrix4x4.CreateRotationY((float)misalignmentAngle);

            // 组合旋转矩阵
            return misalignmentRotation * yawRotation * pitchRotation * rollRotation;
        }

        private static Vector3D ApplyRotationMatrix(Vector3D vector, Matrix4x4 matrix)
        {
            double x = vector.X * matrix.M11 + vector.Y * matrix.M21 + vector.Z * matrix.M31 + matrix.M41;
            double y = vector.X * matrix.M12 + vector.Y * matrix.M22 + vector.Z * matrix.M32 + matrix.M42;
            double z = vector.X * matrix.M13 + vector.Y * matrix.M23 + vector.Z * matrix.M33 + matrix.M43;
            return new Vector3D(x, y, z);
        }
    }

    /// <summary>
    /// 表示三维空间中的方向
    /// </summary>
    public struct Orientation
    {
        /// <summary>
        /// 偏航角（绕Y轴旋转）
        /// </summary>
        public double Yaw { get; set; }

        /// <summary>
        /// 俯仰角（绕X轴旋转）
        /// </summary>
        public double Pitch { get; set; }

        /// <summary>
        /// 滚转角（绕Z轴旋转）
        /// </summary>
        public double Roll { get; set; }

        /// <summary>
        /// 构造函数
        /// </summary>
        public Orientation(double yaw, double pitch, double roll)
        {
            Yaw = yaw;
            Pitch = pitch;
            Roll = roll;
        }

        /// <summary>
        /// 将方向转换为字符串表示
        /// </summary>
        /// <returns>方向的字符串表示</returns>
        public override readonly string ToString()
        {
            return $"(Yaw: {Yaw:F2}, Pitch: {Pitch:F2}, Roll: {Roll:F2})";
        }

        /// <summary>
        /// 将角度归一化到 [-π, π] 范围内
        /// </summary>
        public void Normalize()
        {
            Yaw = NormalizeAngle(Yaw);
            Pitch = NormalizeAngle(Pitch);
            Roll = NormalizeAngle(Roll);
        }

        /// <summary>
        /// 将单个角度归一化到 [-π, π] 范围内
        /// </summary>
        /// <param name="angle">输入角度</param>
        /// <returns>归一化后的角度</returns>
        private static double NormalizeAngle(double angle)
        {
            while (angle > Math.PI) angle -= 2 * Math.PI;
            while (angle <= -Math.PI) angle += 2 * Math.PI;
            return angle;
        }

        internal Vector3D Rotate(Vector3D vector, double misalignmentAngle)
        {
            throw new NotImplementedException();
        }

        /// <summary>
        /// 根<><E6A0B9><EFBFBD>给定的方向向量创建方向
        /// </summary>
        /// <param name="direction">方向向量</param>
        /// <returns>对应的方向</returns>
        public static Orientation LookAt(Vector3D direction)
        {
            double yaw = Math.Atan2(direction.Z, direction.X);
            double pitch = Math.Atan2(direction.Y, Math.Sqrt(direction.X * direction.X + direction.Z * direction.Z));
            return new Orientation(yaw, pitch, 0);
        }

        /// <summary>
        /// 将方向转换为单位向量
        /// </summary>
        /// <returns>对应的单位向量</returns>
        public readonly Vector3D ToVector()
        {
            double cosYaw = Math.Cos(Yaw);
            double sinYaw = Math.Sin(Yaw);
            double cosPitch = Math.Cos(Pitch);
            double sinPitch = Math.Sin(Pitch);

            return new Vector3D(
                cosYaw * cosPitch,
                sinPitch,
                sinYaw * cosPitch
            );
        }

        /// <summary>
        /// 从向量创建方向
        /// </summary>
        /// <param name="vector">输入向量</param>
        /// <returns>对应的方向</returns>
        public static Orientation FromVector(Vector3D vector)
        {
            Vector3D normalized = vector.Normalize();
            double pitch = Math.Asin(normalized.Y);
            double yaw;
            if (Math.Abs(normalized.Y) > 0.9999) // 接近垂直
            {
                yaw = 0; // 或者保持之前的偏航角
            }
            else
            {
                yaw = Math.Atan2(normalized.Z, normalized.X);
            }
            return new Orientation(yaw, pitch, 0);
        }
    }

    /// <summary>
    /// 表示二维空间中的向量
    /// </summary>
    public struct Vector2D
    {
        public double X { get; set; }
        public double Y { get; set; }

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="x">X坐标</param>
        /// <param name="y">Y坐标</param>
        public Vector2D(double x, double y)
        {
            X = x;
            Y = y;
        }

        /// <summary>
        /// 零向量
        /// </summary>
        public static Vector2D Zero => new Vector2D(0, 0);
    }
}