root
63 lines
2.0 KiB
Go
63 lines
2.0 KiB
Go
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) }
|