init commit

This commit is contained in:
2025-10-09 09:57:24 +09:00
commit 4d551bd74f
6636 changed files with 1218703 additions and 0 deletions

View File

@@ -0,0 +1,18 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<AssemblyName>AForge.Math</AssemblyName>
<GenerateAssemblyInfo>False</GenerateAssemblyInfo>
<TargetFramework>net20</TargetFramework>
</PropertyGroup>
<PropertyGroup>
<LangVersion>12.0</LangVersion>
<AllowUnsafeBlocks>True</AllowUnsafeBlocks>
</PropertyGroup>
<PropertyGroup />
<ItemGroup />
<ItemGroup>
<Reference Include="AForge">
<HintPath>../../../desktop/AForge.dll</HintPath>
</Reference>
</ItemGroup>
</Project>

View File

@@ -0,0 +1,473 @@
using System;
using System.Runtime.Serialization;
using System.Text.RegularExpressions;
namespace AForge.Math;
public struct Complex : ICloneable, ISerializable
{
public double Re;
public double Im;
public static readonly Complex Zero = new Complex(0.0, 0.0);
public static readonly Complex One = new Complex(1.0, 0.0);
public static readonly Complex I = new Complex(0.0, 1.0);
public double Magnitude => System.Math.Sqrt(Re * Re + Im * Im);
public double Phase => System.Math.Atan2(Im, Re);
public double SquaredMagnitude => Re * Re + Im * Im;
public Complex(double re, double im)
{
Re = re;
Im = im;
}
public Complex(Complex c)
{
Re = c.Re;
Im = c.Im;
}
public static Complex Add(Complex a, Complex b)
{
return new Complex(a.Re + b.Re, a.Im + b.Im);
}
public static Complex Add(Complex a, double s)
{
return new Complex(a.Re + s, a.Im);
}
public static void Add(Complex a, Complex b, ref Complex result)
{
result.Re = a.Re + b.Re;
result.Im = a.Im + b.Im;
}
public static void Add(Complex a, double s, ref Complex result)
{
result.Re = a.Re + s;
result.Im = a.Im;
}
public static Complex Subtract(Complex a, Complex b)
{
return new Complex(a.Re - b.Re, a.Im - b.Im);
}
public static Complex Subtract(Complex a, double s)
{
return new Complex(a.Re - s, a.Im);
}
public static Complex Subtract(double s, Complex a)
{
return new Complex(s - a.Re, a.Im);
}
public static void Subtract(Complex a, Complex b, ref Complex result)
{
result.Re = a.Re - b.Re;
result.Im = a.Im - b.Im;
}
public static void Subtract(Complex a, double s, ref Complex result)
{
result.Re = a.Re - s;
result.Im = a.Im;
}
public static void Subtract(double s, Complex a, ref Complex result)
{
result.Re = s - a.Re;
result.Im = a.Im;
}
public static Complex Multiply(Complex a, Complex b)
{
double re = a.Re;
double im = a.Im;
double re2 = b.Re;
double im2 = b.Im;
return new Complex(re * re2 - im * im2, re * im2 + im * re2);
}
public static Complex Multiply(Complex a, double s)
{
return new Complex(a.Re * s, a.Im * s);
}
public static void Multiply(Complex a, Complex b, ref Complex result)
{
double re = a.Re;
double im = a.Im;
double re2 = b.Re;
double im2 = b.Im;
result.Re = re * re2 - im * im2;
result.Im = re * im2 + im * re2;
}
public static void Multiply(Complex a, double s, ref Complex result)
{
result.Re = a.Re * s;
result.Im = a.Im * s;
}
public static Complex Divide(Complex a, Complex b)
{
double re = a.Re;
double im = a.Im;
double re2 = b.Re;
double im2 = b.Im;
double num = re2 * re2 + im2 * im2;
if (num == 0.0)
{
throw new DivideByZeroException("Can not divide by zero.");
}
double num2 = 1.0 / num;
return new Complex((re * re2 + im * im2) * num2, (im * re2 - re * im2) * num2);
}
public static Complex Divide(Complex a, double s)
{
if (s == 0.0)
{
throw new DivideByZeroException("Can not divide by zero.");
}
return new Complex(a.Re / s, a.Im / s);
}
public static Complex Divide(double s, Complex a)
{
if (a.Re == 0.0 || a.Im == 0.0)
{
throw new DivideByZeroException("Can not divide by zero.");
}
return new Complex(s / a.Re, s / a.Im);
}
public static void Divide(Complex a, Complex b, ref Complex result)
{
double re = a.Re;
double im = a.Im;
double re2 = b.Re;
double im2 = b.Im;
double num = re2 * re2 + im2 * im2;
if (num == 0.0)
{
throw new DivideByZeroException("Can not divide by zero.");
}
double num2 = 1.0 / num;
result.Re = (re * re2 + im * im2) * num2;
result.Im = (im * re2 - re * im2) * num2;
}
public static void Divide(Complex a, double s, ref Complex result)
{
if (s == 0.0)
{
throw new DivideByZeroException("Can not divide by zero.");
}
result.Re = a.Re / s;
result.Im = a.Im / s;
}
public static void Divide(double s, Complex a, ref Complex result)
{
if (a.Re == 0.0 || a.Im == 0.0)
{
throw new DivideByZeroException("Can not divide by zero.");
}
result.Re = s / a.Re;
result.Im = s / a.Im;
}
public static Complex Negate(Complex a)
{
return new Complex(0.0 - a.Re, 0.0 - a.Im);
}
public static bool ApproxEqual(Complex a, Complex b)
{
return ApproxEqual(a, b, 8.881784197001252E-16);
}
public static bool ApproxEqual(Complex a, Complex b, double tolerance)
{
if (System.Math.Abs(a.Re - b.Re) <= tolerance)
{
return System.Math.Abs(a.Im - b.Im) <= tolerance;
}
return false;
}
public static Complex Parse(string s)
{
Regex regex = new Regex("\\((?<real>.*),(?<imaginary>.*)\\)", RegexOptions.None);
Match match = regex.Match(s);
if (match.Success)
{
return new Complex(double.Parse(match.Result("${real}")), double.Parse(match.Result("${imaginary}")));
}
throw new FormatException("String representation of the complex number is not correctly formatted.");
}
public static bool TryParse(string s, out Complex result)
{
try
{
Complex complex = Parse(s);
result = complex;
return true;
}
catch (FormatException)
{
result = default(Complex);
return false;
}
}
public static Complex Sqrt(Complex a)
{
Complex zero = Zero;
if (a.Re == 0.0 && a.Im == 0.0)
{
return zero;
}
if (a.Im == 0.0)
{
zero.Re = ((a.Re > 0.0) ? System.Math.Sqrt(a.Re) : System.Math.Sqrt(0.0 - a.Re));
zero.Im = 0.0;
}
else
{
double magnitude = a.Magnitude;
zero.Re = System.Math.Sqrt(0.5 * (magnitude + a.Re));
zero.Im = System.Math.Sqrt(0.5 * (magnitude - a.Re));
if (a.Im < 0.0)
{
zero.Im = 0.0 - zero.Im;
}
}
return zero;
}
public static Complex Log(Complex a)
{
Complex zero = Zero;
if (a.Re > 0.0 && a.Im == 0.0)
{
zero.Re = System.Math.Log(a.Re);
zero.Im = 0.0;
}
else if (a.Re == 0.0)
{
if (a.Im > 0.0)
{
zero.Re = System.Math.Log(a.Im);
zero.Im = System.Math.PI / 2.0;
}
else
{
zero.Re = System.Math.Log(0.0 - a.Im);
zero.Im = -System.Math.PI / 2.0;
}
}
else
{
zero.Re = System.Math.Log(a.Magnitude);
zero.Im = System.Math.Atan2(a.Im, a.Re);
}
return zero;
}
public static Complex Exp(Complex a)
{
Complex zero = Zero;
double num = System.Math.Exp(a.Re);
zero.Re = num * System.Math.Cos(a.Im);
zero.Im = num * System.Math.Sin(a.Im);
return zero;
}
public static Complex Sin(Complex a)
{
Complex zero = Zero;
if (a.Im == 0.0)
{
zero.Re = System.Math.Sin(a.Re);
zero.Im = 0.0;
}
else
{
zero.Re = System.Math.Sin(a.Re) * System.Math.Cosh(a.Im);
zero.Im = System.Math.Cos(a.Re) * System.Math.Sinh(a.Im);
}
return zero;
}
public static Complex Cos(Complex a)
{
Complex zero = Zero;
if (a.Im == 0.0)
{
zero.Re = System.Math.Cos(a.Re);
zero.Im = 0.0;
}
else
{
zero.Re = System.Math.Cos(a.Re) * System.Math.Cosh(a.Im);
zero.Im = (0.0 - System.Math.Sin(a.Re)) * System.Math.Sinh(a.Im);
}
return zero;
}
public static Complex Tan(Complex a)
{
Complex zero = Zero;
if (a.Im == 0.0)
{
zero.Re = System.Math.Tan(a.Re);
zero.Im = 0.0;
}
else
{
double num = 2.0 * a.Re;
double value = 2.0 * a.Im;
double num2 = System.Math.Cos(num) + System.Math.Cosh(num);
zero.Re = System.Math.Sin(num) / num2;
zero.Im = System.Math.Sinh(value) / num2;
}
return zero;
}
public override int GetHashCode()
{
return Re.GetHashCode() ^ Im.GetHashCode();
}
public override bool Equals(object obj)
{
if (!(obj is Complex))
{
return false;
}
return this == (Complex)obj;
}
public override string ToString()
{
return $"({Re}, {Im})";
}
public static bool operator ==(Complex u, Complex v)
{
if (u.Re == v.Re)
{
return u.Im == v.Im;
}
return false;
}
public static bool operator !=(Complex u, Complex v)
{
return !(u == v);
}
public static Complex operator -(Complex a)
{
return Negate(a);
}
public static Complex operator +(Complex a, Complex b)
{
return Add(a, b);
}
public static Complex operator +(Complex a, double s)
{
return Add(a, s);
}
public static Complex operator +(double s, Complex a)
{
return Add(a, s);
}
public static Complex operator -(Complex a, Complex b)
{
return Subtract(a, b);
}
public static Complex operator -(Complex a, double s)
{
return Subtract(a, s);
}
public static Complex operator -(double s, Complex a)
{
return Subtract(s, a);
}
public static Complex operator *(Complex a, Complex b)
{
return Multiply(a, b);
}
public static Complex operator *(double s, Complex a)
{
return Multiply(a, s);
}
public static Complex operator *(Complex a, double s)
{
return Multiply(a, s);
}
public static Complex operator /(Complex a, Complex b)
{
return Divide(a, b);
}
public static Complex operator /(Complex a, double s)
{
return Divide(a, s);
}
public static Complex operator /(double s, Complex a)
{
return Divide(s, a);
}
public static explicit operator Complex(float value)
{
return new Complex(value, 0.0);
}
public static explicit operator Complex(double value)
{
return new Complex(value, 0.0);
}
object ICloneable.Clone()
{
return new Complex(this);
}
public Complex Clone()
{
return new Complex(this);
}
public void GetObjectData(SerializationInfo info, StreamingContext context)
{
info.AddValue("Real", Re);
info.AddValue("Imaginary", Im);
}
}

View File

@@ -0,0 +1,134 @@
using System;
namespace AForge.Math;
[Serializable]
public class ContinuousHistogram
{
private int[] values;
private Range range;
private float mean;
private float stdDev;
private float median;
private float min;
private float max;
private int total;
public int[] Values => values;
public Range Range => range;
public float Mean => mean;
public float StdDev => stdDev;
public float Median => median;
public float Min => min;
public float Max => max;
public ContinuousHistogram(int[] values, Range range)
{
this.values = values;
this.range = range;
Update();
}
public Range GetRange(float percent)
{
int num = (int)((float)total * (percent + (1f - percent) / 2f));
int num2 = values.Length;
int num3 = num2 - 1;
int i = 0;
int num4 = total;
for (; i < num2; i++)
{
num4 -= values[i];
if (num4 < num)
{
break;
}
}
int num5 = num3;
num4 = total;
while (num5 >= 0)
{
num4 -= values[num5];
if (num4 < num)
{
break;
}
num5--;
}
return new Range((float)i / (float)num3 * range.Length + range.Min, (float)num5 / (float)num3 * range.Length + range.Min);
}
public void Update()
{
int num = values.Length;
int num2 = num - 1;
float length = range.Length;
float num3 = range.Min;
max = 0f;
min = num;
mean = 0f;
stdDev = 0f;
total = 0;
double num4 = 0.0;
int num5;
for (int i = 0; i < num; i++)
{
num5 = values[i];
if (num5 != 0)
{
if ((float)i > max)
{
max = i;
}
if ((float)i < min)
{
min = i;
}
}
total += num5;
num4 += ((double)i / (double)num2 * (double)length + (double)num3) * (double)num5;
}
if (total != 0)
{
mean = (float)(num4 / (double)total);
}
min = min / (float)num2 * length + num3;
max = max / (float)num2 * length + num3;
num4 = 0.0;
for (int i = 0; i < num; i++)
{
num5 = values[i];
double num6 = (double)i / (double)num2 * (double)length + (double)num3 - (double)mean;
num4 += num6 * num6 * (double)num5;
}
if (total != 0)
{
stdDev = (float)System.Math.Sqrt(num4 / (double)total);
}
int num7 = total / 2;
int j = 0;
num5 = 0;
for (; j < num; j++)
{
num5 += values[j];
if (num5 >= num7)
{
break;
}
}
median = (float)j / (float)num2 * length + num3;
}
}

View File

