init

fixed/vec.go

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