first commit

pkg/mathutil/vec3.go

@@ -0,0 +1,56 @@
+package mathutil
+
+import "math"
+
+type Vec3 struct {
+	X float32
+	Y float32
+	Z float32
+}
+
+func NewVec3(x, y, z float32) Vec3 {
+	return Vec3{X: x, Y: y, Z: z}
+}
+
+func (v Vec3) Add(other Vec3) Vec3 {
+	return Vec3{X: v.X + other.X, Y: v.Y + other.Y, Z: v.Z + other.Z}
+}
+
+func (v Vec3) Sub(other Vec3) Vec3 {
+	return Vec3{X: v.X - other.X, Y: v.Y - other.Y, Z: v.Z - other.Z}
+}
+
+func (v Vec3) Scale(s float32) Vec3 {
+	return Vec3{X: v.X * s, Y: v.Y * s, Z: v.Z * s}
+}
+
+func (v Vec3) Length() float32 {
+	return float32(math.Sqrt(float64(v.X*v.X + v.Y*v.Y + v.Z*v.Z)))
+}
+
+func (v Vec3) LengthSq() float32 {
+	return v.X*v.X + v.Y*v.Y + v.Z*v.Z
+}
+
+func (v Vec3) Normalize() Vec3 {
+	l := v.Length()
+	if l < 1e-8 {
+		return Vec3{}
+	}
+	return v.Scale(1.0 / l)
+}
+
+func (v Vec3) DistanceTo(other Vec3) float32 {
+	return v.Sub(other).Length()
+}
+
+func (v Vec3) DistanceSqTo(other Vec3) float32 {
+	return v.Sub(other).LengthSq()
+}
+
+// DistanceXZ returns distance ignoring Y axis (for ground-plane calculations).
+func (v Vec3) DistanceXZ(other Vec3) float32 {
+	dx := v.X - other.X
+	dz := v.Z - other.Z
+	return float32(math.Sqrt(float64(dx*dx + dz*dz)))
+}