@@ -0,0 +1,280 @@
using System;
namespace AForge.Math;
public static class FourierTransform
{
public enum Direction
{
Forward = 1,
Backward = -1
}
private const int minLength = 2;
private const int maxLength = 16384;
private const int minBits = 1;
private const int maxBits = 14;
private static int[][] reversedBits = new int[14][];
private static Complex[,][] complexRotation = new Complex[14, 2][];
public static void DFT(Complex[] data, Direction direction)
{
int num = data.Length;
Complex[] array = new Complex[num];
for (int i = 0; i < num; i++)
{
ref Complex reference = ref array[i];
reference = Complex.Zero;
double num2 = (double)(0 - direction) * 2.0 * System.Math.PI * (double)i / (double)num;
for (int j = 0; j < num; j++)
{
double num3 = System.Math.Cos((double)j * num2);
double num4 = System.Math.Sin((double)j * num2);
array[i].Re += data[j].Re * num3 - data[j].Im * num4;
array[i].Im += data[j].Re * num4 + data[j].Im * num3;
}
}
if (direction == Direction.Forward)
{
for (int k = 0; k < num; k++)
{
data[k].Re = array[k].Re / (double)num;
data[k].Im = array[k].Im / (double)num;
}
}
else
{
for (int l = 0; l < num; l++)
{
data[l].Re = array[l].Re;
data[l].Im = array[l].Im;
}
}
}
public static void DFT2(Complex[,] data, Direction direction)
{
int length = data.GetLength(0);
int length2 = data.GetLength(1);
Complex[] array = new Complex[System.Math.Max(length, length2)];
for (int i = 0; i < length; i++)
{
for (int j = 0; j < length2; j++)
{
ref Complex reference = ref array[j];
reference = Complex.Zero;
double num = (double)(0 - direction) * 2.0 * System.Math.PI * (double)j / (double)length2;
for (int k = 0; k < length2; k++)
{
double num2 = System.Math.Cos((double)k * num);
double num3 = System.Math.Sin((double)k * num);
array[j].Re += data[i, k].Re * num2 - data[i, k].Im * num3;
array[j].Im += data[i, k].Re * num3 + data[i, k].Im * num2;
}
}
if (direction == Direction.Forward)
{
for (int l = 0; l < length2; l++)
{
data[i, l].Re = array[l].Re / (double)length2;
data[i, l].Im = array[l].Im / (double)length2;
}
}
else
{
for (int m = 0; m < length2; m++)
{
data[i, m].Re = array[m].Re;
data[i, m].Im = array[m].Im;
}
}
}
for (int n = 0; n < length2; n++)
{
for (int num4 = 0; num4 < length; num4++)
{
ref Complex reference2 = ref array[num4];
reference2 = Complex.Zero;
double num = (double)(0 - direction) * 2.0 * System.Math.PI * (double)num4 / (double)length;
for (int num5 = 0; num5 < length; num5++)
{
double num2 = System.Math.Cos((double)num5 * num);
double num3 = System.Math.Sin((double)num5 * num);
array[num4].Re += data[num5, n].Re * num2 - data[num5, n].Im * num3;
array[num4].Im += data[num5, n].Re * num3 + data[num5, n].Im * num2;
}
}
if (direction == Direction.Forward)
{
for (int num6 = 0; num6 < length; num6++)
{
data[num6, n].Re = array[num6].Re / (double)length;
data[num6, n].Im = array[num6].Im / (double)length;
}
}
else
{
for (int num7 = 0; num7 < length; num7++)
{
data[num7, n].Re = array[num7].Re;
data[num7, n].Im = array[num7].Im;
}
}
}
}
public static void FFT(Complex[] data, Direction direction)
{
int num = data.Length;
int num2 = Tools.Log2(num);
ReorderData(data);
int num3 = 1;
for (int i = 1; i <= num2; i++)
{
Complex[] array = GetComplexRotation(i, direction);
int num4 = num3;
num3 <<= 1;
for (int j = 0; j < num4; j++)
{
Complex complex = array[j];
for (int k = j; k < num; k += num3)
{
int num5 = k + num4;
Complex complex2 = data[k];
Complex complex3 = data[num5];
double num6 = complex3.Re * complex.Re - complex3.Im * complex.Im;
double num7 = complex3.Re * complex.Im + complex3.Im * complex.Re;
data[k].Re += num6;
data[k].Im += num7;
data[num5].Re = complex2.Re - num6;
data[num5].Im = complex2.Im - num7;
}
}
}
if (direction == Direction.Forward)
{
for (int l = 0; l < num; l++)
{
data[l].Re /= num;
data[l].Im /= num;
}
}
}
public static void FFT2(Complex[,] data, Direction direction)
{
int length = data.GetLength(0);
int length2 = data.GetLength(1);
if (!Tools.IsPowerOf2(length) || !Tools.IsPowerOf2(length2) || length < 2 || length > 16384 || length2 < 2 || length2 > 16384)
{
throw new ArgumentException("Incorrect data length.");
}
Complex[] array = new Complex[length2];
for (int i = 0; i < length; i++)
{
for (int j = 0; j < length2; j++)
{
ref Complex reference = ref array[j];
reference = data[i, j];
}
FFT(array, direction);
for (int k = 0; k < length2; k++)
{
ref Complex reference2 = ref data[i, k];
reference2 = array[k];
}
}
Complex[] array2 = new Complex[length];
for (int l = 0; l < length2; l++)
{
for (int m = 0; m < length; m++)
{
ref Complex reference3 = ref array2[m];
reference3 = data[m, l];
}
FFT(array2, direction);
for (int n = 0; n < length; n++)
{
ref Complex reference4 = ref data[n, l];
reference4 = array2[n];
}
}
}
private static int[] GetReversedBits(int numberOfBits)
{
if (numberOfBits < 1 || numberOfBits > 14)
{
throw new ArgumentOutOfRangeException();
}
if (reversedBits[numberOfBits - 1] == null)
{
int num = Tools.Pow2(numberOfBits);
int[] array = new int[num];
for (int i = 0; i < num; i++)
{
int num2 = i;
int num3 = 0;
for (int j = 0; j < numberOfBits; j++)
{
num3 = (num3 << 1) | (num2 & 1);
num2 >>= 1;
}
array[i] = num3;
}
reversedBits[numberOfBits - 1] = array;
}
return reversedBits[numberOfBits - 1];
}
private static Complex[] GetComplexRotation(int numberOfBits, Direction direction)
{
int num = ((direction != Direction.Forward) ? 1 : 0);
if (complexRotation[numberOfBits - 1, num] == null)
{
int num2 = 1 << numberOfBits - 1;
double num3 = 1.0;
double num4 = 0.0;
double num5 = System.Math.PI / (double)num2 * (double)direction;
double num6 = System.Math.Cos(num5);
double num7 = System.Math.Sin(num5);
Complex[] array = new Complex[num2];
for (int i = 0; i < num2; i++)
{
ref Complex reference = ref array[i];
reference = new Complex(num3, num4);
double num8 = num3 * num7 + num4 * num6;
num3 = num3 * num6 - num4 * num7;
num4 = num8;
}
complexRotation[numberOfBits - 1, num] = array;
}
return complexRotation[numberOfBits - 1, num];
}
private static void ReorderData(Complex[] data)
{
int num = data.Length;
if (num < 2 || num > 16384 || !Tools.IsPowerOf2(num))
{
throw new ArgumentException("Incorrect data length.");
}
int[] array = GetReversedBits(Tools.Log2(num));
for (int i = 0; i < num; i++)
{
int num2 = array[i];
if (num2 > i)
{
Complex complex = data[i];
ref Complex reference = ref data[i];
reference = data[num2];
data[num2] = complex;
}
}
}
}

View File

@@ -0,0 +1,81 @@
using System;
namespace AForge.Math;
public class Gaussian
{
private double sigma = 1.0;
private double sqrSigma = 1.0;
public double Sigma
{
get
{
return sigma;
}
set
{
sigma = System.Math.Max(1E-08, value);
sqrSigma = sigma * sigma;
}
}
public Gaussian()
{
}
public Gaussian(double sigma)
{
Sigma = sigma;
}
public double Function(double x)
{
return System.Math.Exp(x * x / (-2.0 * sqrSigma)) / (System.Math.Sqrt(System.Math.PI * 2.0) * sigma);
}
public double Function2D(double x, double y)
{
return System.Math.Exp((x * x + y * y) / (-2.0 * sqrSigma)) / (System.Math.PI * 2.0 * sqrSigma);
}
public double[] Kernel(int size)
{
if (size % 2 == 0 || size < 3 || size > 101)
{
throw new ArgumentException("Wrong kernal size.");
}
int num = size / 2;
double[] array = new double[size];
int num2 = -num;
for (int i = 0; i < size; i++)
{
array[i] = Function(num2);
num2++;
}
return array;
}
public double[,] Kernel2D(int size)
{
if (size % 2 == 0 || size < 3 || size > 101)
{
throw new ArgumentException("Wrong kernal size.");
}
int num = size / 2;
double[,] array = new double[size, size];
int num2 = -num;
for (int i = 0; i < size; i++)
{
int num3 = -num;
for (int j = 0; j < size; j++)
{
array[i, j] = Function2D(num3, num2);
num3++;
}
num2++;
}
return array;
}
}

View File

@@ -0,0 +1,67 @@
using System;
using System.Collections.Generic;
namespace AForge.Math.Geometry;
public class ClosePointsMergingOptimizer : IShapeOptimizer
{
private float maxDistanceToMerge = 10f;
public float MaxDistanceToMerge
{
get
{
return maxDistanceToMerge;
}
set
{
maxDistanceToMerge = System.Math.Max(0f, value);
}
}
public ClosePointsMergingOptimizer()
{
}
public ClosePointsMergingOptimizer(float maxDistanceToMerge)
{
this.maxDistanceToMerge = maxDistanceToMerge;
}
public List<IntPoint> OptimizeShape(List<IntPoint> shape)
{
List<IntPoint> list = new List<IntPoint>();
if (shape.Count <= 3)
{
list.AddRange(shape);
}
else
{
float num = 0f;
list.Add(shape[0]);
int num2 = 1;
int i = 1;
for (int count = shape.Count; i < count; i++)
{
num = list[num2 - 1].DistanceTo(shape[i]);
if (num <= maxDistanceToMerge && num2 + (count - i) > 3)
{
list[num2 - 1] = (list[num2 - 1] + shape[i]) / 2;
continue;
}
list.Add(shape[i]);
num2++;
}
if (num2 > 3)
{
num = list[num2 - 1].DistanceTo(list[0]);
if (num <= maxDistanceToMerge)
{
list[0] = (list[num2 - 1] + list[0]) / 2;
list.RemoveAt(num2 - 1);
}
}
}
return list;
}
}

View File

@@ -0,0 +1,216 @@
using System;
namespace AForge.Math.Geometry;
public class CoplanarPosit
{
private const float ErrorLimit = 2f;
private float focalLength;
private Vector3[] modelPoints;
private Matrix3x3 modelVectors;
private Matrix3x3 modelPseudoInverse;
private Vector3 modelNormal;
private Matrix3x3 alternateRotation = default(Matrix3x3);
private Vector3 alternateTranslation = default(Vector3);
private float alternatePoseError;
private Matrix3x3 bestRotation = default(Matrix3x3);
private Vector3 bestTranslation = default(Vector3);
private float bestPoseError;
public Matrix3x3 BestEstimatedRotation => bestRotation;
public Vector3 BestEstimatedTranslation => bestTranslation;
public float BestEstimationError => bestPoseError;
public Matrix3x3 AlternateEstimatedRotation => alternateRotation;
public Vector3 AlternateEstimatedTranslation => alternateTranslation;
public float AlternateEstimationError => alternatePoseError;
public Vector3[] Model => (Vector3[])modelPoints.Clone();
public float FocalLength
{
get
{
return focalLength;
}
set
{
focalLength = value;
}
}
public CoplanarPosit(Vector3[] model, float focalLength)
{
if (model.Length != 4)
{
throw new ArgumentException("The model must have 4 points.");
}
this.focalLength = focalLength;
modelPoints = (Vector3[])model.Clone();
modelVectors = Matrix3x3.CreateFromRows(model[1] - model[0], model[2] - model[0], model[3] - model[0]);
modelVectors.SVD(out var u, out var e, out var v);
modelPseudoInverse = v * Matrix3x3.CreateDiagonal(e.Inverse()) * u.Transpose();
modelNormal = v.GetColumn(e.MinIndex);
}
public void EstimatePose(Point[] points, out Matrix3x3 rotation, out Vector3 translation)
{
if (points.Length != 4)
{
throw new ArgumentException("4 points must be be given for pose estimation.");
}
POS(points, new Vector3(1f), out var rotation2, out var rotation3, out var translation2, out var translation3);
float num = Iterate(points, ref rotation2, ref translation2);
float num2 = Iterate(points, ref rotation3, ref translation3);
if (num < num2)
{
bestRotation = rotation2;
bestTranslation = translation2;
bestPoseError = num;
alternateRotation = rotation3;
alternateTranslation = translation3;
alternatePoseError = num2;
}
else
{
bestRotation = rotation3;
bestTranslation = translation3;
bestPoseError = num2;
alternateRotation = rotation2;
alternateTranslation = translation2;
alternatePoseError = num;
}
rotation = bestRotation;
translation = bestTranslation;
}
private float Iterate(Point[] points, ref Matrix3x3 rotation, ref Vector3 translation)
{
float num = float.MaxValue;
float num2 = 0f;
for (int i = 0; i < 100; i++)
{
Vector3 eps = modelVectors * rotation.GetRow(2) / translation.Z + 1f;
POS(points, eps, out var rotation2, out var rotation3, out var translation2, out var translation3);
float error = GetError(points, rotation2, translation2);
float error2 = GetError(points, rotation3, translation3);
if (error < error2)
{
rotation = rotation2;
translation = translation2;
num2 = error;
}
else
{
rotation = rotation3;
translation = translation3;
num2 = error2;
}
if (num2 <= 2f || num2 > num)
{
break;
}
num = num2;
}
return num2;
}
private void POS(Point[] imagePoints, Vector3 eps, out Matrix3x3 rotation1, out Matrix3x3 rotation2, out Vector3 translation1, out Vector3 translation2)
{
Vector3 vector = new Vector3(imagePoints[1].X, imagePoints[2].X, imagePoints[3].X);
Vector3 vector2 = new Vector3(imagePoints[1].Y, imagePoints[2].Y, imagePoints[3].Y);
Vector3 vector3 = vector * eps - imagePoints[0].X;
Vector3 vector4 = vector2 * eps - imagePoints[0].Y;
Vector3 vector5 = modelPseudoInverse * vector3;
Vector3 vector6 = modelPseudoInverse * vector4;
Vector3 vector7 = default(Vector3);
Vector3 vector8 = default(Vector3);
Vector3 vector9 = default(Vector3);
float num = vector6.Square - vector5.Square;
float num2 = Vector3.Dot(vector5, vector6);
float num3 = 0f;
float num4 = 0f;
if (num == 0f)
{
num4 = (float)(-System.Math.PI / 2.0 * (double)System.Math.Sign(num2));
num3 = (float)System.Math.Sqrt(System.Math.Abs(2f * num2));
}
else
{
num3 = (float)System.Math.Sqrt(System.Math.Sqrt(num * num + 4f * num2 * num2));
num4 = (float)System.Math.Atan(-2f * num2 / num);
if (num < 0f)
{
num4 += (float)System.Math.PI;
}
num4 /= 2f;
}
float num5 = (float)((double)num3 * System.Math.Cos(num4));
float num6 = (float)((double)num3 * System.Math.Sin(num4));
vector7 = vector5 + modelNormal * num5;
vector8 = vector6 + modelNormal * num6;
float num7 = vector7.Normalize();
float num8 = vector8.Normalize();
vector9 = Vector3.Cross(vector7, vector8);
rotation1 = Matrix3x3.CreateFromRows(vector7, vector8, vector9);
float num9 = (num7 + num8) / 2f;
Vector3 vector10 = rotation1 * modelPoints[0];
translation1 = new Vector3(imagePoints[0].X / num9 - vector10.X, imagePoints[0].Y / num9 - vector10.Y, focalLength / num9);
vector7 = vector5 - modelNormal * num5;
vector8 = vector6 - modelNormal * num6;
num7 = vector7.Normalize();
num8 = vector8.Normalize();
vector9 = Vector3.Cross(vector7, vector8);
rotation2 = Matrix3x3.CreateFromRows(vector7, vector8, vector9);
num9 = (num7 + num8) / 2f;
vector10 = rotation2 * modelPoints[0];
translation2 = new Vector3(imagePoints[0].X / num9 - vector10.X, imagePoints[0].Y / num9 - vector10.Y, focalLength / num9);
}
private float GetError(Point[] imagePoints, Matrix3x3 rotation, Vector3 translation)
{
Vector3 vector = rotation * modelPoints[0] + translation;
vector.X = vector.X * focalLength / vector.Z;
vector.Y = vector.Y * focalLength / vector.Z;
Vector3 vector2 = rotation * modelPoints[1] + translation;
vector2.X = vector2.X * focalLength / vector2.Z;
vector2.Y = vector2.Y * focalLength / vector2.Z;
Vector3 vector3 = rotation * modelPoints[2] + translation;
vector3.X = vector3.X * focalLength / vector3.Z;
vector3.Y = vector3.Y * focalLength / vector3.Z;
Vector3 vector4 = rotation * modelPoints[3] + translation;
vector4.X = vector4.X * focalLength / vector4.Z;
vector4.Y = vector4.Y * focalLength / vector4.Z;
Point[] array = new Point[4]
{
new Point(vector.X, vector.Y),
new Point(vector2.X, vector2.Y),
new Point(vector3.X, vector3.Y),
new Point(vector4.X, vector4.Y)
};
float angleBetweenVectors = GeometryTools.GetAngleBetweenVectors(imagePoints[0], imagePoints[1], imagePoints[3]);
float angleBetweenVectors2 = GeometryTools.GetAngleBetweenVectors(imagePoints[1], imagePoints[2], imagePoints[0]);
float angleBetweenVectors3 = GeometryTools.GetAngleBetweenVectors(imagePoints[2], imagePoints[3], imagePoints[1]);
float angleBetweenVectors4 = GeometryTools.GetAngleBetweenVectors(imagePoints[3], imagePoints[0], imagePoints[2]);
float angleBetweenVectors5 = GeometryTools.GetAngleBetweenVectors(array[0], array[1], array[3]);
float angleBetweenVectors6 = GeometryTools.GetAngleBetweenVectors(array[1], array[2], array[0]);
float angleBetweenVectors7 = GeometryTools.GetAngleBetweenVectors(array[2], array[3], array[1]);
float angleBetweenVectors8 = GeometryTools.GetAngleBetweenVectors(array[3], array[0], array[2]);
return (System.Math.Abs(angleBetweenVectors - angleBetweenVectors5) + System.Math.Abs(angleBetweenVectors2 - angleBetweenVectors6) + System.Math.Abs(angleBetweenVectors3 - angleBetweenVectors7) + System.Math.Abs(angleBetweenVectors4 - angleBetweenVectors8)) / 4f;
}
}

View File

@@ -0,0 +1,76 @@
using System;
using System.Collections.Generic;
namespace AForge.Math.Geometry;
public class FlatAnglesOptimizer : IShapeOptimizer
{
private float maxAngleToKeep = 160f;
public float MaxAngleToKeep
{
get
{
return maxAngleToKeep;
}
set
{
maxAngleToKeep = System.Math.Min(180f, System.Math.Max(140f, value));
}
}
public FlatAnglesOptimizer()
{
}
public FlatAnglesOptimizer(float maxAngleToKeep)
{
this.maxAngleToKeep = maxAngleToKeep;
}
public List<IntPoint> OptimizeShape(List<IntPoint> shape)
{
List<IntPoint> list = new List<IntPoint>();
if (shape.Count <= 3)
{
list.AddRange(shape);
}
else
{
float num = 0f;
list.Add(shape[0]);
list.Add(shape[1]);
int num2 = 2;
int i = 2;
for (int count = shape.Count; i < count; i++)
{
list.Add(shape[i]);
num2++;
num = GeometryTools.GetAngleBetweenVectors(list[num2 - 2], list[num2 - 3], list[num2 - 1]);
if (num > maxAngleToKeep && (num2 > 3 || i < count - 1))
{
list.RemoveAt(num2 - 2);
num2--;
}
}
if (num2 > 3)
{
num = GeometryTools.GetAngleBetweenVectors(list[num2 - 1], list[num2 - 2], list[0]);
if (num > maxAngleToKeep)
{
list.RemoveAt(num2 - 1);
num2--;
}
if (num2 > 3)
{
num = GeometryTools.GetAngleBetweenVectors(list[0], list[num2 - 1], list[1]);
if (num > maxAngleToKeep)
{
list.RemoveAt(0);
}
}
}
}
return list;
}
}

