383 lines
12 KiB
C#
383 lines
12 KiB
C#
/*
|
|
* 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 Geometry
|
|
{
|
|
public struct DPVector2
|
|
{
|
|
public static readonly DPVector2 ZERO = new DPVector2(0.0, 0.0);
|
|
|
|
public double x1;
|
|
public double x2;
|
|
|
|
public DPVector2(double x1, double x2)
|
|
{
|
|
this.x1 = x1;
|
|
this.x2 = x2;
|
|
}
|
|
|
|
public DPVector2(in DPVector2 vector)
|
|
{
|
|
this.x1 = vector.x1;
|
|
this.x2 = vector.x2;
|
|
}
|
|
|
|
public DPVector2(in SPVector2 vector)
|
|
{
|
|
this.x1 = vector.x1;
|
|
this.x2 = vector.x2;
|
|
}
|
|
|
|
public readonly double GetSquareModule()
|
|
{
|
|
return this.x1 * this.x1 + this.x2 * this.x2;
|
|
}
|
|
|
|
public readonly double GetModule()
|
|
{
|
|
return Math.Sqrt(this.GetSquareModule());
|
|
}
|
|
|
|
public int Normalize()
|
|
{
|
|
double squareModule = this.GetSquareModule();
|
|
|
|
if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON)
|
|
{
|
|
return 1;
|
|
}
|
|
|
|
if (squareModule <= DPUtility.SQUARE_EPSYLON)
|
|
{
|
|
this.Reset();
|
|
return 0;
|
|
}
|
|
|
|
double module = Math.Sqrt(squareModule);
|
|
|
|
this.x1 /= module;
|
|
this.x2 /= module;
|
|
|
|
return 1;
|
|
}
|
|
|
|
public readonly DPVector2 GetNormalized()
|
|
{
|
|
double squareModule = this.GetSquareModule();
|
|
|
|
if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON)
|
|
{
|
|
return this;
|
|
}
|
|
|
|
if (squareModule <= DPUtility.SQUARE_EPSYLON)
|
|
{
|
|
return ZERO;
|
|
}
|
|
|
|
double module = Math.Sqrt(squareModule);
|
|
|
|
return new DPVector2(
|
|
this.x1 / module,
|
|
this.x2 / module
|
|
);
|
|
}
|
|
|
|
public void Invert()
|
|
{
|
|
this.x1 = -this.x1;
|
|
this.x2 = -this.x2;
|
|
}
|
|
|
|
public readonly DPVector2 GetInverted()
|
|
{
|
|
return new DPVector2(-this.x1, -this.x2);
|
|
}
|
|
|
|
public readonly bool IsZero()
|
|
{
|
|
return this.GetSquareModule() <= DPUtility.SQUARE_EPSYLON;
|
|
}
|
|
|
|
public readonly bool IsUnit()
|
|
{
|
|
double squareModule = this.GetSquareModule();
|
|
return 1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= DPUtility.EPSYLON;
|
|
}
|
|
|
|
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 SPVector2 vector)
|
|
{
|
|
this.x1 = vector.x1;
|
|
this.x2 = vector.x2;
|
|
}
|
|
|
|
public void SetValues(in DPVector2 vector)
|
|
{
|
|
this.x1 = vector.x1;
|
|
this.x2 = vector.x2;
|
|
}
|
|
|
|
public readonly override string ToString()
|
|
{
|
|
return String.Format("DPVector2({0}, {1})", this.x1, this.x2);
|
|
}
|
|
|
|
public static void Add(in DPVector2 vector1, in DPVector2 vector2, out DPVector2 sum)
|
|
{
|
|
sum.x1 = vector1.x1 + vector2.x1;
|
|
sum.x2 = vector1.x2 + vector2.x2;
|
|
}
|
|
|
|
public static void Add3(
|
|
in DPVector2 vector1,
|
|
in DPVector2 vector2,
|
|
in DPVector2 vector3,
|
|
out DPVector2 sum
|
|
)
|
|
{
|
|
sum.x1 = vector1.x1 + vector2.x1 + vector3.x1;
|
|
sum.x2 = vector1.x2 + vector2.x2 + vector3.x2;
|
|
}
|
|
|
|
public static void Add4(
|
|
in DPVector2 vector1,
|
|
in DPVector2 vector2,
|
|
in DPVector2 vector3,
|
|
in DPVector2 vector4,
|
|
out DPVector2 sum
|
|
)
|
|
{
|
|
sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1);
|
|
sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2);
|
|
}
|
|
|
|
public static void Add5(
|
|
in DPVector2 vector1,
|
|
in DPVector2 vector2,
|
|
in DPVector2 vector3,
|
|
in DPVector2 vector4,
|
|
in DPVector2 vector5,
|
|
out DPVector2 sum
|
|
)
|
|
{
|
|
sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1;
|
|
sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2;
|
|
}
|
|
|
|
public static void Subtract(in DPVector2 minuend, in DPVector2 subtrahend, out DPVector2 difference)
|
|
{
|
|
difference.x1 = minuend.x1 - subtrahend.x1;
|
|
difference.x2 = minuend.x2 - subtrahend.x2;
|
|
}
|
|
|
|
public static void GetWeightedSum2(
|
|
double weight1, in DPVector2 vector1,
|
|
double weight2, in DPVector2 vector2,
|
|
out DPVector2 sum
|
|
)
|
|
{
|
|
sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2;
|
|
sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2;
|
|
}
|
|
|
|
public static void GetWeightedSum3(
|
|
double weight1, in DPVector2 vector1,
|
|
double weight2, in DPVector2 vector2,
|
|
double weight3, in DPVector2 vector3,
|
|
out DPVector2 sum
|
|
)
|
|
{
|
|
sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2 + vector3.x1 * weight3;
|
|
sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2 + vector3.x2 * weight3;
|
|
}
|
|
|
|
public static void GetWeightedSum4(
|
|
double weight1, in DPVector2 vector1,
|
|
double weight2, in DPVector2 vector2,
|
|
double weight3, in DPVector2 vector3,
|
|
double weight4, in DPVector2 vector4,
|
|
out DPVector2 sum
|
|
)
|
|
{
|
|
sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4);
|
|
sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4);
|
|
}
|
|
|
|
public static void GetWeightedSum5(
|
|
double weight1, in DPVector2 vector1,
|
|
double weight2, in DPVector2 vector2,
|
|
double weight3, in DPVector2 vector3,
|
|
double weight4, in DPVector2 vector4,
|
|
double weight5, in DPVector2 vector5,
|
|
out DPVector2 sum
|
|
)
|
|
{
|
|
sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4) + vector5.x1 * weight5;
|
|
sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4) + vector5.x2 * weight5;
|
|
}
|
|
|
|
public static void Muliply(in DPVector2 multiplicand, double multiplier, out DPVector2 product)
|
|
{
|
|
product.x1 = multiplicand.x1 * multiplier;
|
|
product.x2 = multiplicand.x2 * multiplier;
|
|
}
|
|
|
|
public static void Divide(in DPVector2 dividend, double divisor, out DPVector2 quotient)
|
|
{
|
|
quotient.x1 = dividend.x1 / divisor;
|
|
quotient.x2 = dividend.x2 / divisor;
|
|
}
|
|
|
|
public static void GetMean2(
|
|
in DPVector2 vector1,
|
|
in DPVector2 vector2,
|
|
out DPVector2 result
|
|
)
|
|
{
|
|
result.x1 = (vector1.x1 + vector2.x1) * 0.5;
|
|
result.x2 = (vector1.x2 + vector2.x2) * 0.5;
|
|
}
|
|
|
|
public static void GetMean3(
|
|
in DPVector2 vector1,
|
|
in DPVector2 vector2,
|
|
in DPVector2 vector3,
|
|
out DPVector2 result
|
|
)
|
|
{
|
|
result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * DPUtility.ONE_THIRD;
|
|
result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * DPUtility.ONE_THIRD;
|
|
}
|
|
|
|
public static void GetMean4(
|
|
in DPVector2 vector1,
|
|
in DPVector2 vector2,
|
|
in DPVector2 vector3,
|
|
in DPVector2 vector4,
|
|
out DPVector2 result
|
|
)
|
|
{
|
|
result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1)) * 0.25;
|
|
result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2)) * 0.25;
|
|
}
|
|
|
|
public static void GetMean5(
|
|
in DPVector2 vector1,
|
|
in DPVector2 vector2,
|
|
in DPVector2 vector3,
|
|
in DPVector2 vector4,
|
|
in DPVector2 vector5,
|
|
out DPVector2 result
|
|
)
|
|
{
|
|
result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1) * 0.2;
|
|
result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2) * 0.2;
|
|
}
|
|
|
|
public static double GetScalarProduct(in DPVector2 vector1, in DPVector2 vector2)
|
|
{
|
|
return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2;
|
|
}
|
|
|
|
public static double GetCrossProduct(in DPVector2 vector1, in DPVector2 vector2)
|
|
{
|
|
return vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1;
|
|
}
|
|
|
|
public static double GetAngle(in DPVector2 vector1, in DPVector2 vector2, AngleUnit unit)
|
|
{
|
|
double squareModule1 = vector1.GetSquareModule();
|
|
|
|
if (squareModule1 <= DPUtility.SQUARE_EPSYLON)
|
|
{
|
|
return 0.0;
|
|
}
|
|
|
|
double squareModule2 = vector2.GetSquareModule();
|
|
|
|
if (squareModule2 <= DPUtility.SQUARE_EPSYLON)
|
|
{
|
|
return 0.0;
|
|
}
|
|
|
|
double cosine = DPVector2.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2);
|
|
|
|
if (1.0 - DPUtility.EPSYLON <= cosine)
|
|
{
|
|
return 0.0;
|
|
}
|
|
|
|
if (cosine <= -(1.0 - DPUtility.EPSYLON))
|
|
{
|
|
return DPAngle.GetHalfCircle(unit);
|
|
}
|
|
|
|
return DPAngle.ConvertFromRadians(Math.Acos(cosine), unit);
|
|
}
|
|
|
|
public static double GetSquareDistance(in DPVector2 vector1, in DPVector2 vector2)
|
|
{
|
|
double dx1 = vector1.x1 - vector2.x1;
|
|
double dx2 = vector1.x2 - vector2.x2;
|
|
|
|
return dx1 * dx1 + dx2 * dx2;
|
|
}
|
|
|
|
public static double GetDistance(in DPVector2 vector1, in DPVector2 vector2)
|
|
{
|
|
return Math.Sqrt(GetSquareDistance(vector1, vector2));
|
|
}
|
|
|
|
public static bool AreEqual(in DPVector2 vector1, in DPVector2 vector2)
|
|
{
|
|
double squareModule1 = vector1.GetSquareModule();
|
|
double squareModule2 = vector2.GetSquareModule();
|
|
double squareModule3 = GetSquareDistance(vector1, vector2);
|
|
|
|
// 2.0 means dimension amount
|
|
if (squareModule1 < DPUtility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < DPUtility.EPSYLON_EFFECTIVENESS_LIMIT)
|
|
{
|
|
return squareModule3 < (2.0 * DPUtility.SQUARE_EPSYLON);
|
|
}
|
|
|
|
if (squareModule1 <= squareModule2)
|
|
{
|
|
return squareModule3 <= (2.0 * DPUtility.SQUARE_EPSYLON) * squareModule2;
|
|
}
|
|
|
|
return squareModule3 <= (2.0 * DPUtility.SQUARE_EPSYLON) * squareModule1;
|
|
}
|
|
}
|
|
}
|