Fixed point math library intended for use with a toy physics engine (a project for another day).

FixedPoint.cs

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);
    }
}

FPMatrix2.cs

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})";
    }
}

FPVector2.cs

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;
    }
}

Program.cs

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");
    }
}