View File

@@ -0,0 +1,21 @@
using System;
namespace AForge.Math.Geometry;
public static class GeometryTools
{
public static float GetAngleBetweenVectors(Point startPoint, Point vector1end, Point vector2end)
{
float num = vector1end.X - startPoint.X;
float num2 = vector1end.Y - startPoint.Y;
float num3 = vector2end.X - startPoint.X;
float num4 = vector2end.Y - startPoint.Y;
return (float)(System.Math.Acos((double)(num * num3 + num2 * num4) / (System.Math.Sqrt(num * num + num2 * num2) * System.Math.Sqrt(num3 * num3 + num4 * num4))) * 180.0 / System.Math.PI);
}
public static float GetAngleBetweenLines(Point a1, Point a2, Point b1, Point b2)
{
Line line = Line.FromPoints(a1, a2);
return line.GetAngleBetweenLines(Line.FromPoints(b1, b2));
}
}

View File

@@ -0,0 +1,120 @@
using System;
using System.Collections.Generic;
namespace AForge.Math.Geometry;
public class GrahamConvexHull : IConvexHullAlgorithm
{
private class PointToProcess : IComparable
{
public int X;
public int Y;
public float K;
public float Distance;
public PointToProcess(IntPoint point)
{
X = point.X;
Y = point.Y;
K = 0f;
Distance = 0f;
}
public int CompareTo(object obj)
{
PointToProcess pointToProcess = (PointToProcess)obj;
if (!(K < pointToProcess.K))
{
if (!(K > pointToProcess.K))
{
if (!(Distance > pointToProcess.Distance))
{
if (!(Distance < pointToProcess.Distance))
{
return 0;
}
return 1;
}
return -1;
}
return 1;
}
return -1;
}
public IntPoint ToPoint()
{
return new IntPoint(X, Y);
}
}
public List<IntPoint> FindHull(List<IntPoint> points)
{
if (points.Count <= 3)
{
return new List<IntPoint>(points);
}
List<PointToProcess> list = new List<PointToProcess>();
foreach (IntPoint point in points)
{
list.Add(new PointToProcess(point));
}
int index = 0;
PointToProcess pointToProcess = list[0];
int i = 1;
for (int count = list.Count; i < count; i++)
{
if (list[i].X < pointToProcess.X || (list[i].X == pointToProcess.X && list[i].Y < pointToProcess.Y))
{
pointToProcess = list[i];
index = i;
}
}
list.RemoveAt(index);
int j = 0;
for (int count2 = list.Count; j < count2; j++)
{
int num = list[j].X - pointToProcess.X;
int num2 = list[j].Y - pointToProcess.Y;
list[j].Distance = num * num + num2 * num2;
list[j].K = ((num == 0) ? float.PositiveInfinity : ((float)num2 / (float)num));
}
list.Sort();
List<PointToProcess> list2 = new List<PointToProcess>();
list2.Add(pointToProcess);
list2.Add(list[0]);
list.RemoveAt(0);
PointToProcess pointToProcess2 = list2[1];
PointToProcess pointToProcess3 = list2[0];
while (list.Count != 0)
{
PointToProcess pointToProcess4 = list[0];
if (pointToProcess4.K == pointToProcess2.K || pointToProcess4.Distance == 0f)
{
list.RemoveAt(0);
}
else if ((pointToProcess4.X - pointToProcess3.X) * (pointToProcess2.Y - pointToProcess4.Y) - (pointToProcess2.X - pointToProcess4.X) * (pointToProcess4.Y - pointToProcess3.Y) < 0)
{
list2.Add(pointToProcess4);
list.RemoveAt(0);
pointToProcess3 = pointToProcess2;
pointToProcess2 = pointToProcess4;
}
else
{
list2.RemoveAt(list2.Count - 1);
pointToProcess2 = pointToProcess3;
pointToProcess3 = list2[list2.Count - 2];
}
}
List<IntPoint> list3 = new List<IntPoint>();
foreach (PointToProcess item in list2)
{
list3.Add(item.ToPoint());
}
return list3;
}
}

View File

@@ -0,0 +1,8 @@
using System.Collections.Generic;
namespace AForge.Math.Geometry;
public interface IConvexHullAlgorithm
{
List<IntPoint> FindHull(List<IntPoint> points);
}

View File

@@ -0,0 +1,8 @@
using System.Collections.Generic;
namespace AForge.Math.Geometry;
public interface IShapeOptimizer
{
List<IntPoint> OptimizeShape(List<IntPoint> shape);
}

View File

@@ -0,0 +1,206 @@
using System;
using System.Globalization;
namespace AForge.Math.Geometry;
public sealed class Line
{
private readonly float k;
private readonly float b;
public bool IsVertical => float.IsInfinity(k);
public bool IsHorizontal => k == 0f;
public float Slope => k;
public float Intercept => b;
public static Line FromPoints(Point point1, Point point2)
{
return new Line(point1, point2);
}
public static Line FromSlopeIntercept(float slope, float intercept)
{
return new Line(slope, intercept);
}
public static Line FromRTheta(float radius, float theta)
{
return new Line(radius, theta, unused: false);
}
public static Line FromPointTheta(Point point, float theta)
{
return new Line(point, theta);
}
private Line(Point start, Point end)
{
if (start == end)
{
throw new ArgumentException("Start point of the line cannot be the same as its end point.");
}
k = (end.Y - start.Y) / (end.X - start.X);
b = (float.IsInfinity(k) ? start.X : (start.Y - k * start.X));
}
private Line(float slope, float intercept)
{
k = slope;
b = intercept;
}
private Line(float radius, float theta, bool unused)
{
if (radius < 0f)
{
throw new ArgumentOutOfRangeException("radius", radius, "Must be non-negative");
}
theta *= (float)System.Math.PI / 180f;
float num = (float)System.Math.Sin(theta);
float num2 = (float)System.Math.Cos(theta);
Point point = new Point(radius * num2, radius * num);
k = (0f - num2) / num;
if (!float.IsInfinity(k))
{
b = point.Y - k * point.X;
}
else
{
b = System.Math.Abs(radius);
}
}
private Line(Point point, float theta)
{
theta *= (float)System.Math.PI / 180f;
k = (float)(-1.0 / System.Math.Tan(theta));
if (!float.IsInfinity(k))
{
b = point.Y - k * point.X;
}
else
{
b = point.X;
}
}
public float GetAngleBetweenLines(Line secondLine)
{
float num = secondLine.k;
bool isVertical = IsVertical;
bool isVertical2 = secondLine.IsVertical;
if (k == num || (isVertical && isVertical2))
{
return 0f;
}
float num2 = 0f;
if (isVertical || isVertical2)
{
num2 = ((!isVertical) ? ((float)(System.Math.PI / 2.0 - System.Math.Atan(k) * (double)System.Math.Sign(k))) : ((float)(System.Math.PI / 2.0 - System.Math.Atan(num) * (double)System.Math.Sign(num))));
}
else
{
float num3 = ((num > k) ? (num - k) : (k - num)) / (1f + k * num);
num2 = (float)System.Math.Atan(num3);
}
num2 *= 57.29578f;
if (num2 < 0f)
{
num2 = 0f - num2;
}
return num2;
}
public Point? GetIntersectionWith(Line secondLine)
{
float num = secondLine.k;
float num2 = secondLine.b;
bool isVertical = IsVertical;
bool isVertical2 = secondLine.IsVertical;
Point? result = null;
if (k == num || (isVertical && isVertical2))
{
if (b == num2)
{
throw new InvalidOperationException("Identical lines do not have an intersection point.");
}
}
else if (isVertical)
{
result = new Point(b, num * b + num2);
}
else if (isVertical2)
{
result = new Point(num2, k * num2 + b);
}
else
{
float num3 = (num2 - b) / (k - num);
result = new Point(num3, k * num3 + b);
}
return result;
}
public Point? GetIntersectionWith(LineSegment other)
{
return other.GetIntersectionWith(this);
}
public float DistanceToPoint(Point point)
{
if (!IsVertical)
{
float num = (float)System.Math.Sqrt(k * k + 1f);
return System.Math.Abs((k * point.X + b - point.Y) / num);
}
return System.Math.Abs(b - point.X);
}
public static bool operator ==(Line line1, Line line2)
{
if (object.ReferenceEquals(line1, line2))
{
return true;
}
if ((object)line1 == null || (object)line2 == null)
{
return false;
}
if (line1.k == line2.k)
{
return line1.b == line2.b;
}
return false;
}
public static bool operator !=(Line line1, Line line2)
{
return !(line1 == line2);
}
public override bool Equals(object obj)
{
if (!(obj is Line))
{
return false;
}
return this == (Line)obj;
}
public override int GetHashCode()
{
float num = k;
int hashCode = num.GetHashCode();
float num2 = b;
return hashCode + num2.GetHashCode();
}
public override string ToString()
{
return string.Format(CultureInfo.InvariantCulture, "k = {0}, b = {1}", new object[2] { k, b });
}
}

View File

@@ -0,0 +1,158 @@
using System;
using System.Globalization;
namespace AForge.Math.Geometry;
public sealed class LineSegment
{
private enum ProjectionLocation
{
RayA,
SegmentAB,
RayB
}
private readonly Point start;
private readonly Point end;
private readonly Line line;
public Point Start => start;
public Point End => end;
public float Length => start.DistanceTo(end);
public LineSegment(Point start, Point end)
{
line = Line.FromPoints(start, end);
this.start = start;
this.end = end;
}
public static explicit operator Line(LineSegment segment)
{
return segment.line;
}
public float DistanceToPoint(Point point)
{
return LocateProjection(point) switch
{
ProjectionLocation.RayA => point.DistanceTo(start),
ProjectionLocation.RayB => point.DistanceTo(end),
_ => line.DistanceToPoint(point),
};
}
public Point? GetIntersectionWith(LineSegment other)
{
Point? result = null;
if (line.Slope == other.line.Slope || (line.IsVertical && other.line.IsVertical))
{
if (line.Intercept == other.line.Intercept)
{
ProjectionLocation projectionLocation = LocateProjection(other.start);
ProjectionLocation projectionLocation2 = LocateProjection(other.end);
if (projectionLocation != ProjectionLocation.SegmentAB && projectionLocation == projectionLocation2)
{
result = null;
}
else if ((start == other.start && projectionLocation2 == ProjectionLocation.RayA) || (start == other.end && projectionLocation == ProjectionLocation.RayA))
{
result = start;
}
else
{
if ((!(end == other.start) || projectionLocation2 != ProjectionLocation.RayB) && (!(end == other.end) || projectionLocation != ProjectionLocation.RayB))
{
throw new InvalidOperationException("Overlapping segments do not have a single intersection point.");
}
result = end;
}
}
}
else
{
result = GetIntersectionWith(other.line);
if (result.HasValue && other.LocateProjection(result.Value) != ProjectionLocation.SegmentAB)
{
result = null;
}
}
return result;
}
public Point? GetIntersectionWith(Line other)
{
Point? result;
if (line.Slope == other.Slope || (line.IsVertical && other.IsVertical))
{
if (line.Intercept == other.Intercept)
{
throw new InvalidOperationException("Segment is a portion of the specified line.");
}
result = null;
}
else
{
result = line.GetIntersectionWith(other);
}
if (result.HasValue && LocateProjection(result.Value) != ProjectionLocation.SegmentAB)
{
result = null;
}
return result;
}
private ProjectionLocation LocateProjection(Point point)
{
Point point2 = end - start;
Point point3 = point - start;
float num = point3.X * point2.X + point3.Y * point2.Y;
float num2 = point2.X * point2.X + point2.Y * point2.Y;
return (!(num < 0f)) ? ((!(num > num2)) ? ProjectionLocation.SegmentAB : ProjectionLocation.RayB) : ProjectionLocation.RayA;
}
public static bool operator ==(LineSegment line1, LineSegment line2)
{
if (object.ReferenceEquals(line1, line2))
{
return true;
}
if ((object)line1 == null || (object)line2 == null)
{
return false;
}
if (line1.start == line2.start)
{
return line1.end == line2.end;
}
return false;
}
public static bool operator !=(LineSegment line1, LineSegment line2)
{
return !(line1 == line2);
}
public override bool Equals(object obj)
{
if (!(obj is LineSegment))
{
return false;
}
return this == (LineSegment)obj;
}
public override int GetHashCode()
{
return start.GetHashCode() + end.GetHashCode();
}
public override string ToString()
{
return string.Format(CultureInfo.InvariantCulture, "({0}) -> ({1})", new object[2] { start, end });
}
}

View File

@@ -0,0 +1,88 @@
using System;
using System.Collections.Generic;
namespace AForge.Math.Geometry;
public class LineStraighteningOptimizer : IShapeOptimizer
{
private float maxDistanceToRemove = 5f;
public float MaxDistanceToRemove
{
get
{
return maxDistanceToRemove;
}
set
{
maxDistanceToRemove = System.Math.Max(0f, value);
}
}
public LineStraighteningOptimizer()
{
}
public LineStraighteningOptimizer(float maxDistanceToRemove)
{
this.maxDistanceToRemove = maxDistanceToRemove;
}
public List<IntPoint> OptimizeShape(List<IntPoint> shape)
{
List<IntPoint> list = new List<IntPoint>();
List<IntPoint> list2 = new List<IntPoint>();
if (shape.Count <= 3)
{
list.AddRange(shape);
}
else
{
float distance = 0f;
list.Add(shape[0]);
list.Add(shape[1]);
int num = 2;
int i = 2;
for (int count = shape.Count; i < count; i++)
{
list.Add(shape[i]);
num++;
list2.Add(list[num - 2]);
PointsCloud.GetFurthestPointFromLine(list2, list[num - 3], list[num - 1], out distance);
if (distance <= maxDistanceToRemove && (num > 3 || i < count - 1))
{
list.RemoveAt(num - 2);
num--;
}
else
{
list2.Clear();
}
}
if (num > 3)
{
list2.Add(list[num - 1]);
PointsCloud.GetFurthestPointFromLine(list2, list[num - 2], list[0], out distance);
if (distance <= maxDistanceToRemove)
{
list.RemoveAt(num - 1);
num--;
}
else
{
list2.Clear();
}
if (num > 3)
{
list2.Add(list[0]);
PointsCloud.GetFurthestPointFromLine(list2, list[num - 1], list[1], out distance);
if (distance <= maxDistanceToRemove)
{
list.RemoveAt(0);
}
}
}
}
return list;
}
}

View File

@@ -0,0 +1,321 @@
using System;
using System.Collections.Generic;
namespace AForge.Math.Geometry;
public static class PointsCloud
{
private static float quadrilateralRelativeDistortionLimit = 0.1f;
public static float QuadrilateralRelativeDistortionLimit
{
get
{
return quadrilateralRelativeDistortionLimit;
}
set
{
quadrilateralRelativeDistortionLimit = System.Math.Max(0f, System.Math.Min(0.25f, value));
}
}
public static void Shift(IList<IntPoint> cloud, IntPoint shift)
{
int i = 0;
for (int count = cloud.Count; i < count; i++)
{
cloud[i] += shift;
}
}
public static void GetBoundingRectangle(IEnumerable<IntPoint> cloud, out IntPoint minXY, out IntPoint maxXY)
{
int num = int.MaxValue;
int num2 = int.MinValue;
int num3 = int.MaxValue;
int num4 = int.MinValue;
foreach (IntPoint item in cloud)
{
int x = item.X;
int y = item.Y;
if (x < num)
{
num = x;
}
if (x > num2)
{
num2 = x;
}
if (y < num3)
{
num3 = y;
}
if (y > num4)
{
num4 = y;
}
}
if (num > num2)
{
throw new ArgumentException("List of points can not be empty.");
}
minXY = new IntPoint(num, num3);
maxXY = new IntPoint(num2, num4);
}
public static Point GetCenterOfGravity(IEnumerable<IntPoint> cloud)
{
int num = 0;
float num2 = 0f;
float num3 = 0f;
foreach (IntPoint item in cloud)
{
num2 += (float)item.X;
num3 += (float)item.Y;
num++;
}
num2 /= (float)num;
num3 /= (float)num;
return new Point(num2, num3);
}
public static IntPoint GetFurthestPoint(IEnumerable<IntPoint> cloud, IntPoint referencePoint)
{
IntPoint result = referencePoint;
float num = -1f;
int x = referencePoint.X;
int y = referencePoint.Y;
foreach (IntPoint item in cloud)
{
int num2 = x - item.X;
int num3 = y - item.Y;
float num4 = num2 * num2 + num3 * num3;
if (num4 > num)
{
num = num4;
result = item;
}
}
return result;
}
public static void GetFurthestPointsFromLine(IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2, out IntPoint furthestPoint1, out IntPoint furthestPoint2)
{
GetFurthestPointsFromLine(cloud, linePoint1, linePoint2, out furthestPoint1, out var _, out furthestPoint2, out var _);
}
public static void GetFurthestPointsFromLine(IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2, out IntPoint furthestPoint1, out float distance1, out IntPoint furthestPoint2, out float distance2)
{
furthestPoint1 = linePoint1;
distance1 = 0f;
furthestPoint2 = linePoint2;
distance2 = 0f;
if (linePoint2.X != linePoint1.X)
{
float num = (float)(linePoint2.Y - linePoint1.Y) / (float)(linePoint2.X - linePoint1.X);
float num2 = (float)linePoint1.Y - num * (float)linePoint1.X;
float num3 = (float)System.Math.Sqrt(num * num + 1f);
float num4 = 0f;
foreach (IntPoint item in cloud)
{
num4 = (num * (float)item.X + num2 - (float)item.Y) / num3;
if (num4 > distance1)
{
distance1 = num4;
furthestPoint1 = item;
}
if (num4 < distance2)
{
distance2 = num4;
furthestPoint2 = item;
}
}
}
else
{
int x = linePoint1.X;
float num5 = 0f;
foreach (IntPoint item2 in cloud)
{
num5 = x - item2.X;
if (num5 > distance1)
{
distance1 = num5;
furthestPoint1 = item2;
}
if (num5 < distance2)
{
distance2 = num5;
furthestPoint2 = item2;
}
}
}
distance2 = 0f - distance2;
}
public static IntPoint GetFurthestPointFromLine(IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2)
{
float distance;
return GetFurthestPointFromLine(cloud, linePoint1, linePoint2, out distance);
}
public static IntPoint GetFurthestPointFromLine(IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2, out float distance)
{
IntPoint result = linePoint1;
distance = 0f;
if (linePoint2.X != linePoint1.X)
{
float num = (float)(linePoint2.Y - linePoint1.Y) / (float)(linePoint2.X - linePoint1.X);
float num2 = (float)linePoint1.Y - num * (float)linePoint1.X;
float num3 = (float)System.Math.Sqrt(num * num + 1f);
float num4 = 0f;
foreach (IntPoint item in cloud)
{
num4 = System.Math.Abs((num * (float)item.X + num2 - (float)item.Y) / num3);
if (num4 > distance)
{
distance = num4;
result = item;
}
}
}
else
{
int x = linePoint1.X;
float num5 = 0f;
foreach (IntPoint item2 in cloud)
{
distance = System.Math.Abs(x - item2.X);
if (num5 > distance)
{
distance = num5;
result = item2;
}
}
}
return result;
}
public static List<IntPoint> FindQuadrilateralCorners(IEnumerable<IntPoint> cloud)
{
List<IntPoint> list = new List<IntPoint>();
GetBoundingRectangle(cloud, out var minXY, out var maxXY);
IntPoint intPoint = maxXY - minXY;
IntPoint referencePoint = minXY + intPoint / 2;
float num = quadrilateralRelativeDistortionLimit * (float)(intPoint.X + intPoint.Y) / 2f;
IntPoint furthestPoint = GetFurthestPoint(cloud, referencePoint);
IntPoint furthestPoint2 = GetFurthestPoint(cloud, furthestPoint);
list.Add(furthestPoint);
list.Add(furthestPoint2);
GetFurthestPointsFromLine(cloud, furthestPoint, furthestPoint2, out var furthestPoint3, out var distance, out var furthestPoint4, out var distance2);
if ((distance >= num && distance2 >= num) || (distance < num && distance != 0f && distance2 < num && distance2 != 0f))
{
if (!list.Contains(furthestPoint3))
{
list.Add(furthestPoint3);
}
if (!list.Contains(furthestPoint4))
{
list.Add(furthestPoint4);
}
}
else
{
IntPoint furthestPoint5 = ((distance > distance2) ? furthestPoint3 : furthestPoint4);
GetFurthestPointsFromLine(cloud, furthestPoint, furthestPoint5, out furthestPoint3, out distance, out furthestPoint4, out distance2);
bool flag = false;
if (distance >= num && distance2 >= num)
{
if (furthestPoint4.DistanceTo(furthestPoint2) > furthestPoint3.DistanceTo(furthestPoint2))
{
furthestPoint3 = furthestPoint4;
}
flag = true;
}
else
{
GetFurthestPointsFromLine(cloud, furthestPoint2, furthestPoint5, out furthestPoint3, out distance, out furthestPoint4, out distance2);
if (distance >= num && distance2 >= num)
{
if (furthestPoint4.DistanceTo(furthestPoint) > furthestPoint3.DistanceTo(furthestPoint))
{
furthestPoint3 = furthestPoint4;
}
flag = true;
}
}
if (!flag)
{
list.Add(furthestPoint5);
}
else
{
list.Add(furthestPoint3);
GetFurthestPointsFromLine(cloud, furthestPoint, furthestPoint3, out furthestPoint5, out var distance3, out furthestPoint4, out distance2);
if (distance2 >= num && distance3 >= num)
{
if (furthestPoint5.DistanceTo(furthestPoint2) > furthestPoint4.DistanceTo(furthestPoint2))
{
furthestPoint4 = furthestPoint5;
}
}
else
{
GetFurthestPointsFromLine(cloud, furthestPoint2, furthestPoint3, out furthestPoint5, out distance3, out furthestPoint4, out distance2);
if (furthestPoint5.DistanceTo(furthestPoint) > furthestPoint4.DistanceTo(furthestPoint) && furthestPoint5 != furthestPoint2 && furthestPoint5 != furthestPoint3)
{
furthestPoint4 = furthestPoint5;
}
}
if (furthestPoint4 != furthestPoint && furthestPoint4 != furthestPoint2 && furthestPoint4 != furthestPoint3)
{
list.Add(furthestPoint4);
}
}
}
int i = 1;
for (int count = list.Count; i < count; i++)
{
if (list[i].X < list[0].X || (list[i].X == list[0].X && list[i].Y < list[0].Y))
{
IntPoint value = list[i];
list[i] = list[0];
list[0] = value;
}
}
float num2 = ((list[1].X != list[0].X) ? ((float)(list[1].Y - list[0].Y) / (float)(list[1].X - list[0].X)) : ((list[1].Y > list[0].Y) ? float.PositiveInfinity : float.NegativeInfinity));
float num3 = ((list[2].X != list[0].X) ? ((float)(list[2].Y - list[0].Y) / (float)(list[2].X - list[0].X)) : ((list[2].Y > list[0].Y) ? float.PositiveInfinity : float.NegativeInfinity));
if (num3 < num2)
{
IntPoint value2 = list[1];
list[1] = list[2];
list[2] = value2;
float num4 = num2;
num2 = num3;
num3 = num4;
}
if (list.Count == 4)
{
float num5 = ((list[3].X != list[0].X) ? ((float)(list[3].Y - list[0].Y) / (float)(list[3].X - list[0].X)) : ((list[3].Y > list[0].Y) ? float.PositiveInfinity : float.NegativeInfinity));
if (num5 < num2)
{
IntPoint value3 = list[1];
list[1] = list[3];
list[3] = value3;
float num6 = num2;
num2 = num5;
num5 = num6;
}
if (num5 < num3)
{
IntPoint value4 = list[2];
list[2] = list[3];
list[3] = value4;
float num7 = num3;
num3 = num5;
num5 = num7;
}
}
return list;
}
}

View File

@@ -0,0 +1,15 @@
namespace AForge.Math.Geometry;
public enum PolygonSubType
{
Unknown,
Trapezoid,
Parallelogram,
Rectangle,
Rhombus,
Square,
EquilateralTriangle,
IsoscelesTriangle,
RectangledTriangle,
RectangledIsoscelesTriangle
}

View File

@@ -0,0 +1,84 @@
using System;
namespace AForge.Math.Geometry;
public class Posit
{
private const float stop_epsilon = 0.0001f;
private float focalLength;
private Vector3[] modelPoints;
private Matrix3x3 modelVectors;
private Matrix3x3 modelPseudoInverse;
public Vector3[] Model => (Vector3[])modelPoints.Clone();
public float FocalLength
{
get
{
return focalLength;
}
set
{
focalLength = value;
}
}
public Posit(Vector3[] model, float focalLength)
{
if (model.Length != 4)
{
throw new ArgumentException("The model must have 4 points.");
}
this.focalLength = focalLength;
modelPoints = (Vector3[])model.Clone();
modelVectors = Matrix3x3.CreateFromRows(model[1] - model[0], model[2] - model[0], model[3] - model[0]);
modelPseudoInverse = modelVectors.PseudoInverse();
}
public void EstimatePose(Point[] points, out Matrix3x3 rotation, out Vector3 translation)
{
if (points.Length != 4)
{
throw new ArgumentException("4 points must be be given for pose estimation.");
}
float num = 0f;
float num2 = 1f;
Vector3 vector = new Vector3(points[0].X);
Vector3 vector2 = new Vector3(points[0].Y);
Vector3 vector3 = new Vector3(points[1].X, points[2].X, points[3].X);
Vector3 vector4 = new Vector3(points[1].Y, points[2].Y, points[3].Y);
int i = 0;
Vector3 vector5 = default(Vector3);
Vector3 vector6 = default(Vector3);
Vector3 vector7 = default(Vector3);
Vector3 vector8 = default(Vector3);
Vector3 vector9 = default(Vector3);
Vector3 vector10 = new Vector3(1f);
for (; i < 100; i++)
{
vector8 = vector3 * vector10 - vector;
vector9 = vector4 * vector10 - vector2;
vector5 = modelPseudoInverse * vector8;
vector6 = modelPseudoInverse * vector9;
float num3 = vector5.Normalize();
float num4 = vector6.Normalize();
num2 = (num3 + num4) / 2f;
vector7 = Vector3.Cross(vector5, vector6);
num = focalLength / num2;
Vector3 vector11 = vector10;
vector10 = modelVectors * vector7 / num + 1f;
if ((vector10 - vector11).Abs().Max < 0.0001f)
{
break;
}
}
rotation = Matrix3x3.CreateFromRows(vector5, vector6, vector7);
Vector3 vector12 = rotation * modelPoints[0];
translation = new Vector3(points[0].X / num2 - vector12.X, points[0].Y / num2 - vector12.Y, focalLength / num2);
}
}

View File

@@ -0,0 +1,9 @@
namespace AForge.Math.Geometry;
public enum ShapeType
{
Unknown,
Circle,
Triangle,
Quadrilateral
}

View File

@@ -0,0 +1,253 @@
using System;
using System.Collections.Generic;
namespace AForge.Math.Geometry;
public class SimpleShapeChecker
{
private FlatAnglesOptimizer shapeOptimizer = new FlatAnglesOptimizer(160f);
private float minAcceptableDistortion = 0.5f;
private float relativeDistortionLimit = 0.03f;
private float angleError = 7f;
private float lengthError = 0.1f;
public float MinAcceptableDistortion
{
get
{
return minAcceptableDistortion;
}
set
{
minAcceptableDistortion = System.Math.Max(0f, value);
}
}
public float RelativeDistortionLimit
{
get
{
return relativeDistortionLimit;
}
set
{
relativeDistortionLimit = System.Math.Max(0f, System.Math.Min(1f, value));
}
}
public float AngleError
{
get
{
return angleError;
}
set
{
angleError = System.Math.Max(0f, System.Math.Min(20f, value));
}
}
public float LengthError
{
get
{
return lengthError;
}
set
{
lengthError = System.Math.Max(0f, System.Math.Min(1f, value));
}
}
public ShapeType CheckShapeType(List<IntPoint> edgePoints)
{
if (IsCircle(edgePoints))
{
return ShapeType.Circle;
}
if (IsConvexPolygon(edgePoints, out var corners))
{
if (corners.Count != 4)
{
return ShapeType.Triangle;
}
return ShapeType.Quadrilateral;
}
return ShapeType.Unknown;
}
public bool IsCircle(List<IntPoint> edgePoints)
{
Point center;
float radius;
return IsCircle(edgePoints, out center, out radius);
}
public bool IsCircle(List<IntPoint> edgePoints, out Point center, out float radius)
{
if (edgePoints.Count < 8)
{
center = new Point(0f, 0f);
radius = 0f;
return false;
}
PointsCloud.GetBoundingRectangle(edgePoints, out var minXY, out var maxXY);
IntPoint intPoint = maxXY - minXY;
center = minXY + (Point)intPoint / 2f;
radius = ((float)intPoint.X + (float)intPoint.Y) / 4f;
float num = 0f;
int i = 0;
for (int count = edgePoints.Count; i < count; i++)
{
num += System.Math.Abs(center.DistanceTo(edgePoints[i]) - radius);
}
num /= (float)edgePoints.Count;
float num2 = System.Math.Max(minAcceptableDistortion, ((float)intPoint.X + (float)intPoint.Y) / 2f * relativeDistortionLimit);
return num <= num2;
}
public bool IsQuadrilateral(List<IntPoint> edgePoints)
{
List<IntPoint> corners;
return IsQuadrilateral(edgePoints, out corners);
}
public bool IsQuadrilateral(List<IntPoint> edgePoints, out List<IntPoint> corners)
{
corners = GetShapeCorners(edgePoints);
if (corners.Count != 4)
{
return false;
}
return CheckIfPointsFitShape(edgePoints, corners);
}
public bool IsTriangle(List<IntPoint> edgePoints)
{
List<IntPoint> corners;
return IsTriangle(edgePoints, out corners);
}
public bool IsTriangle(List<IntPoint> edgePoints, out List<IntPoint> corners)
{
corners = GetShapeCorners(edgePoints);
if (corners.Count != 3)
{
return false;
}
return CheckIfPointsFitShape(edgePoints, corners);
}
public bool IsConvexPolygon(List<IntPoint> edgePoints, out List<IntPoint> corners)
{
corners = GetShapeCorners(edgePoints);
return CheckIfPointsFitShape(edgePoints, corners);
}
public PolygonSubType CheckPolygonSubType(List<IntPoint> corners)
{
PolygonSubType polygonSubType = PolygonSubType.Unknown;
PointsCloud.GetBoundingRectangle(corners, out var minXY, out var maxXY);
IntPoint intPoint = maxXY - minXY;
float num = lengthError * (float)(intPoint.X + intPoint.Y) / 2f;
if (corners.Count == 3)
{
float angleBetweenVectors = GeometryTools.GetAngleBetweenVectors(corners[0], corners[1], corners[2]);
float angleBetweenVectors2 = GeometryTools.GetAngleBetweenVectors(corners[1], corners[2], corners[0]);
float angleBetweenVectors3 = GeometryTools.GetAngleBetweenVectors(corners[2], corners[0], corners[1]);
if (System.Math.Abs(angleBetweenVectors - 60f) <= angleError && System.Math.Abs(angleBetweenVectors2 - 60f) <= angleError && System.Math.Abs(angleBetweenVectors3 - 60f) <= angleError)
{
polygonSubType = PolygonSubType.EquilateralTriangle;
}
else
{
if (System.Math.Abs(angleBetweenVectors - angleBetweenVectors2) <= angleError || System.Math.Abs(angleBetweenVectors2 - angleBetweenVectors3) <= angleError || System.Math.Abs(angleBetweenVectors3 - angleBetweenVectors) <= angleError)
{
polygonSubType = PolygonSubType.IsoscelesTriangle;
}
if (System.Math.Abs(angleBetweenVectors - 90f) <= angleError || System.Math.Abs(angleBetweenVectors2 - 90f) <= angleError || System.Math.Abs(angleBetweenVectors3 - 90f) <= angleError)
{
polygonSubType = ((polygonSubType == PolygonSubType.IsoscelesTriangle) ? PolygonSubType.RectangledIsoscelesTriangle : PolygonSubType.RectangledTriangle);
}
}
}
else if (corners.Count == 4)
{
float angleBetweenLines = GeometryTools.GetAngleBetweenLines(corners[0], corners[1], corners[2], corners[3]);
float angleBetweenLines2 = GeometryTools.GetAngleBetweenLines(corners[1], corners[2], corners[3], corners[0]);
if (angleBetweenLines <= angleError)
{
polygonSubType = PolygonSubType.Trapezoid;
if (angleBetweenLines2 <= angleError)
{
polygonSubType = PolygonSubType.Parallelogram;
if (System.Math.Abs(GeometryTools.GetAngleBetweenVectors(corners[1], corners[0], corners[2]) - 90f) <= angleError)
{
polygonSubType = PolygonSubType.Rectangle;
}
float num2 = corners[0].DistanceTo(corners[1]);
float num3 = corners[0].DistanceTo(corners[3]);
if (System.Math.Abs(num2 - num3) <= num)
{
polygonSubType = ((polygonSubType == PolygonSubType.Parallelogram) ? PolygonSubType.Rhombus : PolygonSubType.Square);
}
}
}
else if (angleBetweenLines2 <= angleError)
{
polygonSubType = PolygonSubType.Trapezoid;
}
}
return polygonSubType;
}
public bool CheckIfPointsFitShape(List<IntPoint> edgePoints, List<IntPoint> corners)
{
int count = corners.Count;
float[] array = new float[count];
float[] array2 = new float[count];
float[] array3 = new float[count];
bool[] array4 = new bool[count];
for (int i = 0; i < count; i++)
{
IntPoint intPoint = corners[i];
IntPoint intPoint2 = ((i + 1 == count) ? corners[0] : corners[i + 1]);
if (!(array4[i] = intPoint2.X == intPoint.X))
{
array[i] = (float)(intPoint2.Y - intPoint.Y) / (float)(intPoint2.X - intPoint.X);
array2[i] = (float)intPoint.Y - array[i] * (float)intPoint.X;
array3[i] = (float)System.Math.Sqrt(array[i] * array[i] + 1f);
}
}
float num = 0f;
int j = 0;
for (int count2 = edgePoints.Count; j < count2; j++)
{
float num2 = float.MaxValue;
for (int k = 0; k < count; k++)
{
float num3 = 0f;
num3 = (array4[k] ? ((float)System.Math.Abs(edgePoints[j].X - corners[k].X)) : System.Math.Abs((array[k] * (float)edgePoints[j].X + array2[k] - (float)edgePoints[j].Y) / array3[k]));
if (num3 < num2)
{
num2 = num3;
}
}
num += num2;
}
num /= (float)edgePoints.Count;
PointsCloud.GetBoundingRectangle(corners, out var minXY, out var maxXY);
IntPoint intPoint3 = maxXY - minXY;
float num4 = System.Math.Max(minAcceptableDistortion, ((float)intPoint3.X + (float)intPoint3.Y) / 2f * relativeDistortionLimit);
return num <= num4;
}
private List<IntPoint> GetShapeCorners(List<IntPoint> edgePoints)
{
return shapeOptimizer.OptimizeShape(PointsCloud.FindQuadrilateralCorners(edgePoints));
}
}

View File

@@ -0,0 +1,72 @@
using System;
namespace AForge.Math;
[Serializable]
public class Histogram
{
private int[] values;
private double mean;
private double stdDev;
private int median;
private int min;
private int max;
private long total;
public int[] Values => values;
public double Mean => mean;
public double StdDev => stdDev;
public int Median => median;
public int Min => min;
public int Max => max;
public long TotalCount => total;
public Histogram(int[] values)
{
this.values = values;
Update();
}
public IntRange GetRange(double percent)
{
return Statistics.GetRange(values, percent);
}
public void Update()
{
int num = values.Length;
max = 0;
min = num;
total = 0L;
for (int i = 0; i < num; i++)
{
if (values[i] != 0)
{
if (i > max)
{
max = i;
}
if (i < min)
{
min = i;
}
total += values[i];
}
}
mean = Statistics.Mean(values);
stdDev = Statistics.StdDev(values, mean);
median = Statistics.Median(values);
}
}

View File

@@ -0,0 +1,424 @@
using System;
namespace AForge.Math;
[Serializable]
public struct Matrix3x3
{
public float V00;
public float V01;
public float V02;
public float V10;
public float V11;
public float V12;
public float V20;
public float V21;
public float V22;
public static Matrix3x3 Identity
{
get
{
Matrix3x3 result = default(Matrix3x3);
result.V00 = (result.V11 = (result.V22 = 1f));
return result;
}
}
public float Determinant => V00 * V11 * V22 + V01 * V12 * V20 + V02 * V10 * V21 - V00 * V12 * V21 - V01 * V10 * V22 - V02 * V11 * V20;
public float[] ToArray()
{
return new float[9] { V00, V01, V02, V10, V11, V12, V20, V21, V22 };
}
public static Matrix3x3 CreateRotationY(float radians)
{
Matrix3x3 result = default(Matrix3x3);
float v = (float)System.Math.Cos(radians);
float num = (float)System.Math.Sin(radians);
result.V00 = (result.V22 = v);
result.V02 = num;
result.V20 = 0f - num;
result.V11 = 1f;
return result;
}
public static Matrix3x3 CreateRotationX(float radians)
{
Matrix3x3 result = default(Matrix3x3);
float v = (float)System.Math.Cos(radians);
float num = (float)System.Math.Sin(radians);
result.V11 = (result.V22 = v);
result.V12 = 0f - num;
result.V21 = num;
result.V00 = 1f;
return result;
}
public static Matrix3x3 CreateRotationZ(float radians)
{
Matrix3x3 result = default(Matrix3x3);
float v = (float)System.Math.Cos(radians);
float num = (float)System.Math.Sin(radians);
result.V00 = (result.V11 = v);
result.V01 = 0f - num;
result.V10 = num;
result.V22 = 1f;
return result;
}
public static Matrix3x3 CreateFromYawPitchRoll(float yaw, float pitch, float roll)
{
return CreateRotationY(yaw) * CreateRotationX(pitch) * CreateRotationZ(roll);
}
public void ExtractYawPitchRoll(out float yaw, out float pitch, out float roll)
{
yaw = (float)System.Math.Atan2(V02, V22);
pitch = (float)System.Math.Asin(0f - V12);
roll = (float)System.Math.Atan2(V10, V11);
}
public static Matrix3x3 CreateFromRows(Vector3 row0, Vector3 row1, Vector3 row2)
{
return new Matrix3x3
{
V00 = row0.X,
V01 = row0.Y,
V02 = row0.Z,
V10 = row1.X,
V11 = row1.Y,
V12 = row1.Z,
V20 = row2.X,
V21 = row2.Y,
V22 = row2.Z
};
}
public static Matrix3x3 CreateFromColumns(Vector3 column0, Vector3 column1, Vector3 column2)
{
return new Matrix3x3
{
V00 = column0.X,
V10 = column0.Y,
V20 = column0.Z,
V01 = column1.X,
V11 = column1.Y,
V21 = column1.Z,
V02 = column2.X,
V12 = column2.Y,
V22 = column2.Z
};
}
public static Matrix3x3 CreateDiagonal(Vector3 vector)
{
return new Matrix3x3
{
V00 = vector.X,
V11 = vector.Y,
V22 = vector.Z
};
}
public Vector3 GetRow(int index)
{
return index switch
{
2 => new Vector3(V20, V21, V22),
1 => new Vector3(V10, V11, V12),
0 => new Vector3(V00, V01, V02),
_ => throw new ArgumentException("Invalid row index was specified.", "index"),
};
}
public Vector3 GetColumn(int index)
{
return index switch
{
2 => new Vector3(V02, V12, V22),
1 => new Vector3(V01, V11, V21),
0 => new Vector3(V00, V10, V20),
_ => throw new ArgumentException("Invalid column index was specified.", "index"),
};
}
public static Matrix3x3 operator *(Matrix3x3 matrix1, Matrix3x3 matrix2)
{
return new Matrix3x3
{
V00 = matrix1.V00 * matrix2.V00 + matrix1.V01 * matrix2.V10 + matrix1.V02 * matrix2.V20,
V01 = matrix1.V00 * matrix2.V01 + matrix1.V01 * matrix2.V11 + matrix1.V02 * matrix2.V21,
V02 = matrix1.V00 * matrix2.V02 + matrix1.V01 * matrix2.V12 + matrix1.V02 * matrix2.V22,
V10 = matrix1.V10 * matrix2.V00 + matrix1.V11 * matrix2.V10 + matrix1.V12 * matrix2.V20,
V11 = matrix1.V10 * matrix2.V01 + matrix1.V11 * matrix2.V11 + matrix1.V12 * matrix2.V21,
V12 = matrix1.V10 * matrix2.V02 + matrix1.V11 * matrix2.V12 + matrix1.V12 * matrix2.V22,
V20 = matrix1.V20 * matrix2.V00 + matrix1.V21 * matrix2.V10 + matrix1.V22 * matrix2.V20,
V21 = matrix1.V20 * matrix2.V01 + matrix1.V21 * matrix2.V11 + matrix1.V22 * matrix2.V21,
V22 = matrix1.V20 * matrix2.V02 + matrix1.V21 * matrix2.V12 + matrix1.V22 * matrix2.V22
};
}
public static Matrix3x3 Multiply(Matrix3x3 matrix1, Matrix3x3 matrix2)
{
return matrix1 * matrix2;
}
public static Matrix3x3 operator +(Matrix3x3 matrix1, Matrix3x3 matrix2)
{
return new Matrix3x3
{
V00 = matrix1.V00 + matrix2.V00,
V01 = matrix1.V01 + matrix2.V01,
V02 = matrix1.V02 + matrix2.V02,
V10 = matrix1.V10 + matrix2.V10,
V11 = matrix1.V11 + matrix2.V11,
V12 = matrix1.V12 + matrix2.V12,
V20 = matrix1.V20 + matrix2.V20,
V21 = matrix1.V21 + matrix2.V21,
V22 = matrix1.V22 + matrix2.V22
};
}
public static Matrix3x3 Add(Matrix3x3 matrix1, Matrix3x3 matrix2)
{
return matrix1 + matrix2;
}
public static Matrix3x3 operator -(Matrix3x3 matrix1, Matrix3x3 matrix2)
{
return new Matrix3x3
{
V00 = matrix1.V00 - matrix2.V00,
V01 = matrix1.V01 - matrix2.V01,
V02 = matrix1.V02 - matrix2.V02,
V10 = matrix1.V10 - matrix2.V10,
V11 = matrix1.V11 - matrix2.V11,
V12 = matrix1.V12 - matrix2.V12,
V20 = matrix1.V20 - matrix2.V20,
V21 = matrix1.V21 - matrix2.V21,
V22 = matrix1.V22 - matrix2.V22
};
}
public static Matrix3x3 Subtract(Matrix3x3 matrix1, Matrix3x3 matrix2)
{
return matrix1 - matrix2;
}
public static Vector3 operator *(Matrix3x3 matrix, Vector3 vector)
{
return new Vector3(matrix.V00 * vector.X + matrix.V01 * vector.Y + matrix.V02 * vector.Z, matrix.V10 * vector.X + matrix.V11 * vector.Y + matrix.V12 * vector.Z, matrix.V20 * vector.X + matrix.V21 * vector.Y + matrix.V22 * vector.Z);
}
public static Vector3 Multiply(Matrix3x3 matrix, Vector3 vector)
{
return matrix * vector;
}
public static Matrix3x3 operator *(Matrix3x3 matrix, float factor)
{
return new Matrix3x3
{
V00 = matrix.V00 * factor,
V01 = matrix.V01 * factor,
V02 = matrix.V02 * factor,
V10 = matrix.V10 * factor,
V11 = matrix.V11 * factor,
V12 = matrix.V12 * factor,
V20 = matrix.V20 * factor,
V21 = matrix.V21 * factor,
V22 = matrix.V22 * factor
};
}
public static Matrix3x3 Multiply(Matrix3x3 matrix, float factor)
{
return matrix * factor;
}
public static Matrix3x3 operator +(Matrix3x3 matrix, float value)
{
return new Matrix3x3
{
V00 = matrix.V00 + value,
V01 = matrix.V01 + value,
V02 = matrix.V02 + value,
V10 = matrix.V10 + value,
V11 = matrix.V11 + value,
V12 = matrix.V12 + value,
V20 = matrix.V20 + value,
V21 = matrix.V21 + value,
V22 = matrix.V22 + value
};
}
public static Matrix3x3 Add(Matrix3x3 matrix, float value)
{
return matrix + value;
}
public static bool operator ==(Matrix3x3 matrix1, Matrix3x3 matrix2)
{
if (matrix1.V00 == matrix2.V00 && matrix1.V01 == matrix2.V01 && matrix1.V02 == matrix2.V02 && matrix1.V10 == matrix2.V10 && matrix1.V11 == matrix2.V11 && matrix1.V12 == matrix2.V12 && matrix1.V20 == matrix2.V20 && matrix1.V21 == matrix2.V21)
{
return matrix1.V22 == matrix2.V22;
}
return false;
}
public static bool operator !=(Matrix3x3 matrix1, Matrix3x3 matrix2)
{
if (matrix1.V00 == matrix2.V00 && matrix1.V01 == matrix2.V01 && matrix1.V02 == matrix2.V02 && matrix1.V10 == matrix2.V10 && matrix1.V11 == matrix2.V11 && matrix1.V12 == matrix2.V12 && matrix1.V20 == matrix2.V20 && matrix1.V21 == matrix2.V21)
{
return matrix1.V22 != matrix2.V22;
}
return true;
}
public bool Equals(Matrix3x3 matrix)
{
return this == matrix;
}
public override bool Equals(object obj)
{
if (obj is Matrix3x3)
{
return Equals((Matrix3x3)obj);
}
return false;
}
public override int GetHashCode()
{
return V00.GetHashCode() + V01.GetHashCode() + V02.GetHashCode() + V10.GetHashCode() + V11.GetHashCode() + V12.GetHashCode() + V20.GetHashCode() + V21.GetHashCode() + V22.GetHashCode();
}
public Matrix3x3 Transpose()
{
return new Matrix3x3
{
V00 = V00,
V01 = V10,
V02 = V20,
V10 = V01,
V11 = V11,
V12 = V21,
V20 = V02,
V21 = V12,
V22 = V22
};
}
public Matrix3x3 MultiplySelfByTranspose()
{
Matrix3x3 result = default(Matrix3x3);
result.V00 = V00 * V00 + V01 * V01 + V02 * V02;
result.V10 = (result.V01 = V00 * V10 + V01 * V11 + V02 * V12);
result.V20 = (result.V02 = V00 * V20 + V01 * V21 + V02 * V22);
result.V11 = V10 * V10 + V11 * V11 + V12 * V12;
result.V21 = (result.V12 = V10 * V20 + V11 * V21 + V12 * V22);
result.V22 = V20 * V20 + V21 * V21 + V22 * V22;
return result;
}
public Matrix3x3 MultiplyTransposeBySelf()
{
Matrix3x3 result = default(Matrix3x3);
result.V00 = V00 * V00 + V10 * V10 + V20 * V20;
result.V10 = (result.V01 = V00 * V01 + V10 * V11 + V20 * V21);
result.V20 = (result.V02 = V00 * V02 + V10 * V12 + V20 * V22);
result.V11 = V01 * V01 + V11 * V11 + V21 * V21;
result.V21 = (result.V12 = V01 * V02 + V11 * V12 + V21 * V22);
result.V22 = V02 * V02 + V12 * V12 + V22 * V22;
return result;
}
public Matrix3x3 Adjugate()
{
return new Matrix3x3
{
V00 = V11 * V22 - V12 * V21,
V01 = 0f - (V01 * V22 - V02 * V21),
V02 = V01 * V12 - V02 * V11,
V10 = 0f - (V10 * V22 - V12 * V20),
V11 = V00 * V22 - V02 * V20,
V12 = 0f - (V00 * V12 - V02 * V10),
V20 = V10 * V21 - V11 * V20,
V21 = 0f - (V00 * V21 - V01 * V20),
V22 = V00 * V11 - V01 * V10
};
}
public Matrix3x3 Inverse()
{
float determinant = Determinant;
if (determinant == 0f)
{
throw new ArgumentException("Cannot calculate inverse of the matrix since it is singular.");
}
float num = 1f / determinant;
Matrix3x3 result = Adjugate();
result.V00 *= num;
result.V01 *= num;
result.V02 *= num;
result.V10 *= num;
result.V11 *= num;
result.V12 *= num;
result.V20 *= num;
result.V21 *= num;
result.V22 *= num;
return result;
}
public Matrix3x3 PseudoInverse()
{
SVD(out var u, out var e, out var v);
return v * CreateDiagonal(e.Inverse()) * u.Transpose();
}
public void SVD(out Matrix3x3 u, out Vector3 e, out Matrix3x3 v)
{
double[,] array = new double[3, 3]
{
{ V00, V01, V02 },
{ V10, V11, V12 },
{ V20, V21, V22 }
};
svd.svdcmp(array, out var w, out var v2);
u = default(Matrix3x3);
u.V00 = (float)array[0, 0];
u.V01 = (float)array[0, 1];
u.V02 = (float)array[0, 2];
u.V10 = (float)array[1, 0];
u.V11 = (float)array[1, 1];
u.V12 = (float)array[1, 2];
u.V20 = (float)array[2, 0];
u.V21 = (float)array[2, 1];
u.V22 = (float)array[2, 2];
v = default(Matrix3x3);
v.V00 = (float)v2[0, 0];
v.V01 = (float)v2[0, 1];
v.V02 = (float)v2[0, 2];
v.V10 = (float)v2[1, 0];
v.V11 = (float)v2[1, 1];
v.V12 = (float)v2[1, 2];
v.V20 = (float)v2[2, 0];
v.V21 = (float)v2[2, 1];
v.V22 = (float)v2[2, 2];
e = default(Vector3);
e.X = (float)w[0];
e.Y = (float)w[1];
e.Z = (float)w[2];
}
}

View File

