Created
May 28, 2018 03:09
-
-
Save kyythane/67dc4c8d3b7e93da77b221dff0c76c73 to your computer and use it in GitHub Desktop.
Fixed point math library intended for use with a toy physics engine (a project for another day).
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
using System; | |
using System.Globalization; | |
public struct FixedPoint | |
{ | |
private readonly int _backing; | |
public int FractionalBits { get; } | |
public float FractionalDivisor { get; } | |
public FixedPoint(int whole, int fractional, int precision) : this((whole << precision) + fractional, precision) | |
{ | |
} | |
public FixedPoint(int initialValue, int precision) : this() | |
{ | |
FractionalBits = precision; | |
FractionalDivisor = 1 << FractionalBits; | |
_backing = initialValue; | |
} | |
public static FixedPoint operator +(FixedPoint f1, FixedPoint f2) | |
{ | |
if (f1.FractionalBits == f2.FractionalBits) return new FixedPoint(f1._backing + f2._backing, f1.FractionalBits); | |
var l1 = (long) f1._backing << (32 - f1.FractionalBits); | |
var l2 = (long) f2._backing << (32 - f2.FractionalBits); | |
var precision = Math.Min(f1.FractionalBits, f2.FractionalBits); | |
return new FixedPoint((int) ((l1 + l2) >> (32 - precision)), precision); | |
} | |
public static FixedPoint operator -(FixedPoint f1, FixedPoint f2) | |
{ | |
if (f1.FractionalBits == f2.FractionalBits) return new FixedPoint(f1._backing - f2._backing, f1.FractionalBits); | |
var l1 = (long) f1._backing << (32 - f1.FractionalBits); | |
var l2 = (long) f2._backing << (32 - f2.FractionalBits); | |
var precision = Math.Min(f1.FractionalBits, f2.FractionalBits); | |
return new FixedPoint((int) ((l1 - l2) >> (32 - precision)), precision); | |
} | |
public static FixedPoint operator -(FixedPoint fp) | |
{ | |
return new FixedPoint(-fp._backing, fp.FractionalBits); | |
} | |
public static FixedPoint operator *(FixedPoint f1, FixedPoint f2) | |
{ | |
var precision = Math.Min(f1.FractionalBits, f2.FractionalBits); | |
var shift = Math.Max(f1.FractionalBits, f2.FractionalBits); | |
//we need to promote this to long so we can multiply values that result in a number greater than 2^16 | |
var mult = (long) f1._backing * f2._backing; | |
mult += (mult & (1 << (shift - 1))) << 1; //round | |
return new FixedPoint((int) (mult >> shift), precision); | |
} | |
public static FixedPoint operator /(FixedPoint f1, FixedPoint f2) | |
{ | |
if (f1.FractionalBits != f2.FractionalBits) throw new ArgumentException("Mixed precision divide is not implemented."); | |
var precision = f1.FractionalBits; | |
var div = (int) (((long) f1._backing << precision) / f2._backing); | |
return new FixedPoint(div, precision); | |
} | |
public static FixedPoint operator +(FixedPoint f1, int f2) | |
{ | |
var precision = f1.FractionalBits; | |
return new FixedPoint(f1._backing + (f2 << precision), precision); | |
} | |
public static FixedPoint operator +(int f1, FixedPoint f2) | |
{ | |
return f2 + f1; //Commutative Property | |
} | |
public static FixedPoint operator -(FixedPoint f1, int f2) | |
{ | |
var precision = f1.FractionalBits; | |
return new FixedPoint(f1._backing - (f2 << precision), precision); | |
} | |
public static FixedPoint operator -(int f1, FixedPoint f2) | |
{ | |
var precision = f2.FractionalBits; | |
return new FixedPoint((f1 << precision) - f2._backing, precision); | |
} | |
public static FixedPoint operator *(FixedPoint f1, int f2) | |
{ | |
var precision = f1.FractionalBits; | |
//we need to promote this to long so we can multiply values that result in a number greater than 2^16 | |
var mult = (long) f1._backing * (f2 << precision); | |
mult += (mult & (1 << (precision - 1))) << 1; //round | |
return new FixedPoint((int) (mult >> precision), precision); | |
} | |
public static FixedPoint operator *(int f1, FixedPoint f2) | |
{ | |
return f2 * f1; //Commutative Property | |
} | |
public static FixedPoint operator /(FixedPoint f1, int f2) | |
{ | |
var precision = f1.FractionalBits; | |
var div = (int) (((long) f1._backing << precision) / (f2 << precision)); | |
return new FixedPoint(div, precision); | |
} | |
public static FixedPoint operator /(int f1, FixedPoint f2) | |
{ | |
var precision = f2.FractionalBits; | |
var div = (int) (((long) (f1 << precision) << precision) / f2._backing); | |
return new FixedPoint(div, precision); | |
} | |
public static bool operator ==(FixedPoint f1, FixedPoint f2) | |
{ | |
return f1._backing == f2._backing; | |
} | |
public static bool operator !=(FixedPoint f1, FixedPoint f2) | |
{ | |
return !(f1 == f2); | |
} | |
public static bool operator >(FixedPoint f1, FixedPoint f2) | |
{ | |
return f1._backing > f2._backing; | |
} | |
public static bool operator <(FixedPoint f1, FixedPoint f2) | |
{ | |
return f1._backing < f2._backing; | |
} | |
public static bool operator >=(FixedPoint f1, FixedPoint f2) | |
{ | |
return f1._backing >= f2._backing; | |
} | |
public static bool operator <=(FixedPoint f1, FixedPoint f2) | |
{ | |
return f1._backing <= f2._backing; | |
} | |
public static bool operator ==(FixedPoint f1, int f2) | |
{ | |
return f1._backing >> f1.FractionalBits == f2; | |
} | |
public static bool operator !=(FixedPoint f1, int f2) | |
{ | |
return !(f1 == f2); | |
} | |
public static bool operator >(FixedPoint f1, int f2) | |
{ | |
return f1._backing >> f1.FractionalBits > f2; | |
} | |
public static bool operator <(FixedPoint f1, int f2) | |
{ | |
return f1._backing >> f1.FractionalBits < f2; | |
} | |
public static bool operator >=(FixedPoint f1, int f2) | |
{ | |
return f1._backing >> f1.FractionalBits >= f2; | |
} | |
public static bool operator <=(FixedPoint f1, int f2) | |
{ | |
return f1._backing >> f1.FractionalBits <= f2; | |
} | |
public static FixedPoint operator >>(FixedPoint f1, int f2) | |
{ | |
return new FixedPoint(f1._backing >> f2, f1.FractionalBits); | |
} | |
public static FixedPoint operator <<(FixedPoint f1, int f2) | |
{ | |
return new FixedPoint(f1._backing << f2, f1.FractionalBits); | |
} | |
public static implicit operator float(FixedPoint f1) | |
{ | |
return f1._backing / f1.FractionalDivisor; | |
} | |
public override int GetHashCode() | |
{ | |
return _backing.GetHashCode(); | |
} | |
public override string ToString() | |
{ | |
return ToString(PrintMode.FloatingPoint); | |
} | |
public enum PrintMode | |
{ | |
Fractional, | |
FloatingPoint | |
} | |
public string ToString(PrintMode mode) | |
{ | |
switch (mode) | |
{ | |
case PrintMode.Fractional: | |
var fractional = _backing & ((1 << FractionalBits) - 1); | |
var whole = _backing >> FractionalBits; | |
if (fractional == 0) return (_backing >> FractionalBits).ToString(); | |
return (whole != 0 ? whole + " and " : "") + fractional + "/" + (1 << FractionalBits); | |
case PrintMode.FloatingPoint: | |
return ((float) this).ToString(CultureInfo.InvariantCulture); | |
default: | |
throw new ArgumentOutOfRangeException(nameof(mode), mode, null); | |
} | |
} | |
public override bool Equals(object obj) | |
{ | |
return obj is FixedPoint point && Equals(point); | |
} | |
public bool Equals(FixedPoint fp) | |
{ | |
return fp == this; | |
} | |
public static FixedPoint Abs(FixedPoint fp) | |
{ | |
return new FixedPoint(Math.Abs(fp._backing), fp.FractionalBits); | |
} | |
public static FixedPoint Sqrt(FixedPoint fp) | |
{ | |
return Sqrt(fp, new FixedPoint(1 << (fp.FractionalBits >> 1), fp.FractionalBits)); | |
} | |
public static FixedPoint Sqrt(FixedPoint fp, FixedPoint error) | |
{ | |
var precision = fp.FractionalBits; | |
var n = (long) fp._backing << (16 - precision); | |
var res = (n >> 1) + 1; | |
long last; | |
var epsilon = (long) error._backing << (16 - precision); | |
n = n << 16; | |
do | |
{ | |
last = res; | |
var div = n / res; | |
res = (res + div) >> 1; | |
} while (Math.Abs(res - last) > epsilon); | |
return new FixedPoint((int) (res >> (16 - precision)), precision); | |
} | |
public static FixedPoint Sin(FixedPoint angle) | |
{ | |
var precision = angle.FractionalBits; | |
//40.24 constants (AngleConstants.AngleFormat) | |
const long coefB = 21361415L; //about 4 / Pi; | |
const long coefC = -6799550L; // about -4 / (Pi * Pi); | |
const long coefP = 3774874L; //about 0.225 | |
var expanded = (long) angle._backing << (AngleConstants.AngleFormat - precision); | |
var approximation = ExpandedMult(coefB, expanded) + | |
ExpandedMult(ExpandedMult(coefC, expanded), Math.Abs(expanded)); | |
var res = ExpandedMult(coefP, ExpandedMult(approximation, Math.Abs(approximation)) - approximation) + | |
approximation; //combining porabolas | |
return new FixedPoint((int) (res >> (AngleConstants.AngleFormat - precision)), precision); | |
} | |
private static long ExpandedMult(long a, long b) | |
{ | |
var mult = a * b; | |
mult += (mult & (1 << (AngleConstants.AngleFormat - 1))) << 1; | |
return mult >> AngleConstants.AngleFormat; | |
} | |
public static FixedPoint Cos(FixedPoint fp) | |
{ | |
//shift Sine | |
fp += AngleConstants.HalfPi; | |
if (fp > AngleConstants.Pi) // Original x > pi/2 | |
fp -= 2 * AngleConstants.Pi; // Wrap: cos(x) = cos(x - 2 pi) | |
return Sin(fp); | |
} | |
public static FixedPoint Max(FixedPoint f1, FixedPoint f2) | |
{ | |
return f1 > f2 ? f1 : f2; | |
} | |
public static FixedPoint Min(FixedPoint f1, FixedPoint f2) | |
{ | |
return f1 < f2 ? f1 : f2; | |
} | |
public static FixedPoint CreateFraction(int numerator, int denominator, int format) | |
{ | |
return new FixedPoint(numerator, format) / new FixedPoint(denominator, format); | |
} | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
public struct FPMatrix2 | |
{ | |
public FixedPoint A { get; } | |
public FixedPoint B { get; } | |
public FixedPoint C { get; } | |
public FixedPoint D { get; } | |
public FPMatrix2(FixedPoint a, FixedPoint b, FixedPoint c, FixedPoint d) : this() | |
{ | |
A = a; | |
B = b; | |
C = c; | |
D = d; | |
} | |
public static FpVector2 operator *(FPMatrix2 matrix, FpVector2 vector) | |
{ | |
var x = matrix.A*vector.X + matrix.B*vector.Y; | |
var y = matrix.C*vector.X + matrix.D*vector.Y; | |
return new FpVector2(x, y); | |
} | |
public static FPMatrix2 operator +(FPMatrix2 left, FPMatrix2 right) | |
{ | |
return new FPMatrix2(left.A + right.A, left.B + right.B, | |
left.C + right.C, left.D + right.D); | |
} | |
public static FPMatrix2 operator *(FPMatrix2 left, FPMatrix2 right) | |
{ | |
var a = left.A * right.A + left.B * right.C; | |
var b = left.A * right.B + left.B * right.D; | |
var c = left.C * right.A + left.D * right.C; | |
var d = left.C * right.B + left.D * right.D; | |
return new FPMatrix2(a, b, c, d); | |
} | |
public static FPMatrix2 CreateRotation(FixedPoint radians) | |
{ | |
var sin = FixedPoint.Sin(radians); | |
var cos = FixedPoint.Cos(radians); | |
return new FPMatrix2(cos, -sin, sin, cos); | |
} | |
public override bool Equals(object obj) | |
{ | |
return obj is FPMatrix2 matrix2 && Equals(matrix2); | |
} | |
public bool Equals(FPMatrix2 mat) | |
{ | |
return mat.A == A && mat.B == B && mat.C == C && mat.D == D; | |
} | |
public override int GetHashCode() | |
{ | |
unchecked | |
{ | |
var hash = (int)2166136261; | |
hash = (hash * 16777619) ^ A.GetHashCode(); | |
hash = (hash * 16777619) ^ B.GetHashCode(); | |
hash = (hash * 16777619) ^ C.GetHashCode(); | |
hash = (hash * 16777619) ^ D.GetHashCode(); | |
return hash; | |
} | |
} | |
public override string ToString() | |
{ | |
return $"({A}, {B},\n{C}, {D})"; | |
} | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
public struct FpVector2 | |
{ | |
public static readonly FpVector2 Zero = new FpVector2(0, 0); | |
public static readonly FpVector2 Left = new FpVector2(-1, 0); | |
public static readonly FpVector2 Up = new FpVector2(0, -1); | |
public static readonly FpVector2 Right = new FpVector2(1, 0); | |
public static readonly FpVector2 Down = new FpVector2(0, 1); | |
public const int DefaultFractionalBits = 8; | |
public FixedPoint X { get; } | |
public FixedPoint Y { get; } | |
private FixedPoint _magnitude; | |
public FixedPoint Magnitude | |
{ | |
get | |
{ | |
if (_magnitude < 0) | |
{ | |
_magnitude = FixedPoint.Sqrt(X * X + Y * Y); | |
} | |
return _magnitude; | |
} | |
} | |
public FixedPoint MagnitudeSquared => X * X + Y * Y; | |
public FpVector2(int x, int y) | |
: this(new FixedPoint(x, 0, DefaultFractionalBits), new FixedPoint(y, 0, DefaultFractionalBits)) { } | |
public FpVector2(FixedPoint x, FixedPoint y) : this() | |
{ | |
X = x; | |
Y = y; | |
_magnitude = new FixedPoint(-1,0); | |
} | |
public static FpVector2 operator +(FpVector2 vectorA, FpVector2 vectorB) | |
{ | |
return new FpVector2(vectorA.X + vectorB.X, | |
vectorA.Y + vectorB.Y); | |
} | |
public static FpVector2 operator -(FpVector2 vectorA, FpVector2 vectorB) | |
{ | |
return new FpVector2(vectorA.X - vectorB.X, | |
vectorA.Y - vectorB.Y); | |
} | |
public static FpVector2 operator *(FpVector2 vectorA, FixedPoint vectorB) | |
{ | |
return new FpVector2(vectorA.X * vectorB, | |
vectorA.Y * vectorB); | |
} | |
public static FpVector2 operator *(FpVector2 vectorA, int vectorB) | |
{ | |
return new FpVector2(vectorA.X * vectorB, | |
vectorA.Y * vectorB); | |
} | |
public static FpVector2 operator /(FpVector2 vectorA, FixedPoint vectorB) | |
{ | |
return new FpVector2(vectorA.X / vectorB, | |
vectorA.Y / vectorB); | |
} | |
public static FpVector2 operator /(FpVector2 vectorA, int vectorB) | |
{ | |
return new FpVector2(vectorA.X / vectorB, | |
vectorA.Y / vectorB); | |
} | |
public static FpVector2 operator -(FpVector2 vector) | |
{ | |
return new FpVector2(-vector.X, -vector.Y); | |
} | |
public static bool operator ==(FpVector2 vector1, FpVector2 vector2) | |
{ | |
return vector1.X == vector2.Y && vector1.Y == vector2.Y; | |
} | |
public static bool operator !=(FpVector2 vector1, FpVector2 vector2) | |
{ | |
return !(vector1 == vector2); | |
} | |
public static FpVector2 Normalize(FpVector2 vector) | |
{ | |
return vector.Magnitude > 0 ? vector/vector.Magnitude : new FpVector2(0, 0); | |
} | |
public static FixedPoint Dot(FpVector2 vectorA, FpVector2 vectorB) | |
{ | |
return vectorA.X*vectorB.X + vectorA.Y*vectorB.Y; | |
} | |
public static FixedPoint Cross(FpVector2 vectorA, FpVector2 vectorB) | |
{ | |
return vectorA.X * vectorB.Y - vectorA.Y * vectorB.X; | |
} | |
public override bool Equals(object obj) | |
{ | |
return obj is FpVector2 vector2 && Equals(vector2); | |
} | |
public bool Equals(FpVector2 vect) | |
{ | |
return vect.X == X && vect.Y == Y; | |
} | |
public override int GetHashCode() | |
{ | |
unchecked | |
{ | |
var hash = (int) 2166136261; | |
hash = (hash*16777619) ^ X.GetHashCode(); | |
hash = (hash*16777619) ^ Y.GetHashCode(); | |
return hash; | |
} | |
} | |
public override string ToString() | |
{ | |
return "(" + X + ", " + Y + ")"; | |
} | |
public static float DistanceFromPointToLineSegment(FpVector2 point, FpVector2 anchor, FpVector2 end) | |
{ | |
var d = end - anchor; | |
var length = d.Magnitude; | |
if (FixedPoint.Abs(length) == 0) | |
{ | |
return (point - anchor).Magnitude; | |
} | |
var nowmalized = Normalize(d); | |
var intersect = Dot((point - anchor), nowmalized); | |
if (intersect < 0) | |
{ | |
return (point - anchor).Magnitude; | |
} | |
return intersect > length ? (point - end).Magnitude : (point - (anchor + nowmalized * intersect)).Magnitude; | |
} | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
using System; | |
using System.IO; | |
using System.Text; | |
internal class Program | |
{ | |
public static void Main(string[] args) | |
{ | |
var sinCosString = new StringBuilder("x, cos fixed, cos float, cos error, sin fixed, sin float, sin error\n"); | |
for (var i = 0; i < 52690944 * 2; i += 65536) | |
{ | |
var x = new FixedPoint(i - 52690944, AngleConstants.AngleFormat); | |
var fixedCos = FixedPoint.Cos(x); | |
var cos = Math.Cos(x); | |
var fixedSin = FixedPoint.Sin(x); | |
var sin = Math.Sin(x); | |
sinCosString.AppendLine($"{x}, {fixedCos}, {cos}, {fixedCos - cos}, {fixedSin}, {sin}, {fixedSin - sin}"); | |
} | |
using (var stream = File.Open("sin_cos_results.csv", FileMode.OpenOrCreate)) | |
{ | |
using (var writer = new StreamWriter(stream)) | |
{ | |
writer.Write(sinCosString); | |
} | |
} | |
var sqrtString = new StringBuilder("x, fixed sqrt, sqrt, error\n"); | |
for (var i = 1; i < 256; i++) | |
{ | |
var x = new FixedPoint(i, 0, 8); | |
var fixedRoot = FixedPoint.Sqrt(x); | |
var root = Math.Sqrt(x); | |
sqrtString.AppendLine($"{x}, {fixedRoot}, {root}, {fixedRoot - root}"); | |
} | |
using (var stream = File.Open("sqrt_results.csv", FileMode.OpenOrCreate)) | |
{ | |
using (var writer = new StreamWriter(stream)) | |
{ | |
writer.Write(sqrtString); | |
} | |
} | |
Console.Out.Write("Complete"); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment