Kimelas/bgc-net
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Исправления в матрицах 2x3 и 3x2 / Several fixes in 2x3 and 3x2 matrixes

Browse source
This commit is contained in:
Andrey Pokidov 2024-11-14 14:13:29 +07:00
parent 3ba55c7524
commit 301cabe8de
22 changed files with 3805 additions and 3744 deletions
Download patch file Download diff file Expand all files Collapse all files

870
Geometry/DPVector3.cs
View file

@ -1,435 +1,435 @@
/*
* 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 DPVector3
{
public static readonly DPVector3 ZERO = new DPVector3(0.0, 0.0, 0.0);
public double x1;
public double x2;
public double x3;
public DPVector3(double x1, double x2, double x3)
{
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
}
public DPVector3(in DPVector3 vector)
{
this.x1 = vector.x1;
this.x2 = vector.x2;
this.x3 = vector.x3;
}
public DPVector3(in SPVector3 vector)
{
this.x1 = vector.x1;
this.x2 = vector.x2;
this.x3 = vector.x3;
}
public readonly double GetSquareModule()
{
return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3;
}
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;
this.x3 /= module;
return 1;
}
public readonly DPVector3 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 DPVector3(
this.x1 / module,
this.x2 / module,
this.x3 / module
);
}
public void Invert()
{
this.x1 = -this.x1;
this.x2 = -this.x2;
this.x3 = -this.x3;
}
public readonly DPVector3 GetInverted()
{
return new DPVector3(-this.x1, -this.x2, -this.x3);
}
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;
this.x3 = 0.0;
}
public void SetValues(double x1, double x2, double x3)
{
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
}
public void SetValues(in SPVector3 vector)
{
this.x1 = vector.x1;
this.x2 = vector.x2;
this.x3 = vector.x3;
}
public void SetValues(in DPVector3 vector)
{
this.x1 = vector.x1;
this.x2 = vector.x2;
this.x3 = vector.x3;
}
public readonly override string ToString()
{
return String.Format("DPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3);
}
public static void Add(in DPVector3 vector1, in DPVector3 vector2, out DPVector3 sum)
{
sum.x1 = vector1.x1 + vector2.x1;
sum.x2 = vector1.x2 + vector2.x2;
sum.x3 = vector1.x3 + vector2.x3;
}
public static void Add3(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
out DPVector3 sum
)
{
sum.x1 = vector1.x1 + vector2.x1 + vector3.x1;
sum.x2 = vector1.x2 + vector2.x2 + vector3.x2;
sum.x3 = vector1.x3 + vector2.x3 + vector3.x3;
}
public static void Add4(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
in DPVector3 vector4,
out DPVector3 sum
)
{
sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1);
sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2);
sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3);
}
public static void Add5(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
in DPVector3 vector4,
in DPVector3 vector5,
out DPVector3 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;
sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3;
}
public static void Subtract(in DPVector3 minuend, in DPVector3 subtrahend, out DPVector3 difference)
{
difference.x1 = minuend.x1 - subtrahend.x1;
difference.x2 = minuend.x2 - subtrahend.x2;
difference.x3 = minuend.x3 - subtrahend.x3;
}
public static void GetWeightedSum2(
double weight1, in DPVector3 vector1,
double weight2, in DPVector3 vector2,
out DPVector3 sum
)
{
sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2;
sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2;
sum.x3 = vector1.x3 * weight1 + vector2.x3 * weight2;
}
public static void GetWeightedSum3(
double weight1, in DPVector3 vector1,
double weight2, in DPVector3 vector2,
double weight3, in DPVector3 vector3,
out DPVector3 sum
)
{
sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2 + vector3.x1 * weight3;
sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2 + vector3.x2 * weight3;
sum.x3 = vector1.x3 * weight1 + vector2.x3 * weight2 + vector3.x3 * weight3;
}
public static void GetWeightedSum4(
double weight1, in DPVector3 vector1,
double weight2, in DPVector3 vector2,
double weight3, in DPVector3 vector3,
double weight4, in DPVector3 vector4,
out DPVector3 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);
sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4);
}
public static void GetWeightedSum5(
double weight1, in DPVector3 vector1,
double weight2, in DPVector3 vector2,
double weight3, in DPVector3 vector3,
double weight4, in DPVector3 vector4,
double weight5, in DPVector3 vector5,
out DPVector3 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;
sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4) + vector5.x3 * weight5;
}
public static void Muliply(in DPVector3 multiplicand, double multiplier, out DPVector3 product)
{
product.x1 = multiplicand.x1 * multiplier;
product.x2 = multiplicand.x2 * multiplier;
product.x3 = multiplicand.x3 * multiplier;
}
public static void Divide(in DPVector3 dividend, double divisor, out DPVector3 quotient)
{
quotient.x1 = dividend.x1 / divisor;
quotient.x2 = dividend.x2 / divisor;
quotient.x3 = dividend.x3 / divisor;
}
public static void GetMean2(
in DPVector3 vector1,
in DPVector3 vector2,
out DPVector3 result
)
{
result.x1 = (vector1.x1 + vector2.x1) * 0.5;
result.x2 = (vector1.x2 + vector2.x2) * 0.5;
result.x3 = (vector1.x3 + vector2.x3) * 0.5;
}
public static void GetMean3(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
out DPVector3 result
)
{
result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * DPUtility.ONE_THIRD;
result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * DPUtility.ONE_THIRD;
result.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * DPUtility.ONE_THIRD;
}
public static void GetMean4(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
in DPVector3 vector4,
out DPVector3 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;
result.x3 = ((vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3)) * 0.25;
}
public static void GetMean5(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
in DPVector3 vector4,
in DPVector3 vector5,
out DPVector3 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;
result.x3 = ((vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3) * 0.2;
}
public static double GetScalarProduct(in DPVector3 vector1, in DPVector3 vector2)
{
return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2 + vector1.x3 * vector2.x3;
}
public static void GetCrossProduct(in DPVector3 vector1, in DPVector3 vector2, out DPVector3 result)
{
double x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2;
double x2 = vector1.x3 * vector2.x1 - vector1.x1 * vector2.x3;
double x3 = vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1;
result.x1 = x1;
result.x2 = x2;
result.x3 = x3;
}
public static double GetTripleProduct(in DPVector3 vector1, in DPVector3 vector2, in DPVector3 vector3)
{
return vector1.x1 * (vector2.x2 * vector3.x3 - vector2.x3 * vector3.x2)
+ vector1.x2 * (vector2.x3 * vector3.x1 - vector2.x1 * vector3.x3)
+ vector1.x3 * (vector2.x1 * vector3.x2 - vector2.x2 * vector3.x1);
}
public static void GetDoubleCrossProduct(in DPVector3 vector1, in DPVector3 vector2, in DPVector3 vector3, out DPVector3 result)
{
// [a x [b x c]] = b * (a, c) - c * (a, b)
double ac = GetScalarProduct(vector1, vector3);
double ab = GetScalarProduct(vector1, vector2);
result.x1 = ac * vector2.x1 - ab * vector3.x1;
result.x2 = ac * vector2.x2 - ab * vector3.x2;
result.x3 = ac * vector2.x3 - ab * vector3.x3;
}
public static double GetAngle(in DPVector3 vector1, in DPVector3 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 = DPVector3.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 DPVector3 vector1, in DPVector3 vector2)
{
double dx1 = vector1.x1 - vector2.x1;
double dx2 = vector1.x2 - vector2.x2;
double dx3 = vector1.x3 - vector2.x3;
return dx1 * dx1 + dx2 * dx2 + dx3 * dx3;
}
public static double GetDistance(in DPVector3 vector1, in DPVector3 vector2)
{
return Math.Sqrt(GetSquareDistance(vector1, vector2));
}
public static bool AreEqual(in DPVector3 vector1, in DPVector3 vector2)
{
double squareModule1 = vector1.GetSquareModule();
double squareModule2 = vector2.GetSquareModule();
double squareModule3 = GetSquareDistance(vector1, vector2);
// 3.0 means dimension amount
if (squareModule1 < DPUtility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < DPUtility.EPSYLON_EFFECTIVENESS_LIMIT)
{
return squareModule3 < (3.0 * DPUtility.SQUARE_EPSYLON);
}
if (squareModule1 <= squareModule2)
{
return squareModule3 <= (3.0 * DPUtility.SQUARE_EPSYLON) * squareModule2;
}
return squareModule3 <= (3.0 * DPUtility.SQUARE_EPSYLON) * squareModule1;
}
}
}
/*
* 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 DPVector3
{
public static readonly DPVector3 ZERO = new DPVector3(0.0, 0.0, 0.0);
public double x1;
public double x2;
public double x3;
public DPVector3(double x1, double x2, double x3)
{
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
}
public DPVector3(in DPVector3 vector)
{
this.x1 = vector.x1;
this.x2 = vector.x2;
this.x3 = vector.x3;
}
public DPVector3(in SPVector3 vector)
{
this.x1 = vector.x1;
this.x2 = vector.x2;
this.x3 = vector.x3;
}
public readonly double GetSquareModule()
{
return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3;
}
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;
this.x3 /= module;
return 1;
}
public readonly DPVector3 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 DPVector3(
this.x1 / module,
this.x2 / module,
this.x3 / module
);
}
public void Invert()
{
this.x1 = -this.x1;
this.x2 = -this.x2;
this.x3 = -this.x3;
}
public readonly DPVector3 GetInverted()
{
return new DPVector3(-this.x1, -this.x2, -this.x3);
}
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;
this.x3 = 0.0;
}
public void SetValues(double x1, double x2, double x3)
{
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
}
public void SetValues(in SPVector3 vector)
{
this.x1 = vector.x1;
this.x2 = vector.x2;
this.x3 = vector.x3;
}
public void SetValues(in DPVector3 vector)
{
this.x1 = vector.x1;
this.x2 = vector.x2;
this.x3 = vector.x3;
}
public readonly override string ToString()
{
return String.Format("DPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3);
}
public static void Add(in DPVector3 vector1, in DPVector3 vector2, out DPVector3 sum)
{
sum.x1 = vector1.x1 + vector2.x1;
sum.x2 = vector1.x2 + vector2.x2;
sum.x3 = vector1.x3 + vector2.x3;
}
public static void Add3(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
out DPVector3 sum
)
{
sum.x1 = vector1.x1 + vector2.x1 + vector3.x1;
sum.x2 = vector1.x2 + vector2.x2 + vector3.x2;
sum.x3 = vector1.x3 + vector2.x3 + vector3.x3;
}
public static void Add4(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
in DPVector3 vector4,
out DPVector3 sum
)
{
sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1);
sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2);
sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3);
}
public static void Add5(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
in DPVector3 vector4,
in DPVector3 vector5,
out DPVector3 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;
sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3;
}
public static void Subtract(in DPVector3 minuend, in DPVector3 subtrahend, out DPVector3 difference)
{
difference.x1 = minuend.x1 - subtrahend.x1;
difference.x2 = minuend.x2 - subtrahend.x2;
difference.x3 = minuend.x3 - subtrahend.x3;
}
public static void GetWeightedSum2(
double weight1, in DPVector3 vector1,
double weight2, in DPVector3 vector2,
out DPVector3 sum
)
{
sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2;
sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2;
sum.x3 = vector1.x3 * weight1 + vector2.x3 * weight2;
}
public static void GetWeightedSum3(
double weight1, in DPVector3 vector1,
double weight2, in DPVector3 vector2,
double weight3, in DPVector3 vector3,
out DPVector3 sum
)
{
sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2 + vector3.x1 * weight3;
sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2 + vector3.x2 * weight3;
sum.x3 = vector1.x3 * weight1 + vector2.x3 * weight2 + vector3.x3 * weight3;
}
public static void GetWeightedSum4(
double weight1, in DPVector3 vector1,
double weight2, in DPVector3 vector2,
double weight3, in DPVector3 vector3,
double weight4, in DPVector3 vector4,
out DPVector3 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);
sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4);
}
public static void GetWeightedSum5(
double weight1, in DPVector3 vector1,
double weight2, in DPVector3 vector2,
double weight3, in DPVector3 vector3,
double weight4, in DPVector3 vector4,
double weight5, in DPVector3 vector5,
out DPVector3 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;
sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4) + vector5.x3 * weight5;
}
public static void Muliply(in DPVector3 multiplicand, double multiplier, out DPVector3 product)
{
product.x1 = multiplicand.x1 * multiplier;
product.x2 = multiplicand.x2 * multiplier;
product.x3 = multiplicand.x3 * multiplier;
}
public static void Divide(in DPVector3 dividend, double divisor, out DPVector3 quotient)
{
quotient.x1 = dividend.x1 / divisor;
quotient.x2 = dividend.x2 / divisor;
quotient.x3 = dividend.x3 / divisor;
}
public static void GetMean2(
in DPVector3 vector1,
in DPVector3 vector2,
out DPVector3 result
)
{
result.x1 = (vector1.x1 + vector2.x1) * 0.5;
result.x2 = (vector1.x2 + vector2.x2) * 0.5;
result.x3 = (vector1.x3 + vector2.x3) * 0.5;
}
public static void GetMean3(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
out DPVector3 result
)
{
result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * DPUtility.ONE_THIRD;
result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * DPUtility.ONE_THIRD;
result.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * DPUtility.ONE_THIRD;
}
public static void GetMean4(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
in DPVector3 vector4,
out DPVector3 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;
result.x3 = ((vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3)) * 0.25;
}
public static void GetMean5(
in DPVector3 vector1,
in DPVector3 vector2,
in DPVector3 vector3,
in DPVector3 vector4,
in DPVector3 vector5,
out DPVector3 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;
result.x3 = ((vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3) * 0.2;
}
public static double GetScalarProduct(in DPVector3 vector1, in DPVector3 vector2)
{
return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2 + vector1.x3 * vector2.x3;
}
public static void GetCrossProduct(in DPVector3 vector1, in DPVector3 vector2, out DPVector3 result)
{
double x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2;
double x2 = vector1.x3 * vector2.x1 - vector1.x1 * vector2.x3;
double x3 = vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1;
result.x1 = x1;
result.x2 = x2;
result.x3 = x3;
}
public static double GetTripleProduct(in DPVector3 vector1, in DPVector3 vector2, in DPVector3 vector3)
{
return vector1.x1 * (vector2.x2 * vector3.x3 - vector2.x3 * vector3.x2)
+ vector1.x2 * (vector2.x3 * vector3.x1 - vector2.x1 * vector3.x3)
+ vector1.x3 * (vector2.x1 * vector3.x2 - vector2.x2 * vector3.x1);
}
public static void GetDoubleCrossProduct(in DPVector3 vector1, in DPVector3 vector2, in DPVector3 vector3, out DPVector3 result)
{
// [a x [b x c]] = b * (a, c) - c * (a, b)
double ac = GetScalarProduct(vector1, vector3);
double ab = GetScalarProduct(vector1, vector2);
result.x1 = ac * vector2.x1 - ab * vector3.x1;
result.x2 = ac * vector2.x2 - ab * vector3.x2;
result.x3 = ac * vector2.x3 - ab * vector3.x3;
}
public static double GetAngle(in DPVector3 vector1, in DPVector3 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 = DPVector3.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 DPVector3 vector1, in DPVector3 vector2)
{
double dx1 = vector1.x1 - vector2.x1;
double dx2 = vector1.x2 - vector2.x2;
double dx3 = vector1.x3 - vector2.x3;
return dx1 * dx1 + dx2 * dx2 + dx3 * dx3;
}
public static double GetDistance(in DPVector3 vector1, in DPVector3 vector2)
{
return Math.Sqrt(GetSquareDistance(vector1, vector2));
}
public static bool AreEqual(in DPVector3 vector1, in DPVector3 vector2)
{
double squareModule1 = vector1.GetSquareModule();
double squareModule2 = vector2.GetSquareModule();
double squareModule3 = GetSquareDistance(vector1, vector2);
// 3.0 means dimension amount
if (squareModule1 < DPUtility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < DPUtility.EPSYLON_EFFECTIVENESS_LIMIT)
{
return squareModule3 < (3.0 * DPUtility.SQUARE_EPSYLON);
}
if (squareModule1 <= squareModule2)
{
return squareModule3 <= (3.0 * DPUtility.SQUARE_EPSYLON) * squareModule2;
}
return squareModule3 <= (3.0 * DPUtility.SQUARE_EPSYLON) * squareModule1;
}
}
}