@@ -0,0 +1,383 @@
using System;
namespace AForge.Math;
[Serializable]
public struct Matrix4x4
{
public float V00;
public float V01;
public float V02;
public float V03;
public float V10;
public float V11;
public float V12;
public float V13;
public float V20;
public float V21;
public float V22;
public float V23;
public float V30;
public float V31;
public float V32;
public float V33;
public static Matrix4x4 Identity
{
get
{
Matrix4x4 result = default(Matrix4x4);
result.V00 = (result.V11 = (result.V22 = (result.V33 = 1f)));
return result;
}
}
public float[] ToArray()
{
return new float[16]
{
V00, V01, V02, V03, V10, V11, V12, V13, V20, V21,
V22, V23, V30, V31, V32, V33
};
}
public static Matrix4x4 CreateRotationY(float radians)
{
Matrix4x4 identity = Identity;
float v = (float)System.Math.Cos(radians);
float num = (float)System.Math.Sin(radians);
identity.V00 = (identity.V22 = v);
identity.V02 = num;
identity.V20 = 0f - num;
return identity;
}
public static Matrix4x4 CreateRotationX(float radians)
{
Matrix4x4 identity = Identity;
float v = (float)System.Math.Cos(radians);
float num = (float)System.Math.Sin(radians);
identity.V11 = (identity.V22 = v);
identity.V12 = 0f - num;
identity.V21 = num;
return identity;
}
public static Matrix4x4 CreateRotationZ(float radians)
{
Matrix4x4 identity = Identity;
float v = (float)System.Math.Cos(radians);
float num = (float)System.Math.Sin(radians);
identity.V00 = (identity.V11 = v);
identity.V01 = 0f - num;
identity.V10 = num;
return identity;
}
public static Matrix4x4 CreateFromYawPitchRoll(float yaw, float pitch, float roll)
{
return CreateRotationY(yaw) * CreateRotationX(pitch) * CreateRotationZ(roll);
}
public void ExtractYawPitchRoll(out float yaw, out float pitch, out float roll)
{
yaw = (float)System.Math.Atan2(V02, V22);
pitch = (float)System.Math.Asin(0f - V12);
roll = (float)System.Math.Atan2(V10, V11);
}
public static Matrix4x4 CreateFromRotation(Matrix3x3 rotationMatrix)
{
Matrix4x4 identity = Identity;
identity.V00 = rotationMatrix.V00;
identity.V01 = rotationMatrix.V01;
identity.V02 = rotationMatrix.V02;
identity.V10 = rotationMatrix.V10;
identity.V11 = rotationMatrix.V11;
identity.V12 = rotationMatrix.V12;
identity.V20 = rotationMatrix.V20;
identity.V21 = rotationMatrix.V21;
identity.V22 = rotationMatrix.V22;
return identity;
}
public static Matrix4x4 CreateTranslation(Vector3 position)
{
Matrix4x4 identity = Identity;
identity.V03 = position.X;
identity.V13 = position.Y;
identity.V23 = position.Z;
return identity;
}
public static Matrix4x4 CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget)
{
Matrix4x4 result = default(Matrix4x4);
Vector3 vector = cameraPosition - cameraTarget;
vector.Normalize();
Vector3 vector2 = Vector3.Cross(new Vector3(0f, 1f, 0f), vector);
vector2.Normalize();
Vector3 vector3 = Vector3.Cross(vector, vector2);
result.V00 = vector2.X;
result.V01 = vector2.Y;
result.V02 = vector2.Z;
result.V10 = vector3.X;
result.V11 = vector3.Y;
result.V12 = vector3.Z;
result.V20 = vector.X;
result.V21 = vector.Y;
result.V22 = vector.Z;
result.V03 = 0f - Vector3.Dot(cameraPosition, vector2);
result.V13 = 0f - Vector3.Dot(cameraPosition, vector3);
result.V23 = 0f - Vector3.Dot(cameraPosition, vector);
result.V33 = 1f;
return result;
}
public static Matrix4x4 CreatePerspective(float width, float height, float nearPlaneDistance, float farPlaneDistance)
{
if (nearPlaneDistance <= 0f || farPlaneDistance <= 0f)
{
throw new ArgumentOutOfRangeException("Both near and far view planes' distances must be greater than zero.");
}
if (nearPlaneDistance >= farPlaneDistance)
{
throw new ArgumentException("Near plane must be closer than the far plane.");
}
return new Matrix4x4
{
V00 = 2f * nearPlaneDistance / width,
V11 = 2f * nearPlaneDistance / height,
V22 = farPlaneDistance / (nearPlaneDistance - farPlaneDistance),
V32 = -1f,
V23 = nearPlaneDistance * farPlaneDistance / (nearPlaneDistance - farPlaneDistance)
};
}
public static Matrix4x4 CreateFromRows(Vector4 row0, Vector4 row1, Vector4 row2, Vector4 row3)
{
return new Matrix4x4
{
V00 = row0.X,
V01 = row0.Y,
V02 = row0.Z,
V03 = row0.W,
V10 = row1.X,
V11 = row1.Y,
V12 = row1.Z,
V13 = row1.W,
V20 = row2.X,
V21 = row2.Y,
V22 = row2.Z,
V23 = row2.W,
V30 = row3.X,
V31 = row3.Y,
V32 = row3.Z,
V33 = row3.W
};
}
public static Matrix4x4 CreateFromColumns(Vector4 column0, Vector4 column1, Vector4 column2, Vector4 column3)
{
return new Matrix4x4
{
V00 = column0.X,
V10 = column0.Y,
V20 = column0.Z,
V30 = column0.W,
V01 = column1.X,
V11 = column1.Y,
V21 = column1.Z,
V31 = column1.W,
V02 = column2.X,
V12 = column2.Y,
V22 = column2.Z,
V32 = column2.W,
V03 = column3.X,
V13 = column3.Y,
V23 = column3.Z,
V33 = column3.W
};
}
public static Matrix4x4 CreateDiagonal(Vector4 vector)
{
return new Matrix4x4
{
V00 = vector.X,
V11 = vector.Y,
V22 = vector.Z,
V33 = vector.W
};
}
public Vector4 GetRow(int index)
{
return index switch
{
3 => new Vector4(V30, V31, V32, V33),
2 => new Vector4(V20, V21, V22, V23),
1 => new Vector4(V10, V11, V12, V13),
0 => new Vector4(V00, V01, V02, V03),
_ => throw new ArgumentException("Invalid row index was specified.", "index"),
};
}
public Vector4 GetColumn(int index)
{
return index switch
{
3 => new Vector4(V03, V13, V23, V33),
2 => new Vector4(V02, V12, V22, V32),
1 => new Vector4(V01, V11, V21, V31),
0 => new Vector4(V00, V10, V20, V30),
_ => throw new ArgumentException("Invalid column index was specified.", "index"),
};
}
public static Matrix4x4 operator *(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
return new Matrix4x4
{
V00 = matrix1.V00 * matrix2.V00 + matrix1.V01 * matrix2.V10 + matrix1.V02 * matrix2.V20 + matrix1.V03 * matrix2.V30,
V01 = matrix1.V00 * matrix2.V01 + matrix1.V01 * matrix2.V11 + matrix1.V02 * matrix2.V21 + matrix1.V03 * matrix2.V31,
V02 = matrix1.V00 * matrix2.V02 + matrix1.V01 * matrix2.V12 + matrix1.V02 * matrix2.V22 + matrix1.V03 * matrix2.V32,
V03 = matrix1.V00 * matrix2.V03 + matrix1.V01 * matrix2.V13 + matrix1.V02 * matrix2.V23 + matrix1.V03 * matrix2.V33,
V10 = matrix1.V10 * matrix2.V00 + matrix1.V11 * matrix2.V10 + matrix1.V12 * matrix2.V20 + matrix1.V13 * matrix2.V30,
V11 = matrix1.V10 * matrix2.V01 + matrix1.V11 * matrix2.V11 + matrix1.V12 * matrix2.V21 + matrix1.V13 * matrix2.V31,
V12 = matrix1.V10 * matrix2.V02 + matrix1.V11 * matrix2.V12 + matrix1.V12 * matrix2.V22 + matrix1.V13 * matrix2.V32,
V13 = matrix1.V10 * matrix2.V03 + matrix1.V11 * matrix2.V13 + matrix1.V12 * matrix2.V23 + matrix1.V13 * matrix2.V33,
V20 = matrix1.V20 * matrix2.V00 + matrix1.V21 * matrix2.V10 + matrix1.V22 * matrix2.V20 + matrix1.V23 * matrix2.V30,
V21 = matrix1.V20 * matrix2.V01 + matrix1.V21 * matrix2.V11 + matrix1.V22 * matrix2.V21 + matrix1.V23 * matrix2.V31,
V22 = matrix1.V20 * matrix2.V02 + matrix1.V21 * matrix2.V12 + matrix1.V22 * matrix2.V22 + matrix1.V23 * matrix2.V32,
V23 = matrix1.V20 * matrix2.V03 + matrix1.V21 * matrix2.V13 + matrix1.V22 * matrix2.V23 + matrix1.V23 * matrix2.V33,
V30 = matrix1.V30 * matrix2.V00 + matrix1.V31 * matrix2.V10 + matrix1.V32 * matrix2.V20 + matrix1.V33 * matrix2.V30,
V31 = matrix1.V30 * matrix2.V01 + matrix1.V31 * matrix2.V11 + matrix1.V32 * matrix2.V21 + matrix1.V33 * matrix2.V31,
V32 = matrix1.V30 * matrix2.V02 + matrix1.V31 * matrix2.V12 + matrix1.V32 * matrix2.V22 + matrix1.V33 * matrix2.V32,
V33 = matrix1.V30 * matrix2.V03 + matrix1.V31 * matrix2.V13 + matrix1.V32 * matrix2.V23 + matrix1.V33 * matrix2.V33
};
}
public static Matrix4x4 Multiply(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
return matrix1 * matrix2;
}
public static Matrix4x4 operator +(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
return new Matrix4x4
{
V00 = matrix1.V00 + matrix2.V00,
V01 = matrix1.V01 + matrix2.V01,
V02 = matrix1.V02 + matrix2.V02,
V03 = matrix1.V03 + matrix2.V03,
V10 = matrix1.V10 + matrix2.V10,
V11 = matrix1.V11 + matrix2.V11,
V12 = matrix1.V12 + matrix2.V12,
V13 = matrix1.V13 + matrix2.V13,
V20 = matrix1.V20 + matrix2.V20,
V21 = matrix1.V21 + matrix2.V21,
V22 = matrix1.V22 + matrix2.V22,
V23 = matrix1.V23 + matrix2.V23,
V30 = matrix1.V30 + matrix2.V30,
V31 = matrix1.V31 + matrix2.V31,
V32 = matrix1.V32 + matrix2.V32,
V33 = matrix1.V33 + matrix2.V33
};
}
public static Matrix4x4 Add(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
return matrix1 + matrix2;
}
public static Matrix4x4 operator -(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
return new Matrix4x4
{
V00 = matrix1.V00 - matrix2.V00,
V01 = matrix1.V01 - matrix2.V01,
V02 = matrix1.V02 - matrix2.V02,
V03 = matrix1.V03 - matrix2.V03,
V10 = matrix1.V10 - matrix2.V10,
V11 = matrix1.V11 - matrix2.V11,
V12 = matrix1.V12 - matrix2.V12,
V13 = matrix1.V13 - matrix2.V13,
V20 = matrix1.V20 - matrix2.V20,
V21 = matrix1.V21 - matrix2.V21,
V22 = matrix1.V22 - matrix2.V22,
V23 = matrix1.V23 - matrix2.V23,
V30 = matrix1.V30 - matrix2.V30,
V31 = matrix1.V31 - matrix2.V31,
V32 = matrix1.V32 - matrix2.V32,
V33 = matrix1.V33 - matrix2.V33
};
}
public static Matrix4x4 Subtract(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
return matrix1 - matrix2;
}
public static Vector4 operator *(Matrix4x4 matrix, Vector4 vector)
{
return new Vector4(matrix.V00 * vector.X + matrix.V01 * vector.Y + matrix.V02 * vector.Z + matrix.V03 * vector.W, matrix.V10 * vector.X + matrix.V11 * vector.Y + matrix.V12 * vector.Z + matrix.V13 * vector.W, matrix.V20 * vector.X + matrix.V21 * vector.Y + matrix.V22 * vector.Z + matrix.V23 * vector.W, matrix.V30 * vector.X + matrix.V31 * vector.Y + matrix.V32 * vector.Z + matrix.V33 * vector.W);
}
public static Vector4 Multiply(Matrix4x4 matrix, Vector4 vector)
{
return matrix * vector;
}
public static bool operator ==(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
if (matrix1.V00 == matrix2.V00 && matrix1.V01 == matrix2.V01 && matrix1.V02 == matrix2.V02 && matrix1.V03 == matrix2.V03 && matrix1.V10 == matrix2.V10 && matrix1.V11 == matrix2.V11 && matrix1.V12 == matrix2.V12 && matrix1.V13 == matrix2.V13 && matrix1.V20 == matrix2.V20 && matrix1.V21 == matrix2.V21 && matrix1.V22 == matrix2.V22 && matrix1.V23 == matrix2.V23 && matrix1.V30 == matrix2.V30 && matrix1.V31 == matrix2.V31 && matrix1.V32 == matrix2.V32)
{
return matrix1.V33 == matrix2.V33;
}
return false;
}
public static bool operator !=(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
if (matrix1.V00 == matrix2.V00 && matrix1.V01 == matrix2.V01 && matrix1.V02 == matrix2.V02 && matrix1.V03 == matrix2.V03 && matrix1.V10 == matrix2.V10 && matrix1.V11 == matrix2.V11 && matrix1.V12 == matrix2.V12 && matrix1.V13 == matrix2.V13 && matrix1.V20 == matrix2.V20 && matrix1.V21 == matrix2.V21 && matrix1.V22 == matrix2.V22 && matrix1.V23 == matrix2.V23 && matrix1.V30 == matrix2.V30 && matrix1.V31 == matrix2.V31 && matrix1.V32 == matrix2.V32)
{
return matrix1.V33 != matrix2.V33;
}
return true;
}
public bool Equals(Matrix4x4 matrix)
{
return this == matrix;
}
public override bool Equals(object obj)
{
if (obj is Matrix4x4)
{
return Equals((Matrix4x4)obj);
}
return false;
}
public override int GetHashCode()
{
return V00.GetHashCode() + V01.GetHashCode() + V02.GetHashCode() + V03.GetHashCode() + V10.GetHashCode() + V11.GetHashCode() + V12.GetHashCode() + V13.GetHashCode() + V20.GetHashCode() + V21.GetHashCode() + V22.GetHashCode() + V23.GetHashCode() + V30.GetHashCode() + V31.GetHashCode() + V32.GetHashCode() + V33.GetHashCode();
}
}

View File

@@ -0,0 +1,10 @@
namespace AForge.Math.Metrics;
public sealed class CosineDistance : IDistance
{
public double GetDistance(double[] p, double[] q)
{
CosineSimilarity cosineSimilarity = new CosineSimilarity();
return 1.0 - cosineSimilarity.GetSimilarityScore(p, q);
}
}

View File

@@ -0,0 +1,33 @@
using System;
namespace AForge.Math.Metrics;
public sealed class CosineSimilarity : ISimilarity
{
public double GetSimilarityScore(double[] p, double[] q)
{
double num = 0.0;
double num2 = 0.0;
double num3 = 0.0;
double num4 = 0.0;
if (p.Length != q.Length)
{
throw new ArgumentException("Input vectors must be of the same dimension.");
}
int i = 0;
for (int num5 = p.Length; i < num5; i++)
{
double num6 = p[i];
double num7 = q[i];
num2 += num6 * num7;
num3 += num6 * num6;
num4 += num7 * num7;
}
num = System.Math.Sqrt(num3) * System.Math.Sqrt(num4);
if (num != 0.0)
{
return num2 / num;
}
return 0.0;
}
}

View File

@@ -0,0 +1,23 @@
using System;
namespace AForge.Math.Metrics;
public sealed class EuclideanDistance : IDistance
{
public double GetDistance(double[] p, double[] q)
{
double num = 0.0;
double num2 = 0.0;
if (p.Length != q.Length)
{
throw new ArgumentException("Input vectors must be of the same dimension.");
}
int i = 0;
for (int num3 = p.Length; i < num3; i++)
{
num2 = p[i] - q[i];
num += num2 * num2;
}
return System.Math.Sqrt(num);
}
}

View File

@@ -0,0 +1,11 @@
namespace AForge.Math.Metrics;
public sealed class EuclideanSimilarity : ISimilarity
{
public double GetSimilarityScore(double[] p, double[] q)
{
double num = 0.0;
EuclideanDistance euclideanDistance = new EuclideanDistance();
return 1.0 / (1.0 + euclideanDistance.GetDistance(p, q));
}
}

View File

@@ -0,0 +1,24 @@
using System;
namespace AForge.Math.Metrics;
public sealed class HammingDistance : IDistance
{
public double GetDistance(double[] p, double[] q)
{
double num = 0.0;
if (p.Length != q.Length)
{
throw new ArgumentException("Input vectors must be of the same dimension.");
}
int i = 0;
for (int num2 = p.Length; i < num2; i++)
{
if (p[i] != q[i])
{
num += 1.0;
}
}
return num;
}
}

