Skip to content

Instantly share code, notes, and snippets.

@antonfirsov
Last active February 5, 2017 13:00
Show Gist options
  • Select an option

  • Save antonfirsov/661d924f4ac5764b01d53db7c3f47378 to your computer and use it in GitHub Desktop.

Select an option

Save antonfirsov/661d924f4ac5764b01d53db7c3f47378 to your computer and use it in GitHub Desktop.
using System;
using System.Collections.Generic;
using System.Diagnostics.Contracts;
using System.Linq;
using OurVerySecretLib.MathF.LinearAlgebra;
using System.Globalization;
using M = System.Math;
namespace OurVerySecretLib.MathF
{
/// <summary>
/// Compares numbers and vectors with a tolerance Eps, which is passed in the constructor.
/// </summary>
public struct ApproximateComparerF :
IComparer<float>,
IEqualityComparer<float>,
IEqualityComparer<Vect2F>,
IEqualityComparer<Vect3F>,
IEqualityComparer<Vect4F>
{
// --- Data Members ---------------------------------------------------
private readonly float _negEps;
private readonly float _negEps2;
private readonly float _negEps15;
/// <summary>
/// This field is not read-only, so be careful ...
/// </summary>
public static ApproximateComparerF Default;
/// <summary>
/// Epsilon value
/// </summary>
public float Eps { get; }
/// <summary>
/// Squared epsilon
/// </summary>
public float Eps2 { get; }
/// <summary>
/// Eps ^ 1.5
/// </summary>
public float Eps15 { get; }
public int RoundingDecimals { get; }
// --- Construction ---------------------------------------------------
static ApproximateComparerF()
{
Default = CreateDefault();
}
public ApproximateComparerF(float eps)
{
Eps = eps;
Eps2 = eps * eps;
Eps15 = (float)M.Pow(Eps, 1.5);
_negEps = -Eps;
_negEps2 = -Eps2;
_negEps15 = -Eps15;
RoundingDecimals = CalcDecimals(Eps);
}
public static ApproximateComparerF CreateDefault()
{
return new ApproximateComparerF(MathX.Eps);
}
public static ApproximateComparerF CreateSmallToleranceComparer()
{
return new ApproximateComparerF(MathX.SmallEps);
}
public static ApproximateComparerF CreateLargeToleranceComparer()
{
return new ApproximateComparerF(0.001f);
}
public static int CalcDecimals(float eps)
{
return M.Max(0, (int)M.Round(-M.Log10(eps)));
}
// --- Object overrides -----------------------------------------------
public override string ToString()
{
return "Epsilon: " + Eps + ", Rounding decimals: " + RoundingDecimals;
}
public string ToString(float x)
{
return ToString(x, CultureInfo.InvariantCulture);
}
public string ToString(float x, IFormatProvider formatProvider)
{
return x != 0 && IsZero(x) ? (x < 0 ? "-0" : "+0") : x.ToString(formatProvider);
}
// --- Operator overloads ---------------------------------------------
public static ApproximateComparerF operator *(ApproximateComparerF apx, float s)
{
return new ApproximateComparerF(apx.Eps * s);
}
public static ApproximateComparerF operator *(float s, ApproximateComparerF apx)
{
return new ApproximateComparerF(apx.Eps * s);
}
public static ApproximateComparerF operator /(ApproximateComparerF apx, float s)
{
return new ApproximateComparerF(apx.Eps / s);
}
// Public methods --------------------------------------------
public int Compare(float a, float b)
{
var diff = a - b;
if (diff < _negEps)
return -1;
if (diff > Eps)
return +1;
return 0;
}
public bool IsLeft(Vect2F checkedVect, Vect2F baseVect)
{
var cross = baseVect.x * checkedVect.y - baseVect.y * checkedVect.x;
return cross > Eps2;
}
public bool IsRight(Vect2F checkedVect, Vect2F baseVect)
{
var cross = baseVect.x * checkedVect.y - baseVect.y * checkedVect.x;
return cross < _negEps2;
}
#region Standard comparsions
[Pure]
public bool IsZero(float x)
{
return x <= Eps && x >= _negEps;
}
[Pure]
public bool IsZero(Vect4F v)
{
return IsZero(v.x) && IsZero(v.y) && IsZero(v.z) && IsZero(v.w);
}
[Pure]
public bool IsZero(Vect3F v)
{
return IsZero(v.x) && IsZero(v.y) && IsZero(v.z);
}
[Pure]
public bool IsZero(Vect2F v)
{
return IsZero(v.x) && IsZero(v.y);
}
[Pure]
public bool IsZero(ref Vect4F v)
{
return IsZero(v.x) && IsZero(v.y) && IsZero(v.z) && IsZero(v.w);
}
[Pure]
public bool IsZero(ref Vect3F v)
{
return IsZero(v.x) && IsZero(v.y) && IsZero(v.z);
}
[Pure]
public bool IsZero(ref Vect2F v)
{
return IsZero(v.x) && IsZero(v.y);
}
[Pure]
public bool IsZero(ref Matrix3F a)
{
return IsZero(a.m11) &&
IsZero(a.m12) &&
IsZero(a.m13) &&
IsZero(a.m21) &&
IsZero(a.m22) &&
IsZero(a.m23) &&
IsZero(a.m31) &&
IsZero(a.m32) &&
IsZero(a.m33);
}
[Pure]
public bool IsZero(ref Matrix4F a)
{
return IsZero(a.m11) &&
IsZero(a.m12) &&
IsZero(a.m13) &&
IsZero(a.m14) &&
IsZero(a.m21) &&
IsZero(a.m22) &&
IsZero(a.m23) &&
IsZero(a.m24) &&
IsZero(a.m31) &&
IsZero(a.m32) &&
IsZero(a.m33) &&
IsZero(a.m34) &&
IsZero(a.m41) &&
IsZero(a.m42) &&
IsZero(a.m43) &&
IsZero(a.m44);
}
/// <summary>
/// a &gt; b
/// </summary>
[Pure]
public bool IsGreater(float a, float b)
{
return a > b + Eps;
}
/// <summary>
/// a &lt; b
/// </summary>
[Pure]
public bool IsLess(float a, float b)
{
return a < b - Eps;
}
/// <summary>
/// a == b
/// </summary>
[Pure]
public bool Equals(float a, float b)
{
var d = a - b;
return d < Eps && d > _negEps;
}
[Pure]
public bool Equals(Vect2F a, Vect2F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
return IsZero(dx) && IsZero(dy);
}
[Pure]
public bool Equals(ref Vect2F a, Vect2F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
return IsZero(dx) && IsZero(dy);
}
[Pure]
public bool Equals(ref Vect2F a, ref Vect2F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
return IsZero(dx) && IsZero(dy);
}
[Pure]
public bool Equals(Vect3F a, Vect3F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
var dz = a.z - b.z;
return IsZero(dx) && IsZero(dy) && IsZero(dz);
}
[Pure]
public bool Equals(ref Vect3F a, Vect3F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
var dz = a.z - b.z;
return IsZero(dx) && IsZero(dy) && IsZero(dz);
}
[Pure]
public bool Equals(ref Vect3F a, ref Vect3F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
var dz = a.z - b.z;
return IsZero(dx) && IsZero(dy) && IsZero(dz);
}
[Pure]
public bool Equals(Vect4F a, Vect4F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
var dz = a.z - b.z;
var dw = a.w - b.w;
return IsZero(dx) && IsZero(dy) && IsZero(dz) && IsZero(dw);
}
[Pure]
public bool Equals(ref Vect4F a, Vect4F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
var dz = a.z - b.z;
var dw = a.w - b.w;
return IsZero(dx) && IsZero(dy) && IsZero(dz) && IsZero(dw);
}
[Pure]
public bool Equals(ref Vect4F a, ref Vect4F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
var dz = a.z - b.z;
var dw = a.w - b.w;
return IsZero(dx) && IsZero(dy) && IsZero(dz) && IsZero(dw);
}
/// <summary>
/// Transform matrices are often accessed through properties. Properties cannot be passed by ref to a function. :(
/// So these property-accessed matrices HAVE TO be copied (it's a value type :( )
/// </summary>
/// <param name="a">Matrix A.</param>
/// <param name="b">Matrix B</param>
/// <returns><see langword="true"/> iff the two matrices are approximately equal.</returns>
[Pure]
public bool Equals(Matrix3F a, Matrix3F b)
{
return Equals(ref a, ref b);
}
/// <summary>
/// Transform matrices are often accessed through properties. Properties cannot be passed by ref to a function. :(
/// So these property-accessed matrices HAVE TO be copied (it's a value type :( )
/// </summary>
/// <param name="a">Matrix A.</param>
/// <param name="b">Matrix B</param>
/// <returns><see langword="true"/> iff the two matrices are approximately equal.</returns>
[Pure]
public bool Equals(ref Matrix3F a, Matrix3F b)
{
return Equals(ref a, ref b);
}
[Pure]
public bool Equals(ref Matrix3F a, ref Matrix3F b)
{
return Equals(a.m11, b.m11) &&
Equals(a.m12, b.m12) &&
Equals(a.m13, b.m13) &&
Equals(a.m21, b.m21) &&
Equals(a.m22, b.m22) &&
Equals(a.m23, b.m23) &&
Equals(a.m31, b.m31) &&
Equals(a.m32, b.m32) &&
Equals(a.m33, b.m33);
}
/// <summary>
/// Transform matrices are often accessed through properties. Properties cannot be passed by ref to a function. :(
/// So these property-accessed matrices HAVE TO be copied (it's a value type :( )
/// </summary>
/// <param name="a">Matrix A.</param>
/// <param name="b">Matrix B</param>
/// <returns><see langword="true"/> iff the two matrices are approximately equal.</returns>
[Pure]
public bool Equals(Matrix4F a, Matrix4F b)
{
return Equals(ref a, ref b);
}
/// <summary>
/// Transform matrices are often accessed through properties. Properties cannot be passed by ref to a function. :(
/// So these property-accessed matrices HAVE TO be copied (it's a value type :( )
/// </summary>
/// <param name="a">Matrix A.</param>
/// <param name="b">Matrix B</param>
/// <returns><see langword="true"/> iff the two matrices are approximately equal.</returns>
[Pure]
public bool Equals(ref Matrix4F a, Matrix4F b)
{
return Equals(ref a, ref b);
}
[Pure]
public bool Equals(ref Matrix4F a, ref Matrix4F b)
{
return Equals(a.m11, b.m11) &&
Equals(a.m12, b.m12) &&
Equals(a.m13, b.m13) &&
Equals(a.m14, b.m14) &&
Equals(a.m21, b.m21) &&
Equals(a.m22, b.m22) &&
Equals(a.m23, b.m23) &&
Equals(a.m24, b.m24) &&
Equals(a.m31, b.m31) &&
Equals(a.m32, b.m32) &&
Equals(a.m33, b.m33) &&
Equals(a.m34, b.m34) &&
Equals(a.m41, b.m41) &&
Equals(a.m42, b.m42) &&
Equals(a.m43, b.m43) &&
Equals(a.m44, b.m44);
}
[Pure]
public bool IsPositive(float x)
{
return x > Eps;
}
[Pure]
public bool IsNegative(float x)
{
return x < _negEps;
}
/// <summary>
/// a &gt;= b
/// </summary>
[Pure]
public bool IsGreaterOrEqual(float a, float b)
{
return a >= b - Eps;
}
/// <summary>
/// Compares the two vectors (A and B) component-by-component.
/// </summary>
/// <param name="a">Vector A.</param>
/// <param name="b">Vector B.</param>
/// <returns><see langword="true"/> iff the vector A &gt;= vector B.</returns>
[Pure]
public bool IsGreaterOrEqual(Vect2F a, Vect2F b)
{
return IsGreaterOrEqual(a.x, b.x) &&
IsGreaterOrEqual(a.y, b.y);
}
/// <summary>
/// Compares the two vectors (A and B) component-by-component.
/// </summary>
/// <param name="a">Vector A.</param>
/// <param name="b">Vector B.</param>
/// <returns><see langword="true"/> iff the vector A &gt;= vector B.</returns>
[Pure]
public bool IsGreaterOrEqual(Vect3F a, Vect3F b)
{
return IsGreaterOrEqual(a.x, b.x) &&
IsGreaterOrEqual(a.y, b.y) &&
IsGreaterOrEqual(a.z, b.z);
}
/// <summary>
/// Compares the two vectors (A and B) component-by-component.
/// </summary>
/// <param name="a">Vector A.</param>
/// <param name="b">Vector B.</param>
/// <returns><see langword="true"/> iff the vector A &gt;= vector B.</returns>
[Pure]
public bool IsGreaterOrEqual(Vect4F a, Vect4F b)
{
return IsGreaterOrEqual(a.x, b.x) &&
IsGreaterOrEqual(a.y, b.y) &&
IsGreaterOrEqual(a.z, b.z) &&
IsGreaterOrEqual(a.w, b.w);
}
/// <summary>
/// a &lt;= b
/// </summary>
[Pure]
public bool IsLessOrEqual(float a, float b) // a <= b
{
return b >= a - Eps;
}
/// <summary>
/// Compares the two vectors (A and B) component-by-component.
/// </summary>
/// <param name="a">Vector A.</param>
/// <param name="b">Vector B.</param>
/// <returns><see langword="true"/> iff the vector A &lt;= vector B.</returns>
[Pure]
public bool IsLessOrEqual(Vect2F a, Vect2F b)
{
return IsLessOrEqual(a.x, b.x) &&
IsLessOrEqual(a.y, b.y);
}
/// <summary>
/// Compares the two vectors (A and B) component-by-component.
/// </summary>
/// <param name="a">Vector A.</param>
/// <param name="b">Vector B.</param>
/// <returns><see langword="true"/> iff the vector A &lt;= vector B.</returns>
[Pure]
public bool IsLessOrEqual(Vect3F a, Vect3F b)
{
return IsLessOrEqual(a.x, b.x) &&
IsLessOrEqual(a.y, b.y) &&
IsLessOrEqual(a.z, b.z);
}
/// <summary>
/// Compares the two vectors (A and B) component-by-component.
/// </summary>
/// <param name="a">Vector A.</param>
/// <param name="b">Vector B.</param>
/// <returns><see langword="true"/> iff the vector A &lt;= vector B.</returns>
[Pure]
public bool IsLessOrEqual(Vect4F a, Vect4F b)
{
return IsLessOrEqual(a.x, b.x) &&
IsLessOrEqual(a.y, b.y) &&
IsLessOrEqual(a.z, b.z) &&
IsLessOrEqual(a.w, b.w);
}
/// <summary>
/// Detects if t is inside [a, b]
/// </summary>
/// <param name="t"></param>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns><see langword="true"/> if t is in the closed interval [a, b], <see langword="false"/> otherwise.</returns>
[Pure]
public bool InInterval(float t, float a, float b)
{
return IsGreaterOrEqual(t, a) && IsLessOrEqual(t, b);
}
[Pure]
public int Sign(float a)
{
if (a >= Eps) return 1;
if (a <= _negEps) return -1;
return 0;
}
#endregion
#region Quadratic comparsions
[Pure]
public bool IsZero2(float x)
{
return x > _negEps2 && x < Eps2;
}
[Pure]
public bool IsZero2(Vect3F v)
{
return IsZero2(v.x) && IsZero2(v.y) && IsZero2(v.z);
}
[Pure]
public bool IsZero2(Vect2F v)
{
return IsZero2(v.x) && IsZero2(v.y);
}
[Pure]
public bool IsZero2(ref Vect3F v)
{
return IsZero2(v.x) && IsZero2(v.y) && IsZero2(v.z);
}
[Pure]
public bool IsZero2(ref Vect2F v)
{
return IsZero2(v.x) && IsZero2(v.y);
}
/// <summary>
/// a &gt; b
/// </summary>
[Pure]
public bool IsGreater2(float a, float b)
{
return a > b + Eps2;
}
/// <summary>
/// a &lt; b
/// </summary>
[Pure]
public bool IsLess2(float a, float b)
{
return a < b - Eps2;
}
/// <summary>
/// a == b
/// </summary>
[Pure]
public bool Equals2(float a, float b)
{
var d = a - b;
return d < Eps2 && d > _negEps2;
}
[Pure]
public bool Equals2(Vect2F a, Vect2F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
return IsZero2(dx) && IsZero2(dy);
}
[Pure]
public bool Equals2(ref Vect2F a, ref Vect2F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
return IsZero2(dx) && IsZero2(dy);
}
[Pure]
public bool Equals2(Vect3F a, Vect3F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
var dz = a.z - b.z;
return IsZero2(dx) && IsZero2(dy) && IsZero2(dz);
}
[Pure]
public bool Equals2(ref Vect3F a, ref Vect3F b)
{
var dx = a.x - b.x;
var dy = a.y - b.y;
var dz = a.z - b.z;
return IsZero2(dx) && IsZero2(dy) && IsZero2(dz);
}
[Pure]
public bool IsPositive2(float x)
{
return x > Eps2;
}
[Pure]
public bool IsNegative2(float x)
{
return x < _negEps2;
}
/// <summary>
/// a &gt;= b
/// </summary>
[Pure]
public bool IsGreaterOrEqual2(float a, float b)
{
return a > b - Eps2;
}
/// <summary>
/// a &lt;= b
/// </summary>
[Pure]
public bool IsLessOrEqual2(float a, float b) // a <= b
{
return b > a - Eps2;
}
[Pure]
public int Sign2(float a)
{
if (a > Eps2) return 1;
if (a < _negEps2) return -1;
return 0;
}
#endregion
[Pure]
public bool IsZero15(float a)
{
return a >= _negEps15 && a <= Eps15;
}
/// <summary>
/// Compares 2 normal vectors, if looking to the same direction, and their enclosing angles beeing: sin(a) &lt; Eps.
/// The input vectors must be normalized!
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
[Pure]
public bool SameNormals(ref Vect3F a, ref Vect3F b)
{
if (Vect3F.Dot(ref a, ref b) < 0) return false;
var c = Vect3F.Cross(ref a, ref b);
return IsZero2(c.LengthSquared);
}
/// <summary>
/// Compares 2 normal vectors, if looking to the same direction, and their enclosing angles beeing: sin(a) &lt; Eps.
/// The input vectors must be normalized!
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
[Pure]
public bool SameNormals(Vect3F a, Vect3F b)
{
if (Vect3F.Dot(ref a, ref b) < 0) return false;
var c = Vect3F.Cross(ref a, ref b);
return IsZero2(c.LengthSquared);
}
/// <summary>
/// Same as SameNormals() but, the input vectors are not required to be normalized.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
[Pure]
public bool SameNormalsSafe(Vect3F a, Vect3F b)
{
a.Normalize();
b.Normalize();
if (Vect3F.Dot(ref a, ref b) < 0) return false;
var c = Vect3F.Cross(ref a, ref b);
return IsZero2(c.LengthSquared);
}
[Pure]
public float Round(float x)
{
return (float)M.Round(x, RoundingDecimals);
}
[Pure]
public Vect2F Round(Vect2F v)
{
return new Vect2F(
M.Round(v.x, RoundingDecimals),
M.Round(v.y, RoundingDecimals));
}
[Pure]
public Vect3F Round(Vect3F v)
{
return new Vect3F(
M.Round(v.x, RoundingDecimals),
M.Round(v.y, RoundingDecimals),
M.Round(v.z, RoundingDecimals));
}
[Pure]
public Vect4F Round(Vect4F v)
{
return new Vect4F(
M.Round(v.x, RoundingDecimals),
M.Round(v.y, RoundingDecimals),
M.Round(v.z, RoundingDecimals),
M.Round(v.w, RoundingDecimals));
}
/// <summary>
/// Calculate the following value:
/// res = |a b| = a*b + c*d
/// |c d|
/// Returns exactly 0.0 if IsZero15(res) is expected.
///
/// Idea taken from:
/// http://realtimecollisiondetection.net/pubs/GDC07_Ericson_Physics_Tutorial_Numerical_Robustness.ppt
/// (slide 42)
/// </summary>
public double MulSum(float a, float b, float c, float d)
{
var max = M.Max(M.Max(M.Max(M.Abs(a), M.Abs(b)), M.Abs(c)), M.Abs(d));
var eps = max < 1 ? Eps15 : max * Eps15;
var det = a * b + c * d;
if (det > -eps && det < eps) det = 0;
return det;
}
/// <summary>
/// Calculate corrected 2x2 determinant:
/// det = |a b| = a*b - c*d
/// |c d|
/// Returns exactly 0.0 if IsZero15(det) is expected.
///
/// Idea taken from:
/// http://realtimecollisiondetection.net/pubs/GDC07_Ericson_Physics_Tutorial_Numerical_Robustness.ppt
/// (slide 42)
/// </summary>
public double MulDiff(float a, float b, float c, float d)
{
var max = M.Max(M.Max(M.Max(M.Abs(a), M.Abs(b)), M.Abs(c)), M.Abs(d));
var eps = max < 1 ? Eps15 : max * Eps15;
var det = a * b - c * d;
if (det > -eps && det < eps) det = 0;
return det;
}
/// <summary>
/// Corrected difference. If IsZero(result) is true, it's set to be exactly 0.
/// </summary>
/// <returns></returns>
public float CDiff0(float a, float b)
{
var d = a - b;
if (_negEps > d && Eps < d) d = 0;
return d;
}
#region Conversion
public static explicit operator ApproximateComparerF(Math.ApproximateComparer dApx)
{
return new ApproximateComparerF((float)dApx.Eps);
}
public static explicit operator Math.ApproximateComparer(ApproximateComparerF fApx)
{
return new Math.ApproximateComparer(fApx.Eps);
}
#endregion
#region IEqualityComparer Members
bool IEqualityComparer<float>.Equals(float x, float y)
{
return Equals(x, y);
}
int IEqualityComparer<float>.GetHashCode(float obj)
{
throw new InvalidOperationException();
}
bool IEqualityComparer<Vect2F>.Equals(Vect2F x, Vect2F y)
{
return Equals(ref x, ref y);
}
int IEqualityComparer<Vect2F>.GetHashCode(Vect2F obj)
{
throw new InvalidOperationException();
}
bool IEqualityComparer<Vect3F>.Equals(Vect3F x, Vect3F y)
{
return Equals(ref x, ref y);
}
int IEqualityComparer<Vect3F>.GetHashCode(Vect3F obj)
{
throw new InvalidOperationException();
}
bool IEqualityComparer<Vect4F>.Equals(Vect4F x, Vect4F y)
{
return Equals(ref x, ref y);
}
int IEqualityComparer<Vect4F>.GetHashCode(Vect4F obj)
{
throw new InvalidOperationException();
}
#endregion
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment