Halcyon/fixed/fixed.go

// Package fixed implements deterministic Q32.32 fixed-point arithmetic.
//
// All simulation state uses integer math only, so results are bit-exact on
// every platform. Values range over roughly ±2.1e9 with a resolution of
// about 2.3e-10.
package fixed

import (
	"math/bits"
)

// F is a signed Q32.32 fixed-point number.
type F int64

const (
	Frac   = 32
	One  F = 1 << Frac
	Half F = One / 2
	Zero F = 0
	Max  F = 1<<63 - 1
	Min  F = -1 << 63

	// Pi and friends, Q32.32.
	Pi     F = 13493037704
	TwoPi  F = 26986075409
	HalfPi F = 6746518852
)

// FromInt converts an integer to fixed point.
func FromInt(i int64) F { return F(i) << Frac }

// FromRatio returns n/d.
func FromRatio(n, d int64) F { return FromInt(n).Div(FromInt(d)) }

// Int truncates toward zero.
func (a F) Int() int64 {
	if a < 0 {
		return -int64(-a >> Frac)
	}
	return int64(a >> Frac)
}

// Floor returns the integer floor.
func (a F) Floor() int64 { return int64(a >> Frac) }

// Float64 is for display/debugging only; never use it inside the simulation.
func (a F) Float64() float64 { return float64(a) / float64(One) }

func (a F) Add(b F) F { return a + b }
func (a F) Sub(b F) F { return a - b }
func (a F) Neg() F    { return -a }

func (a F) Abs() F {
	if a < 0 {
		return -a
	}
	return a
}

// Mul returns a*b, rounded toward negative infinity, saturating on overflow.
func (a F) Mul(b F) F {
	neg := (a < 0) != (b < 0)
	hi, lo := bits.Mul64(uabs(a), uabs(b))
	// Shift the 128-bit product right by Frac.
	res := hi<<(64-Frac) | lo>>Frac
	if hi>>Frac != 0 || res > 1<<63-1 {
		if neg {
			return Min
		}
		return Max
	}
	if neg {
		// Floor for negatives keeps rounding consistent; simple truncation
		// toward zero is also deterministic, we choose truncation.
		return -F(res)
	}
	return F(res)
}

// Div returns a/b, truncated toward zero, saturating on overflow. Division by
// zero saturates to Max/Min according to the sign of a (0/0 is 0).
func (a F) Div(b F) F {
	if b == 0 {
		switch {
		case a > 0:
			return Max
		case a < 0:
			return Min
		}
		return 0
	}
	neg := (a < 0) != (b < 0)
	ua, ub := uabs(a), uabs(b)
	hi, lo := ua>>(64-Frac), ua<<Frac
	if hi >= ub {
		if neg {
			return Min
		}
		return Max
	}
	q, _ := bits.Div64(hi, lo, ub)
	if q > 1<<63-1 {
		if neg {
			return Min
		}
		return Max
	}
	if neg {
		return -F(q)
	}
	return F(q)
}

// MulInt multiplies by a plain integer.
func (a F) MulInt(n int64) F { return a * F(n) }

// DivInt divides by a plain integer.
func (a F) DivInt(n int64) F { return a / F(n) }

func uabs(a F) uint64 {
	if a < 0 {
		return uint64(-a)
	}
	return uint64(a)
}

func Min2(a, b F) F {
	if a < b {
		return a
	}
	return b
}

func Max2(a, b F) F {
	if a > b {
		return a
	}
	return b
}

func Clamp(v, lo, hi F) F {
	if v < lo {
		return lo
	}
	if v > hi {
		return hi
	}
	return v
}

// Sqrt returns the square root of a. Negative input returns 0.
func (a F) Sqrt() F {
	if a <= 0 {
		return 0
	}
	// sqrt(a/2^32)*2^32 = sqrt(a*2^32); a*2^32 fits in 96 bits.
	hi, lo := uint64(a)>>(64-Frac), uint64(a)<<Frac
	return F(isqrt128(hi, lo))
}

// isqrt128 returns floor(sqrt(hi:lo)) using the restoring bit-by-bit method.
func isqrt128(hi, lo uint64) uint64 {
	var root uint64
	var remHi, remLo uint64
	for i := 0; i < 64; i++ {
		// Bring down the next two bits of the radicand.
		remHi = remHi<<2 | remLo>>62
		remLo = remLo<<2 | hi>>62
		hi = hi<<2 | lo>>62
		lo <<= 2
		// trial = (oldRoot<<2)|1 = (newRoot<<1)|1
		root <<= 1
		trialHi, trialLo := root>>63, root<<1|1
		if remHi > trialHi || (remHi == trialHi && remLo >= trialLo) {
			var borrow uint64
			remLo, borrow = bits.Sub64(remLo, trialLo, 0)
			remHi, _ = bits.Sub64(remHi, trialHi, borrow)
			root |= 1
		}
	}
	return root
}

// IntSqrt returns floor(sqrt(n)) for n >= 0.
func IntSqrt(n uint64) uint64 { return isqrt128(0, n) }

const cordicIters = len(atanTable)

// SinCos returns sin and cos of the angle a (radians).
func SinCos(a F) (sin, cos F) {
	// Reduce to [-pi, pi).
	a = a % TwoPi
	if a >= Pi {
		a -= TwoPi
	} else if a < -Pi {
		a += TwoPi
	}
	// Reduce to [-pi/2, pi/2] with a cosine sign flip.
	negCos := false
	if a > HalfPi {
		a = Pi - a
		negCos = true
	} else if a < -HalfPi {
		a = -Pi - a
		negCos = true
	}
	x, y, z := cordicInvK, int64(0), int64(a)<<30
	for i := 0; i < cordicIters; i++ {
		dx, dy := y>>uint(i), x>>uint(i)
		if z >= 0 {
			x, y, z = x-dx, y+dy, z-atanTable[i]
		} else {
			x, y, z = x+dx, y-dy, z+atanTable[i]
		}
	}
	s, c := F(round30(y)), F(round30(x))
	if negCos {
		c = -c
	}
	return s, c
}

func round30(v int64) int64 { return (v + 1<<29) >> 30 }

func Sin(a F) F { s, _ := SinCos(a); return s }
func Cos(a F) F { _, c := SinCos(a); return c }

// Atan2 returns the angle of the vector (x, y) in (-pi, pi].
func Atan2(y, x F) F {
	if x == 0 && y == 0 {
		return 0
	}
	// Scale so the vector is large but cannot overflow during iteration.
	vx, vy := int64(x), int64(y)
	for (vx > 1<<60 || vx < -(1<<60)) || (vy > 1<<60 || vy < -(1<<60)) {
		vx >>= 1
		vy >>= 1
	}
	var offset int64 // multiples of pi, in Q32
	if vx < 0 {
		// Rotate by pi so x >= 0.
		vx, vy = -vx, -vy
		if y >= 0 {
			offset = int64(Pi)
		} else {
			offset = -int64(Pi)
		}
	}
	// Normalize magnitude up to use precision (keep < 2^61).
	for (vx < 1<<59) && (vy < 1<<59) && (vy > -(1 << 59)) {
		vx <<= 1
		vy <<= 1
	}
	var z int64
	for i := 0; i < cordicIters; i++ {
		dx, dy := vy>>uint(i), vx>>uint(i)
		if vy > 0 {
			vx, vy, z = vx+dx, vy-dy, z+atanTable[i]
		} else {
			vx, vy, z = vx-dx, vy+dy, z-atanTable[i]
		}
	}
	return F(round30(z) + offset)
}