View File

@@ -0,0 +1,6 @@
namespace AForge.Math.Metrics;
public interface IDistance
{
double GetDistance(double[] p, double[] q);
}

View File

@@ -0,0 +1,6 @@
namespace AForge.Math.Metrics;
public interface ISimilarity
{
double GetSimilarityScore(double[] p, double[] q);
}

View File

@@ -0,0 +1,34 @@
using System;
namespace AForge.Math.Metrics;
public sealed class JaccardDistance : IDistance
{
public double GetDistance(double[] p, double[] q)
{
double num = 0.0;
int num2 = 0;
int num3 = 0;
if (p.Length != q.Length)
{
throw new ArgumentException("Input vectors must be of the same dimension.");
}
int i = 0;
for (int num4 = p.Length; i < num4; i++)
{
if (p[i] != 0.0 || q[i] != 0.0)
{
if (p[i] == q[i])
{
num2++;
}
num3++;
}
}
if (num3 != 0)
{
return 1.0 - (double)num2 / (double)num3;
}
return 0.0;
}
}

View File

@@ -0,0 +1,21 @@
using System;
namespace AForge.Math.Metrics;
public sealed class ManhattanDistance : IDistance
{
public double GetDistance(double[] p, double[] q)
{
double num = 0.0;
if (p.Length != q.Length)
{
throw new ArgumentException("Input vectors must be of the same dimension.");
}
int i = 0;
for (int num2 = p.Length; i < num2; i++)
{
num += System.Math.Abs(p[i] - q[i]);
}
return num;
}
}

View File

@@ -0,0 +1,37 @@
using System;
namespace AForge.Math.Metrics;
public sealed class PearsonCorrelation : ISimilarity
{
public double GetSimilarityScore(double[] p, double[] q)
{
double num = 0.0;
double num2 = 0.0;
double num3 = 0.0;
double num4 = 0.0;
double num5 = 0.0;
int num6 = p.Length;
if (num6 != q.Length)
{
throw new ArgumentException("Input vectors must be of the same dimension.");
}
for (int i = 0; i < num6; i++)
{
double num7 = p[i];
double num8 = q[i];
num += num7;
num2 += num8;
num3 += num7 * num7;
num4 += num8 * num8;
num5 += num7 * num8;
}
double num9 = num5 - num * num2 / (double)num6;
double num10 = System.Math.Sqrt((num3 - num * num / (double)num6) * (num4 - num2 * num2 / (double)num6));
if (num10 != 0.0)
{
return num9 / num10;
}
return 0.0;
}
}

View File

@@ -0,0 +1,149 @@
using System;
namespace AForge.Math;
public class PerlinNoise
{
private double initFrequency = 1.0;
private double initAmplitude = 1.0;
private double persistence = 0.65;
private int octaves = 4;
public double InitFrequency
{
get
{
return initFrequency;
}
set
{
initFrequency = value;
}
}
public double InitAmplitude
{
get
{
return initAmplitude;
}
set
{
initAmplitude = value;
}
}
public double Persistence
{
get
{
return persistence;
}
set
{
persistence = value;
}
}
public int Octaves
{
get
{
return octaves;
}
set
{
octaves = System.Math.Max(1, System.Math.Min(32, value));
}
}
public PerlinNoise()
{
}
public PerlinNoise(int octaves, double persistence)
{
this.octaves = octaves;
this.persistence = persistence;
}
public PerlinNoise(int octaves, double persistence, double initFrequency, double initAmplitude)
{
this.octaves = octaves;
this.persistence = persistence;
this.initFrequency = initFrequency;
this.initAmplitude = initAmplitude;
}
public double Function(double x)
{
double num = initFrequency;
double num2 = initAmplitude;
double num3 = 0.0;
for (int i = 0; i < octaves; i++)
{
num3 += SmoothedNoise(x * num) * num2;
num *= 2.0;
num2 *= persistence;
}
return num3;
}
public double Function2D(double x, double y)
{
double num = initFrequency;
double num2 = initAmplitude;
double num3 = 0.0;
for (int i = 0; i < octaves; i++)
{
num3 += SmoothedNoise(x * num, y * num) * num2;
num *= 2.0;
num2 *= persistence;
}
return num3;
}
private double Noise(int x)
{
int num = (x << 13) ^ x;
return 1.0 - (double)((num * (num * num * 15731 + 789221) + 1376312589) & 0x7FFFFFFF) / 1073741824.0;
}
private double Noise(int x, int y)
{
int num = x + y * 57;
num = (num << 13) ^ num;
return 1.0 - (double)((num * (num * num * 15731 + 789221) + 1376312589) & 0x7FFFFFFF) / 1073741824.0;
}
private double SmoothedNoise(double x)
{
int num = (int)x;
double a = x - (double)num;
return CosineInterpolate(Noise(num), Noise(num + 1), a);
}
private double SmoothedNoise(double x, double y)
{
int num = (int)x;
int num2 = (int)y;
double a = x - (double)num;
double a2 = y - (double)num2;
double x2 = Noise(num, num2);
double x3 = Noise(num + 1, num2);
double x4 = Noise(num, num2 + 1);
double x5 = Noise(num + 1, num2 + 1);
double x6 = CosineInterpolate(x2, x3, a);
double x7 = CosineInterpolate(x4, x5, a);
return CosineInterpolate(x6, x7, a2);
}
private double CosineInterpolate(double x1, double x2, double a)
{
double num = (1.0 - System.Math.Cos(a * System.Math.PI)) * 0.5;
return x1 * (1.0 - num) + x2 * num;
}
}

View File

@@ -0,0 +1,41 @@
using System;
namespace AForge.Math.Random;
public class ExponentialGenerator : IRandomNumberGenerator
{
private UniformOneGenerator rand;
private float rate;
public float Rate => rate;
public float Mean => 1f / rate;
public float Variance => 1f / (rate * rate);
public ExponentialGenerator(float rate)
: this(rate, 0)
{
}
public ExponentialGenerator(float rate, int seed)
{
if (rate <= 0f)
{
throw new ArgumentException("Rate value should be greater than zero.");
}
rand = new UniformOneGenerator(seed);
this.rate = rate;
}
public float Next()
{
return (0f - (float)System.Math.Log(rand.Next())) / rate;
}
public void SetSeed(int seed)
{
rand = new UniformOneGenerator(seed);
}
}

View File

@@ -0,0 +1,38 @@
namespace AForge.Math.Random;
public class GaussianGenerator : IRandomNumberGenerator
{
private StandardGenerator rand;
private float mean;
private float stdDev;
public float Mean => mean;
public float Variance => stdDev * stdDev;
public float StdDev => stdDev;
public GaussianGenerator(float mean, float stdDev)
: this(mean, stdDev, 0)
{
}
public GaussianGenerator(float mean, float stdDev, int seed)
{
this.mean = mean;
this.stdDev = stdDev;
rand = new StandardGenerator(seed);
}
public float Next()
{
return rand.Next() * stdDev + mean;
}
public void SetSeed(int seed)
{
rand = new StandardGenerator(seed);
}
}

View File

@@ -0,0 +1,12 @@
namespace AForge.Math.Random;
public interface IRandomNumberGenerator
{
float Mean { get; }
float Variance { get; }
float Next();
void SetSeed(int seed);
}

View File

@@ -0,0 +1,56 @@
using System;
namespace AForge.Math.Random;
public class StandardGenerator : IRandomNumberGenerator
{
private UniformOneGenerator rand;
private float secondValue;
private bool useSecond;
public float Mean => 0f;
public float Variance => 1f;
public StandardGenerator()
{
rand = new UniformOneGenerator();
}
public StandardGenerator(int seed)
{
rand = new UniformOneGenerator(seed);
}
public float Next()
{
if (useSecond)
{
useSecond = false;
return secondValue;
}
float num;
float num2;
float num3;
do
{
num = rand.Next() * 2f - 1f;
num2 = rand.Next() * 2f - 1f;
num3 = num * num + num2 * num2;
}
while (num3 >= 1f);
num3 = (float)System.Math.Sqrt(-2.0 * System.Math.Log(num3) / (double)num3);
float result = num * num3;
secondValue = num2 * num3;
useSecond = true;
return result;
}
public void SetSeed(int seed)
{
rand = new UniformOneGenerator(seed);
useSecond = false;
}
}

View File

@@ -0,0 +1,38 @@
namespace AForge.Math.Random;
public class UniformGenerator : IRandomNumberGenerator
{
private UniformOneGenerator rand;
private float min;
private float length;
public float Mean => (min + min + length) / 2f;
public float Variance => length * length / 12f;
public Range Range => new Range(min, min + length);
public UniformGenerator(Range range)
: this(range, 0)
{
}
public UniformGenerator(Range range, int seed)
{
rand = new UniformOneGenerator(seed);
min = range.Min;
length = range.Length;
}
public float Next()
{
return rand.Next() * length + min;
}
public void SetSeed(int seed)
{
rand = new UniformOneGenerator(seed);
}
}

View File

@@ -0,0 +1,30 @@
namespace AForge.Math.Random;
public class UniformOneGenerator : IRandomNumberGenerator
{
private ThreadSafeRandom rand;
public float Mean => 0.5f;
public float Variance => 1f / 12f;
public UniformOneGenerator()
{
rand = new ThreadSafeRandom(0);
}
public UniformOneGenerator(int seed)
{
rand = new ThreadSafeRandom(seed);
}
public float Next()
{
return (float)rand.NextDouble();
}
public void SetSeed(int seed)
{
rand = new ThreadSafeRandom(seed);
}
}

View File

@@ -0,0 +1,142 @@
using System;
namespace AForge.Math;
public static class Statistics
{
public static double Mean(int[] values)
{
long num = 0L;
double num2 = 0.0;
int i = 0;
for (int num3 = values.Length; i < num3; i++)
{
int num4 = values[i];
num2 += (double)(i * num4);
num += num4;
}
if (num != 0)
{
return num2 / (double)num;
}
return 0.0;
}
public static double StdDev(int[] values)
{
return StdDev(values, Mean(values));
}
public static double StdDev(int[] values, double mean)
{
double num = 0.0;
int num2 = 0;
int i = 0;
for (int num3 = values.Length; i < num3; i++)
{
int num4 = values[i];
double num5 = (double)i - mean;
num += num5 * num5 * (double)num4;
num2 += num4;
}
if (num2 != 0)
{
return System.Math.Sqrt(num / (double)num2);
}
return 0.0;
}
public static int Median(int[] values)
{
int num = 0;
int num2 = values.Length;
for (int i = 0; i < num2; i++)
{
num += values[i];
}
int num3 = num / 2;
int j = 0;
int num4 = 0;
for (; j < num2; j++)
{
num4 += values[j];
if (num4 >= num3)
{
break;
}
}
return j;
}
public static IntRange GetRange(int[] values, double percent)
{
int num = 0;
int num2 = values.Length;
for (int i = 0; i < num2; i++)
{
num += values[i];
}
int num3 = (int)((double)num * (percent + (1.0 - percent) / 2.0));
int j = 0;
int num4 = num;
for (; j < num2; j++)
{
num4 -= values[j];
if (num4 < num3)
{
break;
}
}
int num5 = num2 - 1;
num4 = num;
while (num5 >= 0)
{
num4 -= values[num5];
if (num4 < num3)
{
break;
}
num5--;
}
return new IntRange(j, num5);
}
public static double Entropy(int[] values)
{
int num = values.Length;
int num2 = 0;
double num3 = 0.0;
for (int i = 0; i < num; i++)
{
num2 += values[i];
}
if (num2 != 0)
{
for (int j = 0; j < num; j++)
{
double num4 = (double)values[j] / (double)num2;
if (num4 != 0.0)
{
num3 += (0.0 - num4) * System.Math.Log(num4, 2.0);
}
}
}
return num3;
}
public static int Mode(int[] values)
{
int result = 0;
int num = 0;
int i = 0;
for (int num2 = values.Length; i < num2; i++)
{
if (values[i] > num)
{
num = values[i];
result = i;
}
}
return result;
}
}

View File

@@ -0,0 +1,151 @@
namespace AForge.Math;
public static class Tools
{
public static int Pow2(int power)
{
if (power < 0 || power > 30)
{
return 0;
}
return 1 << power;
}
public static bool IsPowerOf2(int x)
{
if (x <= 0)
{
return false;
}
return (x & (x - 1)) == 0;
}
public static int Log2(int x)
{
if (x <= 65536)
{
if (x <= 256)
{
if (x <= 16)
{
if (x <= 4)
{
if (x <= 2)
{
if (x <= 1)
{
return 0;
}
return 1;
}
return 2;
}
if (x <= 8)
{
return 3;
}
return 4;
}
if (x <= 64)
{
if (x <= 32)
{
return 5;
}
return 6;
}
if (x <= 128)
{
return 7;
}
return 8;
}
if (x <= 4096)
{
if (x <= 1024)
{
if (x <= 512)
{
return 9;
}
return 10;
}
if (x <= 2048)
{
return 11;
}
return 12;
}
if (x <= 16384)
{
if (x <= 8192)
{
return 13;
}
return 14;
}
if (x <= 32768)
{
return 15;
}
return 16;
}
if (x <= 16777216)
{
if (x <= 1048576)
{
if (x <= 262144)
{
if (x <= 131072)
{
return 17;
}
return 18;
}
if (x <= 524288)
{
return 19;
}
return 20;
}
if (x <= 4194304)
{
if (x <= 2097152)
{
return 21;
}
return 22;
}
if (x <= 8388608)
{
return 23;
}
return 24;
}
if (x <= 268435456)
{
if (x <= 67108864)
{
if (x <= 33554432)
{
return 25;
}
return 26;
}
if (x <= 134217728)
{
return 27;
}
return 28;
}
if (x <= 1073741824)
{
if (x <= 536870912)
{
return 29;
}
return 30;
}
return 31;
}
}

View File

@@ -0,0 +1,276 @@
using System;
using System.Globalization;
namespace AForge.Math;
[Serializable]
public struct Vector3
{
public float X;
public float Y;
public float Z;
public float Max
{
get
{
if (!(X > Y))
{
if (!(Y > Z))
{
return Z;
}
return Y;
}
if (!(X > Z))
{
return Z;
}
return X;
}
}
public float Min
{
get
{
if (!(X < Y))
{
if (!(Y < Z))
{
return Z;
}
return Y;
}
if (!(X < Z))
{
return Z;
}
return X;
}
}
public int MaxIndex
{
get
{
if (!(X >= Y))
{
if (!(Y >= Z))
{
return 2;
}
return 1;
}
if (!(X >= Z))
{
return 2;
}
return 0;
}
}
public int MinIndex
{
get
{
if (!(X <= Y))
{
if (!(Y <= Z))
{
return 2;
}
return 1;
}
if (!(X <= Z))
{
return 2;
}
return 0;
}
}
public float Norm => (float)System.Math.Sqrt(X * X + Y * Y + Z * Z);
public float Square => X * X + Y * Y + Z * Z;
public Vector3(float x, float y, float z)
{
X = x;
Y = y;
Z = z;
}
public Vector3(float value)
{
X = (Y = (Z = value));
}
public override string ToString()
{
return string.Format(CultureInfo.InvariantCulture, "{0}, {1}, {2}", new object[3] { X, Y, Z });
}
public float[] ToArray()
{
return new float[3] { X, Y, Z };
}
public static Vector3 operator +(Vector3 vector1, Vector3 vector2)
{
return new Vector3(vector1.X + vector2.X, vector1.Y + vector2.Y, vector1.Z + vector2.Z);
}
public static Vector3 Add(Vector3 vector1, Vector3 vector2)
{
return vector1 + vector2;
}
public static Vector3 operator +(Vector3 vector, float value)
{
return new Vector3(vector.X + value, vector.Y + value, vector.Z + value);
}
public static Vector3 Add(Vector3 vector, float value)
{
return vector + value;
}
public static Vector3 operator -(Vector3 vector1, Vector3 vector2)
{
return new Vector3(vector1.X - vector2.X, vector1.Y - vector2.Y, vector1.Z - vector2.Z);
}
public static Vector3 Subtract(Vector3 vector1, Vector3 vector2)
{
return vector1 - vector2;
}
public static Vector3 operator -(Vector3 vector, float value)
{
return new Vector3(vector.X - value, vector.Y - value, vector.Z - value);
}
public static Vector3 Subtract(Vector3 vector, float value)
{
return vector - value;
}
public static Vector3 operator *(Vector3 vector1, Vector3 vector2)
{
return new Vector3(vector1.X * vector2.X, vector1.Y * vector2.Y, vector1.Z * vector2.Z);
}
public static Vector3 Multiply(Vector3 vector1, Vector3 vector2)
{
return vector1 * vector2;
}
public static Vector3 operator *(Vector3 vector, float factor)
{
return new Vector3(vector.X * factor, vector.Y * factor, vector.Z * factor);
}
public static Vector3 Multiply(Vector3 vector, float factor)
{
return vector * factor;
}
public static Vector3 operator /(Vector3 vector1, Vector3 vector2)
{
return new Vector3(vector1.X / vector2.X, vector1.Y / vector2.Y, vector1.Z / vector2.Z);
}
public static Vector3 Divide(Vector3 vector1, Vector3 vector2)
{
return vector1 / vector2;
}
public static Vector3 operator /(Vector3 vector, float factor)
{
return new Vector3(vector.X / factor, vector.Y / factor, vector.Z / factor);
}
public static Vector3 Divide(Vector3 vector, float factor)
{
return vector / factor;
}
public static bool operator ==(Vector3 vector1, Vector3 vector2)
{
if (vector1.X == vector2.X && vector1.Y == vector2.Y)
{
return vector1.Z == vector2.Z;
}
return false;
}
public static bool operator !=(Vector3 vector1, Vector3 vector2)
{
if (vector1.X == vector2.X && vector1.Y == vector2.Y)
{
return vector1.Z != vector2.Z;
}
return true;
}
public bool Equals(Vector3 vector)
{
if (vector.X == X && vector.Y == Y)
{
return vector.Z == Z;
}
return false;
}
public override bool Equals(object obj)
{
if (obj is Vector3)
{
return Equals((Vector3)obj);
}
return false;
}
public override int GetHashCode()
{
return X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode();
}
public float Normalize()
{
float num = (float)System.Math.Sqrt(X * X + Y * Y + Z * Z);
float num2 = 1f / num;
X *= num2;
Y *= num2;
Z *= num2;
return num;
}
public Vector3 Inverse()
{
return new Vector3((X == 0f) ? 0f : (1f / X), (Y == 0f) ? 0f : (1f / Y), (Z == 0f) ? 0f : (1f / Z));
}
public Vector3 Abs()
{
return new Vector3(System.Math.Abs(X), System.Math.Abs(Y), System.Math.Abs(Z));
}
public static Vector3 Cross(Vector3 vector1, Vector3 vector2)
{
return new Vector3(vector1.Y * vector2.Z - vector1.Z * vector2.Y, vector1.Z * vector2.X - vector1.X * vector2.Z, vector1.X * vector2.Y - vector1.Y * vector2.X);
}
public static float Dot(Vector3 vector1, Vector3 vector2)
{
return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
}
public Vector4 ToVector4()
{
return new Vector4(X, Y, Z, 1f);
}
}

View File

@@ -0,0 +1,295 @@
using System;
using System.Globalization;
namespace AForge.Math;
[Serializable]
public struct Vector4
{
public float X;
public float Y;
public float Z;
public float W;
public float Max
{
get
{
float num = ((X > Y) ? X : Y);
float num2 = ((Z > W) ? Z : W);
if (!(num > num2))
{
return num2;
}
return num;
}
}
public float Min
{
get
{
float num = ((X < Y) ? X : Y);
float num2 = ((Z < W) ? Z : W);
if (!(num < num2))
{
return num2;
}
return num;
}
}
public int MaxIndex
{
get
{
float num = 0f;
float num2 = 0f;
int num3 = 0;
int num4 = 0;
if (X >= Y)
{
num = X;
num3 = 0;
}
else
{
num = Y;
num3 = 1;
}
if (Z >= W)
{
num2 = Z;
num4 = 2;
}
else
{
num2 = W;
num4 = 3;
}
if (!(num >= num2))
{
return num4;
}
return num3;
}
}
public int MinIndex
{
get
{
float num = 0f;
float num2 = 0f;
int num3 = 0;
int num4 = 0;
if (X <= Y)
{
num = X;
num3 = 0;
}
else
{
num = Y;
num3 = 1;
}
if (Z <= W)
{
num2 = Z;
num4 = 2;
}
else
{
num2 = W;
num4 = 3;
}
if (!(num <= num2))
{
return num4;
}
return num3;
}
}
public float Norm => (float)System.Math.Sqrt(X * X + Y * Y + Z * Z + W * W);
public float Square => X * X + Y * Y + Z * Z + W * W;
public Vector4(float x, float y, float z, float w)
{
X = x;
Y = y;
Z = z;
W = w;
}
public Vector4(float value)
{
X = (Y = (Z = (W = value)));
}
public override string ToString()
{
return string.Format(CultureInfo.InvariantCulture, "{0}, {1}, {2}, {3}", X, Y, Z, W);
}
public float[] ToArray()
{
return new float[4] { X, Y, Z, W };
}
public static Vector4 operator +(Vector4 vector1, Vector4 vector2)
{
return new Vector4(vector1.X + vector2.X, vector1.Y + vector2.Y, vector1.Z + vector2.Z, vector1.W + vector2.W);
}
public static Vector4 Add(Vector4 vector1, Vector4 vector2)
{
return vector1 + vector2;
}
public static Vector4 operator +(Vector4 vector, float value)
{
return new Vector4(vector.X + value, vector.Y + value, vector.Z + value, vector.W + value);
}
public static Vector4 Add(Vector4 vector, float value)
{
return vector + value;
}
public static Vector4 operator -(Vector4 vector1, Vector4 vector2)
{
return new Vector4(vector1.X - vector2.X, vector1.Y - vector2.Y, vector1.Z - vector2.Z, vector1.W - vector2.W);
}
public static Vector4 Subtract(Vector4 vector1, Vector4 vector2)
{
return vector1 - vector2;
}
public static Vector4 operator -(Vector4 vector, float value)
{
return new Vector4(vector.X - value, vector.Y - value, vector.Z - value, vector.W - value);
}
public static Vector4 Subtract(Vector4 vector, float value)
{
return vector - value;
}
public static Vector4 operator *(Vector4 vector1, Vector4 vector2)
{
return new Vector4(vector1.X * vector2.X, vector1.Y * vector2.Y, vector1.Z * vector2.Z, vector1.W * vector2.W);
}
public static Vector4 Multiply(Vector4 vector1, Vector4 vector2)
{
return vector1 * vector2;
}
public static Vector4 operator *(Vector4 vector, float factor)
{
return new Vector4(vector.X * factor, vector.Y * factor, vector.Z * factor, vector.W * factor);
}
public static Vector4 Multiply(Vector4 vector, float factor)
{
return vector * factor;
}
public static Vector4 operator /(Vector4 vector1, Vector4 vector2)
{
return new Vector4(vector1.X / vector2.X, vector1.Y / vector2.Y, vector1.Z / vector2.Z, vector1.W / vector2.W);
}
public static Vector4 Divide(Vector4 vector1, Vector4 vector2)
{
return vector1 / vector2;
}
public static Vector4 operator /(Vector4 vector, float factor)
{
return new Vector4(vector.X / factor, vector.Y / factor, vector.Z / factor, vector.W / factor);
}
public static Vector4 Divide(Vector4 vector, float factor)
{
return vector / factor;
}
public static bool operator ==(Vector4 vector1, Vector4 vector2)
{
if (vector1.X == vector2.X && vector1.Y == vector2.Y && vector1.Z == vector2.Z)
{
return vector1.W == vector2.W;
}
return false;
}
public static bool operator !=(Vector4 vector1, Vector4 vector2)
{
if (vector1.X == vector2.X && vector1.Y == vector2.Y && vector1.Z == vector2.Z)
{
return vector1.W != vector2.W;
}
return true;
}
public bool Equals(Vector4 vector)
{
if (vector.X == X && vector.Y == Y && vector.Z == Z)
{
return vector.W == W;
}
return false;
}
public override bool Equals(object obj)
{
if (obj is Vector4)
{
return Equals((Vector4)obj);
}
return false;
}
public override int GetHashCode()
{
return X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode();
}
public float Normalize()
{
float num = (float)System.Math.Sqrt(X * X + Y * Y + Z * Z + W * W);
float num2 = 1f / num;
X *= num2;
Y *= num2;
Z *= num2;
W *= num2;
return num;
}
public Vector4 Inverse()
{
return new Vector4((X == 0f) ? 0f : (1f / X), (Y == 0f) ? 0f : (1f / Y), (Z == 0f) ? 0f : (1f / Z), (W == 0f) ? 0f : (1f / W));
}
public Vector4 Abs()
{
return new Vector4(System.Math.Abs(X), System.Math.Abs(Y), System.Math.Abs(Z), System.Math.Abs(W));
}
public static float Dot(Vector4 vector1, Vector4 vector2)
{
return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W;
}
public Vector3 ToVector3()
{
return new Vector3(X / W, Y / W, Z / W);
}
}

View File

@@ -0,0 +1,336 @@
using System;
namespace AForge.Math;
internal class svd
{
public static void svdcmp(double[,] a, out double[] w, out double[,] v)
{
int length = a.GetLength(0);
int length2 = a.GetLength(1);
if (length < length2)
{
throw new ArgumentException("Number of rows in A must be greater or equal to number of columns");
}
w = new double[length2];
v = new double[length2, length2];
int num = 0;
int num2 = 0;
double[] array = new double[length2];
double num5;
double num4;
double num3 = (num4 = (num5 = 0.0));
for (int i = 0; i < length2; i++)
{
num = i + 1;
array[i] = num4 * num3;
double num6;
num3 = (num6 = (num4 = 0.0));
if (i < length)
{
for (int j = i; j < length; j++)
{
num4 += System.Math.Abs(a[j, i]);
}
if (num4 != 0.0)
{
for (int j = i; j < length; j++)
{
a[j, i] /= num4;
num6 += a[j, i] * a[j, i];
}
double num7 = a[i, i];
num3 = 0.0 - Sign(System.Math.Sqrt(num6), num7);
double num8 = num7 * num3 - num6;
a[i, i] = num7 - num3;
if (i != length2 - 1)
{
for (int k = num; k < length2; k++)
{
num6 = 0.0;
for (int j = i; j < length; j++)
{
num6 += a[j, i] * a[j, k];
}
num7 = num6 / num8;
for (int j = i; j < length; j++)
{
a[j, k] += num7 * a[j, i];
}
}
}
for (int j = i; j < length; j++)
{
a[j, i] *= num4;
}
}
}
w[i] = num4 * num3;
num3 = (num6 = (num4 = 0.0));
if (i < length && i != length2 - 1)
{
for (int j = num; j < length2; j++)
{
num4 += System.Math.Abs(a[i, j]);
}
if (num4 != 0.0)
{
for (int j = num; j < length2; j++)
{
a[i, j] /= num4;
num6 += a[i, j] * a[i, j];
}
double num7 = a[i, num];
num3 = 0.0 - Sign(System.Math.Sqrt(num6), num7);
double num8 = num7 * num3 - num6;
a[i, num] = num7 - num3;
for (int j = num; j < length2; j++)
{
array[j] = a[i, j] / num8;
}
if (i != length - 1)
{
for (int k = num; k < length; k++)
{
num6 = 0.0;
for (int j = num; j < length2; j++)
{
num6 += a[k, j] * a[i, j];
}
for (int j = num; j < length2; j++)
{
a[k, j] += num6 * array[j];
}
}
}
for (int j = num; j < length2; j++)
{
a[i, j] *= num4;
}
}
}
num5 = System.Math.Max(num5, System.Math.Abs(w[i]) + System.Math.Abs(array[i]));
}
for (int i = length2 - 1; i >= 0; i--)
{
if (i < length2 - 1)
{
if (num3 != 0.0)
{
for (int k = num; k < length2; k++)
{
v[k, i] = a[i, k] / a[i, num] / num3;
}
for (int k = num; k < length2; k++)
{
double num6 = 0.0;
for (int j = num; j < length2; j++)
{
num6 += a[i, j] * v[j, k];
}
for (int j = num; j < length2; j++)
{
v[j, k] += num6 * v[j, i];
}
}
}
for (int k = num; k < length2; k++)
{
v[i, k] = (v[k, i] = 0.0);
}
}
v[i, i] = 1.0;
num3 = array[i];
num = i;
}
for (int i = length2 - 1; i >= 0; i--)
{
num = i + 1;
num3 = w[i];
if (i < length2 - 1)
{
for (int k = num; k < length2; k++)
{
a[i, k] = 0.0;
}
}
if (num3 != 0.0)
{
num3 = 1.0 / num3;
if (i != length2 - 1)
{
for (int k = num; k < length2; k++)
{
double num6 = 0.0;
for (int j = num; j < length; j++)
{
num6 += a[j, i] * a[j, k];
}
double num7 = num6 / a[i, i] * num3;
for (int j = i; j < length; j++)
{
a[j, k] += num7 * a[j, i];
}
}
}
for (int k = i; k < length; k++)
{
a[k, i] *= num3;
}
}
else
{
for (int k = i; k < length; k++)
{
a[k, i] = 0.0;
}
}
a[i, i] += 1.0;
}
for (int j = length2 - 1; j >= 0; j--)
{
for (int l = 1; l <= 30; l++)
{
int num9 = 1;
for (num = j; num >= 0; num--)
{
num2 = num - 1;
if (System.Math.Abs(array[num]) + num5 == num5)
{
num9 = 0;
break;
}
if (System.Math.Abs(w[num2]) + num5 == num5)
{
break;
}
}
double num10;
double num6;
double num7;
double num8;
double num11;
double num12;
if (num9 != 0)
{
num10 = 0.0;
num6 = 1.0;
for (int i = num; i <= j; i++)
{
num7 = num6 * array[i];
if (System.Math.Abs(num7) + num5 != num5)
{
num3 = w[i];
num8 = Pythag(num7, num3);
w[i] = num8;
num8 = 1.0 / num8;
num10 = num3 * num8;
num6 = (0.0 - num7) * num8;
for (int k = 1; k <= length; k++)
{
num11 = a[k, num2];
num12 = a[k, i];
a[k, num2] = num11 * num10 + num12 * num6;
a[k, i] = num12 * num10 - num11 * num6;
}
}
}
}
num12 = w[j];
if (num == j)
{
if (num12 < 0.0)
{
w[j] = 0.0 - num12;
for (int k = 0; k < length2; k++)
{
v[k, j] = 0.0 - v[k, j];
}
}
break;
}
if (l == 30)
{
throw new ApplicationException("No convergence in 30 svdcmp iterations");
}
double num13 = w[num];
num2 = j - 1;
num11 = w[num2];
num3 = array[num2];
num8 = array[j];
num7 = ((num11 - num12) * (num11 + num12) + (num3 - num8) * (num3 + num8)) / (2.0 * num8 * num11);
num3 = Pythag(num7, 1.0);
num7 = ((num13 - num12) * (num13 + num12) + num8 * (num11 / (num7 + Sign(num3, num7)) - num8)) / num13;
num10 = (num6 = 1.0);
for (int k = num; k <= num2; k++)
{
int i = k + 1;
num3 = array[i];
num11 = w[i];
num8 = num6 * num3;
num3 = num10 * num3;
num12 = (array[k] = Pythag(num7, num8));
num10 = num7 / num12;
num6 = num8 / num12;
num7 = num13 * num10 + num3 * num6;
num3 = num3 * num10 - num13 * num6;
num8 = num11 * num6;
num11 *= num10;
for (int m = 0; m < length2; m++)
{
num13 = v[m, k];
num12 = v[m, i];
v[m, k] = num13 * num10 + num12 * num6;
v[m, i] = num12 * num10 - num13 * num6;
}
num12 = Pythag(num7, num8);
w[k] = num12;
if (num12 != 0.0)
{
num12 = 1.0 / num12;
num10 = num7 * num12;
num6 = num8 * num12;
}
num7 = num10 * num3 + num6 * num11;
num13 = num10 * num11 - num6 * num3;
for (int m = 0; m < length; m++)
{
num11 = a[m, k];
num12 = a[m, i];
a[m, k] = num11 * num10 + num12 * num6;
a[m, i] = num12 * num10 - num11 * num6;
}
}
array[num] = 0.0;
array[j] = num7;
w[j] = num13;
}
}
}
private static double Sign(double a, double b)
{
if (!(b >= 0.0))
{
return 0.0 - System.Math.Abs(a);
}
return System.Math.Abs(a);
}
private static double Pythag(double a, double b)
{
double num = System.Math.Abs(a);
double num2 = System.Math.Abs(b);
if (num > num2)
{
double num3 = num2 / num;
return num * System.Math.Sqrt(1.0 + num3 * num3);
}
if (num2 > 0.0)
{
double num3 = num / num2;
return num2 * System.Math.Sqrt(1.0 + num3 * num3);
}
return 0.0;
}
}

View File

@@ -0,0 +1,16 @@
using System.Diagnostics;
using System.Reflection;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
[assembly: AssemblyTitle("AForge.Math")]
[assembly: AssemblyDescription("")]
[assembly: AssemblyConfiguration("")]
[assembly: AssemblyCompany("AForge")]
[assembly: AssemblyProduct("AForge.NET")]
[assembly: AssemblyCopyright("AForge © 2012")]
[assembly: AssemblyTrademark("")]
[assembly: ComVisible(false)]
[assembly: Guid("34dc1605-0542-49e2-b72b-89363706b71a")]
[assembly: AssemblyFileVersion("2.2.5.0")]
[assembly: AssemblyVersion("2.2.5.0")]