init
This commit is contained in:
root
@@ -0,0 +1,48 @@
|
||||
package fixed
|
||||
|
||||
// Generated with arbitrary-precision arithmetic; atan(2^-i) in Q2.62.
|
||||
var atanTable = [...]int64{
|
||||
3622009729038561421,
|
||||
2138197195906305896,
|
||||
1129764675555192497,
|
||||
573486189672913777,
|
||||
287855953345232184,
|
||||
144068303048368714,
|
||||
72051730834756821,
|
||||
36028064038054492,
|
||||
18014306884351854,
|
||||
9007187801521083,
|
||||
4503598195715549,
|
||||
2251799634728302,
|
||||
1125899884473003,
|
||||
562949950625109,
|
||||
281474976361130,
|
||||
140737488311637,
|
||||
70368744172202,
|
||||
35184372088149,
|
||||
17592186044330,
|
||||
8796093022197,
|
||||
4398046511102,
|
||||
2199023255551,
|
||||
1099511627775,
|
||||
549755813887,
|
||||
274877906943,
|
||||
137438953471,
|
||||
68719476735,
|
||||
34359738367,
|
||||
17179869183,
|
||||
8589934591,
|
||||
4294967295,
|
||||
2147483647,
|
||||
1073741823,
|
||||
536870911,
|
||||
268435455,
|
||||
134217727,
|
||||
67108863,
|
||||
33554431,
|
||||
16777215,
|
||||
8388607,
|
||||
}
|
||||
|
||||
// cordicInvK is 1/K (the CORDIC gain compensation) in Q2.62.
|
||||
const cordicInvK int64 = 2800459870029452953
|
||||
+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)
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
package fixed
|
||||
|
||||
import (
|
||||
"math"
|
||||
"math/rand"
|
||||
"testing"
|
||||
)
|
||||
|
||||
func near(t *testing.T, name string, got F, want, tol float64) {
|
||||
t.Helper()
|
||||
if d := math.Abs(got.Float64() - want); d > tol {
|
||||
t.Errorf("%s: got %v want %v (diff %g)", name, got.Float64(), want, d)
|
||||
}
|
||||
}
|
||||
|
||||
func TestMulDiv(t *testing.T) {
|
||||
near(t, "mul", FromInt(3).Mul(FromRatio(1, 2)), 1.5, 1e-9)
|
||||
near(t, "mulneg", FromInt(-3).Mul(FromRatio(1, 2)), -1.5, 1e-9)
|
||||
near(t, "div", FromInt(1).Div(FromInt(3)), 1.0/3, 1e-9)
|
||||
near(t, "divneg", FromInt(-7).Div(FromInt(2)), -3.5, 1e-9)
|
||||
if FromInt(1<<30).Mul(FromInt(1<<30)) != Max {
|
||||
t.Error("expected saturation")
|
||||
}
|
||||
if FromInt(1).Div(0) != Max || FromInt(-1).Div(0) != Min {
|
||||
t.Error("div by zero should saturate")
|
||||
}
|
||||
}
|
||||
|
||||
func TestSqrt(t *testing.T) {
|
||||
for _, v := range []float64{0.25, 1, 2, 3, 100, 1e6, 2e9} {
|
||||
f := F(v * float64(One))
|
||||
near(t, "sqrt", f.Sqrt(), math.Sqrt(v), 1e-8)
|
||||
}
|
||||
if IntSqrt(1<<62) != 1<<31 || IntSqrt(99) != 9 {
|
||||
t.Error("IntSqrt")
|
||||
}
|
||||
}
|
||||
|
||||
func TestSinCos(t *testing.T) {
|
||||
r := rand.New(rand.NewSource(1))
|
||||
for i := 0; i < 2000; i++ {
|
||||
a := (r.Float64() - 0.5) * 40
|
||||
f := F(a * float64(One))
|
||||
s, c := SinCos(f)
|
||||
af := f.Float64()
|
||||
near(t, "sin", s, math.Sin(af), 2e-9)
|
||||
near(t, "cos", c, math.Cos(af), 2e-9)
|
||||
}
|
||||
}
|
||||
|
||||
func TestAtan2(t *testing.T) {
|
||||
r := rand.New(rand.NewSource(2))
|
||||
for i := 0; i < 2000; i++ {
|
||||
x, y := (r.Float64()-0.5)*1e6, (r.Float64()-0.5)*1e6
|
||||
got := Atan2(F(y*float64(One)), F(x*float64(One)))
|
||||
near(t, "atan2", got, math.Atan2(y, x), 2e-8)
|
||||
}
|
||||
near(t, "atan2(0,-1)", Atan2(0, -One), math.Pi, 1e-8)
|
||||
}
|
||||
|
||||
func TestVec3(t *testing.T) {
|
||||
// A 3-4-12 vector has length 13; scale it far past what a naive x*x would allow.
|
||||
v := Vec{FromInt(3_000_000), FromInt(4_000_000), FromInt(12_000_000)}
|
||||
near(t, "norm3", v.Len(), 13_000_000, 1e-6)
|
||||
u := Vec{FromInt(3), FromInt(4), FromInt(12)}.Unit()
|
||||
near(t, "unit", u.Len(), 1, 1e-8)
|
||||
|
||||
d := FromSpherical(FromRatio(1, 2), FromRatio(3, 10))
|
||||
near(t, "spherical len", d.Len(), 1, 1e-8)
|
||||
near(t, "spherical z", d.Z, math.Sin(0.3), 1e-8)
|
||||
near(t, "spherical x", d.X, math.Cos(0.3)*math.Cos(0.5), 1e-8)
|
||||
|
||||
r := Vec{One, 0, 0}.RotateX(HalfPi).RotateZ(HalfPi)
|
||||
near(t, "rot x", r.X, 0, 1e-8)
|
||||
near(t, "rot y", r.Y, 1, 1e-8)
|
||||
r = Vec{0, One, 0}.RotateX(HalfPi)
|
||||
near(t, "rotx z", r.Z, 1, 1e-8)
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
package fixed
|
||||
|
||||
import "math/bits"
|
||||
|
||||
// Vec is a 3D fixed-point vector.
|
||||
type Vec struct{ X, Y, Z F }
|
||||
|
||||
func (a Vec) Add(b Vec) Vec { return Vec{a.X + b.X, a.Y + b.Y, a.Z + b.Z} }
|
||||
func (a Vec) Sub(b Vec) Vec { return Vec{a.X - b.X, a.Y - b.Y, a.Z - b.Z} }
|
||||
func (a Vec) Scale(k F) Vec { return Vec{a.X.Mul(k), a.Y.Mul(k), a.Z.Mul(k)} }
|
||||
|
||||
// Len returns the vector magnitude. The squares are accumulated in 128 bits
|
||||
// (three squares of 63-bit values cannot overflow that), so it is exact to the
|
||||
// last bit over the whole F range.
|
||||
func (a Vec) Len() F { return Norm3(a.X, a.Y, a.Z) }
|
||||
|
||||
// Hypot returns sqrt(x*x + y*y) without intermediate overflow.
|
||||
func Hypot(x, y F) F { return Norm3(x, y, 0) }
|
||||
|
||||
// Norm3 returns sqrt(x*x + y*y + z*z) without intermediate overflow.
|
||||
func Norm3(x, y, z F) F {
|
||||
h1, l1 := bits.Mul64(uabs(x), uabs(x))
|
||||
h2, l2 := bits.Mul64(uabs(y), uabs(y))
|
||||
h3, l3 := bits.Mul64(uabs(z), uabs(z))
|
||||
lo, c := bits.Add64(l1, l2, 0)
|
||||
hi, _ := bits.Add64(h1, h2, c)
|
||||
lo, c = bits.Add64(lo, l3, 0)
|
||||
hi, _ = bits.Add64(hi, h3, c)
|
||||
return F(isqrt128(hi, lo))
|
||||
}
|
||||
|
||||
// Unit returns the unit vector, or zero for the zero vector.
|
||||
func (a Vec) Unit() Vec {
|
||||
l := a.Len()
|
||||
if l == 0 {
|
||||
return Vec{}
|
||||
}
|
||||
return Vec{a.X.Div(l), a.Y.Div(l), a.Z.Div(l)}
|
||||
}
|
||||
|
||||
// FromSpherical returns the unit vector with the given azimuth (angle from +X
|
||||
// in the XY plane) and elevation (angle above the XY plane).
|
||||
func FromSpherical(azimuth, elevation F) Vec {
|
||||
sa, ca := SinCos(azimuth)
|
||||
se, ce := SinCos(elevation)
|
||||
return Vec{ce.Mul(ca), ce.Mul(sa), se}
|
||||
}
|
||||
|
||||
// RotateZ rotates counter-clockwise about the Z axis.
|
||||
func (a Vec) RotateZ(ang F) Vec {
|
||||
s, c := SinCos(ang)
|
||||
return Vec{a.X.Mul(c) - a.Y.Mul(s), a.X.Mul(s) + a.Y.Mul(c), a.Z}
|
||||
}
|
||||
|
||||
// RotateX rotates counter-clockwise about the X axis.
|
||||
func (a Vec) RotateX(ang F) Vec {
|
||||
s, c := SinCos(ang)
|
||||
return Vec{a.X, a.Y.Mul(c) - a.Z.Mul(s), a.Y.Mul(s) + a.Z.Mul(c)}
|
||||
}
|
||||
|
||||
// Dot returns the dot product, saturating on overflow.
|
||||
func (a Vec) Dot(b Vec) F { return a.X.Mul(b.X) + a.Y.Mul(b.Y) + a.Z.Mul(b.Z) }
|
||||
Reference in New Issue
Block a user