2 files changed, 81 insertions, 0 deletions
diff --git a/src/3d/visual_debug.cc b/src/3d/visual_debug.cc
index 9667d87..3f5778a 100644
--- a/src/3d/visual_debug.cc
+++ b/src/3d/visual_debug.cc
@@ -225,6 +225,81 @@ void VisualDebug::add_mesh_normals(const mat4& transform, uint32_t num_vertices,
   }
 }
 
+void VisualDebug::add_line(const vec3& start, const vec3& end, const vec3& color) {
+  lines_.push_back({start, end, color});
+}
+
+void VisualDebug::add_cross(const vec3& center, float size, const vec3& color) {
+  float s = size * 0.5f;
+  add_line(center - vec3(s, 0, 0), center + vec3(s, 0, 0), color);
+  add_line(center - vec3(0, s, 0), center + vec3(0, s, 0), color);
+  add_line(center - vec3(0, 0, s), center + vec3(0, 0, s), color);
+}
+
+void VisualDebug::add_sphere(const vec3& center, float radius, const vec3& color) {
+  const int kSegments = 16;
+  const float kStep = 6.2831853f / kSegments;
+
+  auto draw_circle = [&](int axis1, int axis2) {
+    for (int i = 0; i < kSegments; ++i) {
+      float a1 = i * kStep;
+      float a2 = (i + 1) * kStep;
+      vec3 p1 = center;
+      vec3 p2 = center;
+      
+      // axis mapping: 0=x, 1=y, 2=z
+      p1[axis1] += std::cos(a1) * radius;
+      p1[axis2] += std::sin(a1) * radius;
+      
+      p2[axis1] += std::cos(a2) * radius;
+      p2[axis2] += std::sin(a2) * radius;
+      
+      add_line(p1, p2, color);
+    }
+  };
+
+  draw_circle(0, 1); // XY
+  draw_circle(0, 2); // XZ
+  draw_circle(1, 2); // YZ
+}
+
+void VisualDebug::add_cone(const vec3& apex, const vec3& dir, float height, float radius, const vec3& color) {
+  vec3 d = dir.normalize();
+  vec3 base_center = apex + d * height;
+  
+  // Rotation to align (0, 1, 0) with d
+  quat q = quat::from_to(vec3(0, 1, 0), d);
+  
+  const int kSegments = 16;
+  const float kStep = 6.2831853f / kSegments;
+  
+  for (int i = 0; i < kSegments; ++i) {
+    float a1 = i * kStep;
+    float a2 = (i + 1) * kStep;
+    
+    // Circle points in XZ plane (local space, y=0)
+    vec3 p1_local(std::cos(a1) * radius, 0, std::sin(a1) * radius);
+    vec3 p2_local(std::cos(a2) * radius, 0, std::sin(a2) * radius);
+    
+    // Rotate and translate to base_center
+    vec3 p1 = base_center + q.rotate(p1_local);
+    vec3 p2 = base_center + q.rotate(p2_local);
+    
+    add_line(p1, p2, color); // Base circle
+    
+    // Connect to apex (draw every 4th segment to avoid clutter)
+    if (i % 4 == 0) {
+      add_line(apex, p1, color);
+    }
+  }
+}
+
+void VisualDebug::add_trajectory(const std::vector<vec3>& points, const vec3& color) {
+  if (points.size() < 2) return;
+  for (size_t i = 0; i < points.size() - 1; ++i) {
+    add_line(points[i], points[i + 1], color);
+  }
+}
 
 void VisualDebug::update_buffers(const mat4& view_proj) {
   // Update Uniforms
diff --git a/src/3d/visual_debug.h b/src/3d/visual_debug.h
index 505a799..7f1aa8b 100644
--- a/src/3d/visual_debug.h
+++ b/src/3d/visual_debug.h
@@ -31,6 +31,12 @@ class VisualDebug {
 
   void add_mesh_normals(const mat4& transform, uint32_t num_vertices, const MeshVertex* vertices);
 
+  void add_line(const vec3& start, const vec3& end, const vec3& color);
+  void add_cross(const vec3& center, float size, const vec3& color);
+  void add_sphere(const vec3& center, float radius, const vec3& color);
+  void add_cone(const vec3& apex, const vec3& dir, float height, float radius, const vec3& color);
+  void add_trajectory(const std::vector<vec3>& points, const vec3& color);
+
   // Render all queued primitives and clear the queue
   void render(WGPURenderPassEncoder pass, const mat4& view_proj);