ActiveProtect/Models/Common.cs

using System;

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>
        /// <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>
    /// 表示三维空间中的方向
    /// </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;
        }

        /// <summary>
        /// 根据给定的方向向量创建方向
        /// </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);
        }
    }

    public struct Vector2D
    {
        public double X { get; set; }
        public double Y { get; set; }

        public Vector2D(double x, double y)
        {
            X = x;
            Y = y;
        }

        public static Vector2D Zero => new Vector2D(0, 0);
    }
}