bgc-net/BasicGeometry/Vector2FP64.cs

/*
 * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using System;

/*
 * Author: Andrey Pokidov
 * Date: 1 Feb 2019
 */

namespace BasicGeometry
{
    public struct Vector2FP64
    {
        public static readonly Vector2FP64 ZERO = new Vector2FP64(0.0, 0.0);

        public double x1 = 0.0;
        public double x2 = 0.0;

        public Vector2FP64(double x1, double x2)
        {
            this.x1 = x1;
            this.x2 = x2;
        }

        public Vector2FP64(in Vector2FP64 vector)
        {
            this.x1 = vector.x1;
            this.x2 = vector.x2;
        }

        public Vector2FP64(in Vector2FP32 vector)
        {
            this.x1 = vector.x1;
            this.x2 = vector.x2;
        }

        public readonly double GetSquareModulus()
        {
            return this.x1 * this.x1 + this.x2 * this.x2;
        }

        public readonly double GetModulus()
        {
            return Math.Sqrt(this.GetSquareModulus());
        }

        public int Normalize()
        {
            double squareModulus = this.GetSquareModulus();

            if (UtilityFP64.IsSqareUnit(squareModulus))
            {
                return 1;
            }

            if (squareModulus <= UtilityFP64.SQUARE_EPSYLON)
            {
                this.Reset();
                return 0;
            }

            double multiplier = Math.Sqrt(1.0 / squareModulus);

            this.x1 *= multiplier;
            this.x2 *= multiplier;

            return 1;
        }

        public void Reverse()
        {
            this.x1 = -this.x1;
            this.x2 = -this.x2;
        }

        public readonly bool IsZero()
        {
            return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON;
        }

        public readonly bool IsUnit()
        {
            return UtilityFP64.IsSqareUnit(this.GetSquareModulus());
        }

        public void Reset()
        {
            this.x1 = 0.0;
            this.x2 = 0.0;
        }

        public void SetValues(double x1, double x2)
        {
            this.x1 = x1;
            this.x2 = x2;
        }

        public void SetValues(in Vector2FP64 vector)
        {
            this.x1 = vector.x1;
            this.x2 = vector.x2;
        }

        public void SetValues(in Vector2FP32 vector)
        {
            this.x1 = vector.x1;
            this.x2 = vector.x2;
        }

        public void SetReverseOf(in Vector2FP64 vector)
        {
            this.x1 = -vector.x1;
            this.x2 = -vector.x2;
        }

        public void SetReverseOf(in Vector2FP32 vector)
        {
            this.x1 = -vector.x1;
            this.x2 = -vector.x2;
        }

        public void AppendScaled(Vector2FP64 summand, double scale)
        {
            this.x1 += summand.x1 * scale;
            this.x2 += summand.x2 * scale;
        }

        public readonly override string ToString()
        {
            return String.Format("DPVector2({0}, {1})", this.x1, this.x2);
        }

        public static void Add(in Vector2FP64 vector1, in Vector2FP64 vector2, out Vector2FP64 sum)
        {
            sum.x1 = vector1.x1 + vector2.x1;
            sum.x2 = vector1.x2 + vector2.x2;
        }

        public static void Subtract(in Vector2FP64 minuend, in Vector2FP64 subtrahend, out Vector2FP64 difference)
        {
            difference.x1 = minuend.x1 - subtrahend.x1;
            difference.x2 = minuend.x2 - subtrahend.x2;
        }

        public static void Multiply(in Vector2FP64 multiplicand, double multiplier, out Vector2FP64 product)
        {
            product.x1 = multiplicand.x1 * multiplier;
            product.x2 = multiplicand.x2 * multiplier;
        }

        public static void Divide(in Vector2FP64 dividend, double divisor, out Vector2FP64 quotient)
        {
            Multiply(dividend, 1.0 / divisor, out quotient);
        }

        public static void GetMean2(in Vector2FP64 vector1, in Vector2FP64 vector2, out Vector2FP64 result)
        {
            result.x1 = (vector1.x1 + vector2.x1) * 0.5;
            result.x2 = (vector1.x2 + vector2.x2) * 0.5;
        }

        public static void GetMean3(in Vector2FP64 vector1, in Vector2FP64 vector2, in Vector2FP64 vector3, out Vector2FP64 result)
        {
            result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD;
            result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD;
        }

        public static double GetScalarProduct(in Vector2FP64 vector1, in Vector2FP64 vector2)
        {
            return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2;
        }
 
        public static double GetCrossProduct(in Vector2FP64 vector1, in Vector2FP64 vector2)
        {
            return vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1;
        }

        public static double GetAngle(in Vector2FP64 vector1, in Vector2FP64 vector2, AngleUnit unit)
        {
            double squareModulus1 = vector1.GetSquareModulus();

            if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON)
            {
                return 0.0;
            }

            double squareModulus2 = vector2.GetSquareModulus();

            if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON)
            {
                return 0.0;
            }

            double cosine = Vector2FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2);

            if (1.0 - UtilityFP64.EPSYLON <= cosine)
            {
                return 0.0;
            }

            if (cosine <= -(1.0 - UtilityFP64.EPSYLON))
            {
                return AngleFP64.GetHalfCircle(unit);
            }
    
            return RadianFP64.ToUnits(Math.Acos(cosine), unit);
        }

        public static double GetSquareDistance(in Vector2FP64 vector1, in Vector2FP64 vector2)
        {
            double dx1 = vector1.x1 - vector2.x1;
            double dx2 = vector1.x2 - vector2.x2;
        
            return dx1 * dx1 + dx2 * dx2;
        }
    
        public static double GetDistance(in Vector2FP64 vector1, in Vector2FP64 vector2)
        {
            return Math.Sqrt(GetSquareDistance(vector1, vector2));
        }
    
        public static bool AreEqual(in Vector2FP64 vector1, in Vector2FP64 vector2)
        {
            double squareModulus1 = vector1.GetSquareModulus();
            double squareModulus2 = vector2.GetSquareModulus();
            double squareModulus3 = GetSquareDistance(vector1, vector2);

            // 2.0 means dimension amount
            if (squareModulus1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT)
            {
                return squareModulus3 < (2.0 * UtilityFP64.SQUARE_EPSYLON);
            }

            if (squareModulus1 <= squareModulus2)
            {
                return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus2;
            }

            return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus1;
        }
    }
}