init
This commit is contained in:
root
+264
@@ -0,0 +1,264 @@
|
||||
// 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)
|
||||
}
|
||||
Reference in New Issue
Block a user