From e2a005d2b9eac41dd82e4fdfe9cb8ab9bf723428 Mon Sep 17 00:00:00 2001
From: Zack Buhman <zack@buhman.org>
Date: Fri, 25 Jul 2025 13:32:07 -0500
Subject: [PATCH] demo: add collision

---
 demo.mk                               |   5 +-
 src/demo/collision.cpp                | 124 ++++++++++++
 src/demo/collision.hpp                |  28 +++
 src/demo/sailboat.cpp                 |   1 -
 src/physics/collide.cpp               | 107 +++++++++++
 src/physics/collide.hpp               |  78 ++++++++
 src/physics/contact.hpp               |  16 ++
 src/platform/graphics.cpp             |   6 +-
 src/platform/graphics_primitive.cpp   | 263 ++++++++++++++++++++++++++
 src/platform/graphics_primitive.hpp   |  63 ++----
 src/platform/ta_parameter_presets.hpp |  27 ++-
 11 files changed, 665 insertions(+), 53 deletions(-)
 create mode 100644 src/demo/collision.cpp
 create mode 100644 src/demo/collision.hpp
 create mode 100644 src/physics/collide.cpp
 create mode 100644 src/physics/collide.hpp
 create mode 100644 src/physics/contact.hpp
 create mode 100644 src/platform/graphics_primitive.cpp

diff --git a/demo.mk b/demo.mk
index 5d4667d..f5552c5 100644
--- a/demo.mk
+++ b/demo.mk
@@ -15,17 +15,20 @@ DEMO_OBJ = \
 	$(LIB)/printf/parse.o \
 	src/platform/main.o \
 	src/platform/graphics.o \
+	src/platform/graphics_primitive.o \
 	src/platform/input.o \
 	src/platform/texture.o \
 	src/platform/font.o \
 	src/demo/ballistics.o \
 	src/demo/bridge.o \
 	src/demo/sailboat.o \
+	src/demo/collision.o \
 	src/physics/particle.o \
 	src/physics/particle_link.o \
 	src/physics/particle_contact.o \
 	src/physics/body.o \
-	src/physics/force_generator.o
+	src/physics/force_generator.o \
+	src/physics/collide.o
 
 demo.elf: LDSCRIPT = $(LIB)/main.lds
 demo.elf: $(START_OBJ) $(DEMO_OBJ) $(FONT_OBJ) $(LIBGCC)
diff --git a/src/demo/collision.cpp b/src/demo/collision.cpp
new file mode 100644
index 0000000..a3f7c5a
--- /dev/null
+++ b/src/demo/collision.cpp
@@ -0,0 +1,124 @@
+#include "math/float_types.hpp"
+#include "math/transform.hpp"
+
+#include "platform/graphics_primitive.hpp"
+#include "platform/font.hpp"
+
+#include "demo/collision.hpp"
+
+#include "assert.h"
+
+namespace demo {
+  static inline mat3x3 inertia_tensor_coeffs(float ix, float iy, float iz,
+                                             float ixy = 0, float ixz = 0, float iyz = 0)
+  {
+    return mat3x3(  ix, -ixy, -ixz,
+                  -ixy,   iy, -iyz,
+                  -ixz, -iyz,   iz);
+  }
+
+  mat4x4 collision::init()
+  {
+    plane.body = &plane_body;
+    plane.body->position = {0, 0, 0};
+    plane.body->orientation = {0, 0, 0, 1};
+    plane.body->calculate_derived_data();
+    plane.normal = {0, 1, 0};
+    plane.distance = 0;
+    plane.offset = mat4x4();
+    plane.update_transform();
+
+    sphere.body = &sphere_body;
+    sphere.body->inverse_mass = 1.0f / 1.0f;
+    sphere.body->position = {0, 1, 0};
+    sphere.body->orientation = {0, 0, 0, 1};
+    sphere.body->velocity = {0, 0, 0};
+    sphere.body->rotation = {0, 0, 0};
+    sphere.body->set_inertia_tensor(inertia_tensor_coeffs(5.0, 5.0, 5.0));
+    sphere.body->linear_damping = 0.998f;
+    sphere.body->angular_damping = 0.998f;
+    sphere.body->acceleration = {0, -9.81, 0};
+    sphere.body->calculate_derived_data();
+    sphere.radius = 0.25f;
+    sphere.offset = mat4x4();
+    sphere.update_transform();
+
+    collision_data.ix = 0;
+    collision_data.length = 128;
+    collision_data.restitution = 0.2;
+    collision_data.friction = 0.9;
+
+    mat4x4 view_trans
+      = translate(vec3(0.0, 0.0, 0.5))
+      * rotate_x(pi / 4)
+      * rotate_y(pi / 4)
+      * scale(vec3(-3, -3, 3));
+
+    return view_trans;
+  }
+
+  void collision::update()
+  {
+    sphere.body->clear_accumulators();
+
+    // generate contacts
+    collision_data.ix = 0;
+    int res = physics::collision_detector::sphere_and_halfspace(sphere, plane, collision_data);
+    printf("res %d\n", res);
+
+    // set iterations
+
+    // resolve contacts
+
+    float duration = 1.0f / 600.0f;
+    sphere.body->integrate(duration);
+    sphere.update_transform();
+  }
+
+  void collision::draw_hud(ta_parameter_writer& writer)
+  {
+    const int title_length = 15;
+    const int title_width_2 = (font::ter_u12n.hori_advance * title_length) >> 1;
+    const int framebuffer_width_2 = framebuffer::framebuffer.px_width >> 1;
+    const int x = framebuffer_width_2 - title_width_2;
+    vec3 center_p = vec3(x, 5, 10);
+
+    font::ter_u12n.global(writer);
+    font::ter_u12n.draw_string(writer, center_p, "demo: collision", 0xffffffff);
+  }
+
+  void collision::draw(ta_parameter_writer& writer, const mat4x4& trans)
+  {
+    // punch through
+    draw_hud(writer);
+
+    writer.append<ta_global_parameter::end_of_list>() =
+      ta_global_parameter::end_of_list(para_control::para_type::end_of_list);
+
+    // opaque
+    draw_axis(writer, trans);
+
+    draw_icosphere(writer,
+                   trans * sphere.transform * scale(sphere.radius),
+                   vec3(1, 0, 1));
+
+    vec3 n = plane.normal;
+    vec3 k = plane.normal * plane.distance;
+    vec3 color(1, 1, 0);
+    draw_halfspace(writer,
+                   k,
+                   n,
+                   trans,
+                   color);
+
+    writer.append<ta_global_parameter::end_of_list>() =
+      ta_global_parameter::end_of_list(para_control::para_type::end_of_list);
+
+  }
+}
+
+
+extern "C"
+void memset()
+{
+}
diff --git a/src/demo/collision.hpp b/src/demo/collision.hpp
new file mode 100644
index 0000000..9ab5a24
--- /dev/null
+++ b/src/demo/collision.hpp
@@ -0,0 +1,28 @@
+#pragma once
+
+#include "demo/scene.hpp"
+#include "physics/collide.hpp"
+
+namespace demo {
+
+  struct collision : scene {
+    physics::contact contacts[128];
+    physics::collision_data collision_data;
+
+    physics::rigid_body plane_body;
+    physics::collision_plane plane;
+    physics::rigid_body sphere_body;
+    physics::collision_sphere sphere;
+
+    collision()
+      : collision_data(contacts, 128, 0.2, 0.9)
+    {}
+
+    mat4x4 init() override;
+
+    void update() override;
+
+    void draw_hud(ta_parameter_writer& writer);
+    void draw(ta_parameter_writer& writer, const mat4x4& trans) override;
+  };
+}
diff --git a/src/demo/sailboat.cpp b/src/demo/sailboat.cpp
index faf948f..ce09a20 100644
--- a/src/demo/sailboat.cpp
+++ b/src/demo/sailboat.cpp
@@ -25,7 +25,6 @@ namespace demo {
                                  0.3f * mass * (squares.x + squares.y));
   }
 
-
   mat4x4 sailboat::init()
   {
     buoyancy.center_of_buoyancy = vec3(0.0f, 0.5f, 0.0f);
diff --git a/src/physics/collide.cpp b/src/physics/collide.cpp
new file mode 100644
index 0000000..74da309
--- /dev/null
+++ b/src/physics/collide.cpp
@@ -0,0 +1,107 @@
+#include "physics/collide.hpp"
+
+namespace physics {
+
+  int collision_detector::sphere_and_sphere(const collision_sphere& a,
+                                            const collision_sphere& b,
+                                            collision_data& data)
+  {
+    if (data.ix >= data.length)
+      return 0;
+
+    vec3 a_position = col(a.transform, 3);
+    vec3 b_position = col(b.transform, 3);
+
+    vec3 mid = a_position - b_position;
+    float length = magnitude(mid);
+
+    if (length >= (a.radius + b.radius)) {
+      return 0;
+    }
+
+    vec3 normal = mid * (1.0f / length);
+    contact * contact = data.add_contact();
+    contact->contact_normal = normal;
+    contact->contact_point = a_position + mid * 0.5f;
+    contact->penetration = a.radius + b.radius - length;
+    contact->body[0] = a.body;
+    contact->body[1] = b.body;
+    contact->restitution = data.restitution;
+    contact->friction = data.friction;
+
+    return 1;
+  }
+
+  int collision_detector::sphere_and_halfspace(const collision_sphere& sphere,
+                                               const collision_plane& plane,
+                                               collision_data& data)
+  {
+    if (data.ix >= data.length)
+      return 0;
+
+    vec3 position = col(sphere.transform, 3);
+    float distance = dot(plane.normal, position) - sphere.radius - plane.distance;
+
+    if (distance >= 0)
+      return 0;
+
+    contact * contact = data.add_contact();
+    contact->contact_normal = plane.normal;
+    contact->penetration = -distance;
+    contact->contact_point = position - plane.normal * (distance + sphere.radius);
+    contact->body[0] = sphere.body;
+    contact->body[1] = nullptr;
+    contact->restitution = data.restitution;
+    contact->friction = data.friction;
+
+    return 1;
+  }
+
+  int collision_detector::sphere_and_plane(const collision_sphere& sphere,
+                                           const collision_plane& plane,
+                                           collision_data& data)
+  {
+    if (data.ix >= data.length)
+      return 0;
+
+    vec3 position = col(sphere.transform, 3);
+
+    float distance = dot(plane.normal, position) - plane.distance;
+
+    if (distance * distance > sphere.radius * sphere.radius) {
+      return 0;
+    }
+
+    vec3 normal;
+    float penetration;
+    if (distance < 0) {
+      normal = -plane.normal;
+      penetration = distance;
+    } else {
+      normal = plane.normal;
+      penetration = -distance;
+    }
+    penetration += sphere.radius;
+
+    contact * contact = data.add_contact();
+    contact->contact_normal = normal;
+    contact->penetration = penetration;
+    contact->contact_point = position - plane.normal * distance;
+    contact->body[0] = sphere.body;
+    contact->body[1] = nullptr;
+    contact->restitution = data.restitution;
+    contact->friction = data.friction;
+
+    return 1;
+  }
+
+  int collision_detector::box_and_halfspace(const collision_box& box,
+                                            const collision_plane& plane,
+                                            collision_data& data)
+  {
+    if (data.ix >= data.length)
+      return 0;
+
+    return 0;
+  }
+}
diff --git a/src/physics/collide.hpp b/src/physics/collide.hpp
new file mode 100644
index 0000000..4ed8e1c
--- /dev/null
+++ b/src/physics/collide.hpp
@@ -0,0 +1,78 @@
+#include "physics/body.hpp"
+#include "physics/contact.hpp"
+
+namespace physics {
+
+  struct collision_primitive
+  {
+    rigid_body * body;
+    mat4x4 offset;
+    mat4x4 transform;
+
+    inline void update_transform()
+    {
+      transform = body->transform_matrix * offset;
+    }
+  };
+
+  struct collision_sphere : collision_primitive
+  {
+    float radius;
+  };
+
+  struct collision_plane : collision_primitive
+  {
+    vec3 normal;
+    float distance;
+  };
+
+  struct collision_box : collision_primitive
+  {
+    vec3 half_size;
+  };
+
+  struct collision_data
+  {
+    contact * const contacts;
+    int ix;
+    int length; // const
+
+    // what...
+    float restitution; // const
+    float friction; // const
+
+    collision_data(contact * contacts,
+                   int length,
+                   float restitution,
+                   float friction)
+      : contacts(contacts), ix(0), length(length), restitution(restitution), friction(friction)
+    {}
+
+    inline contact * add_contact()
+    {
+      contact * contact = &contacts[ix];
+      ix += 1;
+      return contact;
+    }
+  };
+
+  struct collision_detector
+  {
+    static int sphere_and_sphere(const collision_sphere& a,
+                                 const collision_sphere& b,
+                                 collision_data& data);
+
+    static int sphere_and_halfspace(const collision_sphere& sphere,
+                                    const collision_plane& plane,
+                                    collision_data& data);
+
+    static int sphere_and_plane(const collision_sphere& sphere,
+                                const collision_plane& plane,
+                                collision_data& data);
+
+    static int box_and_halfspace(const collision_box& box,
+                                 const collision_plane& plane,
+                                 collision_data& data);
+  };
+
+}
diff --git a/src/physics/contact.hpp b/src/physics/contact.hpp
new file mode 100644
index 0000000..49f4f88
--- /dev/null
+++ b/src/physics/contact.hpp
@@ -0,0 +1,16 @@
+#include "math/float_types.hpp"
+#include "physics/body.hpp"
+
+namespace physics {
+
+  struct contact {
+    rigid_body * body[2];
+
+    float restitution;
+    float friction;
+
+    vec3 contact_point;
+    vec3 contact_normal;
+    float penetration;
+  };
+}
diff --git a/src/platform/graphics.cpp b/src/platform/graphics.cpp
index 3defb5b..757413f 100644
--- a/src/platform/graphics.cpp
+++ b/src/platform/graphics.cpp
@@ -26,6 +26,7 @@
 #include "demo/ballistics.hpp"
 #include "demo/bridge.hpp"
 #include "demo/sailboat.hpp"
+#include "demo/collision.hpp"
 
 #include "maple/maple_bus_bits.hpp"
 
@@ -96,7 +97,8 @@ namespace graphics {
   demo::ballistics _ballistics;
   demo::bridge _bridge;
   demo::sailboat _sailboat;
-  demo::scene * current_scene = &_sailboat;
+  demo::collision _collision;
+  demo::scene * current_scene = &_collision;
 
   void draw()
   {
@@ -188,7 +190,7 @@ namespace graphics {
 
         current_scene->analog(dx);
 
-        view_trans = view_trans * rotate_y(dy);
+        view_trans = view_trans * rotate_y(dy);// * rotate_x(dx);
       }
     }
   }
diff --git a/src/platform/graphics_primitive.cpp b/src/platform/graphics_primitive.cpp
new file mode 100644
index 0000000..9777da4
--- /dev/null
+++ b/src/platform/graphics_primitive.cpp
@@ -0,0 +1,263 @@
+#include "math/transform.hpp"
+
+#include "platform/graphics_primitive.hpp"
+
+static inline float _intensity(const mat4x4& trans, const vec3& normal)
+{
+  const vec3 light_vec = {-1, -1, -10};
+
+  vec3 n = normal_multiply(trans, normal);
+  float n_dot_l = dot(n, light_vec);
+  float intensity = 0.5f;
+  if (n_dot_l > 0)
+    intensity += 0.5f * n_dot_l * (inverse_length(n) * inverse_length(light_vec));
+  return intensity;
+}
+
+void draw_cube(ta_parameter_writer& writer,
+               const mat4x4& trans,
+               const vec3& color)
+{
+  static const vec3 position[] = {
+    {-1.0, -1.0, 1.0},
+    {-1.0, 1.0, 1.0},
+    {-1.0, -1.0, -1.0},
+    {-1.0, 1.0, -1.0},
+    {1.0, -1.0, 1.0},
+    {1.0, 1.0, 1.0},
+    {1.0, -1.0, -1.0},
+    {1.0, 1.0, -1.0},
+  };
+  const int position_length = (sizeof (position)) / (sizeof (position[0]));
+
+  struct quad {
+    int a, b, c, d;
+  };
+
+  static const quad quads[] = {
+    {0, 1, 3, 2},
+    {2, 3, 7, 6},
+    {6, 7, 5, 4},
+    {4, 5, 1, 0},
+    {2, 6, 4, 0},
+    {7, 3, 1, 5}
+  };
+  const int quads_length = (sizeof (quads)) / (sizeof (quads[0]));
+
+  vec3 cache[position_length];
+  for (int i = 0; i < position_length; i++) {
+    cache[i] = screen_transform(trans * position[i]);
+  }
+
+  global_polygon_intensity(writer, color);
+
+  for (int i = 0; i < quads_length; i++) {
+    const vec3& ap = cache[quads[i].a];
+    const vec3& bp = cache[quads[i].b];
+    const vec3& cp = cache[quads[i].c];
+    const vec3& dp = cache[quads[i].d];
+
+    quad_type_2(writer,
+                ap,
+                bp,
+                cp,
+                dp,
+                1.0f);
+  }
+}
+
+void draw_icosphere(ta_parameter_writer& writer,
+                    const mat4x4& trans,
+                    const vec3& color)
+{
+  static const vec3 position[] = {
+    {0.000000, -1.000000, 0.000000},
+    {0.723600, -0.447215, 0.525720},
+    {-0.276385, -0.447215, 0.850640},
+    {-0.894425, -0.447215, 0.000000},
+    {-0.276385, -0.447215, -0.850640},
+    {0.723600, -0.447215, -0.525720},
+    {0.276385, 0.447215, 0.850640},
+    {-0.723600, 0.447215, 0.525720},
+    {-0.723600, 0.447215, -0.525720},
+    {0.276385, 0.447215, -0.850640},
+    {0.894425, 0.447215, 0.000000},
+    {0.000000, 1.000000, 0.000000},
+  };
+  const int position_length = (sizeof (position)) / (sizeof (position[0]));
+  static const vec3 normal[] = {
+    {0.1876, -0.7947, 0.5774},
+    {0.6071, -0.7947, -0.0000},
+    {-0.4911, -0.7947, 0.3568},
+    {-0.4911, -0.7947, -0.3568},
+    {0.1876, -0.7947, -0.5774},
+    {0.9822, -0.1876, -0.0000},
+    {0.3035, -0.1876, 0.9342},
+    {-0.7946, -0.1876, 0.5774},
+    {-0.7946, -0.1876, -0.5774},
+    {0.3035, -0.1876, -0.9342},
+    {0.7946, 0.1876, 0.5774},
+    {-0.3035, 0.1876, 0.9342},
+    {-0.9822, 0.1876, -0.0000},
+    {-0.3035, 0.1876, -0.9342},
+    {0.7946, 0.1876, -0.5774},
+    {0.4911, 0.7947, 0.3568},
+    {-0.1876, 0.7947, 0.5774},
+    {-0.6071, 0.7947, -0.0000},
+    {-0.1876, 0.7947, -0.5774},
+    {0.4911, 0.7947, -0.3568},
+  };
+
+  struct pn {
+    int p, n;
+  };
+
+  struct tri {
+    int a;
+    int b;
+    int c;
+    int n;
+  };
+
+  static const tri tris[] = {
+    {0, 1, 2, 0},
+    {1, 0, 5, 1},
+    {0, 2, 3, 2},
+    {0, 3, 4, 3},
+    {0, 4, 5, 4},
+    {1, 5, 10, 5},
+    {2, 1, 6, 6},
+    {3, 2, 7, 7},
+    {4, 3, 8, 8},
+    {5, 4, 9, 9},
+    {1, 10, 6, 10},
+    {2, 6, 7, 11},
+    {3, 7, 8, 12},
+    {4, 8, 9, 13},
+    {5, 9, 10, 14},
+    {6, 10, 11, 15},
+    {7, 6, 11, 16},
+    {8, 7, 11, 17},
+    {9, 8, 11, 18},
+    {10, 9, 11, 19},
+  };
+  const int tris_length = (sizeof (tris)) / (sizeof (tris[0]));
+
+  static vec3 position_cache[position_length];
+  for (int i = 0; i < position_length; i++) {
+    position_cache[i] = screen_transform(trans * position[i]);
+  }
+
+  global_polygon_intensity(writer, color);
+
+  for (int i = 0; i < tris_length; i++) {
+    const vec3& ap = position_cache[tris[i].a];
+    const vec3& bp = position_cache[tris[i].b];
+    const vec3& cp = position_cache[tris[i].c];
+    const float& intensity = _intensity(trans, normal[tris[i].n]);
+
+    tri_type_2(writer,
+               ap,
+               bp,
+               cp,
+               intensity);
+  }
+}
+
+void draw_plane(ta_parameter_writer& writer,
+                const vec3& k,
+                const vec3& n,
+                const mat4x4& trans,
+                const vec3& color)
+{
+  static const vec3 position[] = {
+    {-1.000000, 0.000000,  1.000000},
+    { 1.000000, 0.000000,  1.000000},
+    {-1.000000, 0.000000, -1.000000},
+    { 1.000000, 0.000000, -1.000000},
+  };
+
+  //const vec3 normal = {0.0000 1.0000 0.0000};
+
+  vec3 u = {0, 1, 0};
+  if (dot(u, n) > 0.7071f) u = {0, 0, 1};
+  vec3 r = normalize(cross(u, n));
+  u = normalize(cross(n, r));
+
+  mat4x4 m = {
+    r.x, n.x, u.x, k.x,
+    r.y, n.y, u.y, k.y,
+    r.z, n.z, u.z, k.z,
+    0, 0, 0, 1
+  };
+  mat4x4 t = trans * m;
+
+  vec3 ap = screen_transform(t * position[0]);
+  vec3 bp = screen_transform(t * position[1]);
+  vec3 cp = screen_transform(t * position[3]);
+  vec3 dp = screen_transform(t * position[2]);
+
+  global_polygon_intensity(writer, color);
+  quad_type_2(writer,
+              ap,
+              bp,
+              cp,
+              dp,
+              1.0f);
+}
+
+void draw_halfspace(ta_parameter_writer& writer,
+                    const vec3& k,
+                    const vec3& n,
+                    const mat4x4& trans,
+                    const vec3& color)
+{
+  static const vec3 position[] = {
+    {-1.000000, 0.000000,  1.000000},
+    { 1.000000, 0.000000,  1.000000},
+    {-1.000000, 0.000000, -1.000000},
+    { 1.000000, 0.000000, -1.000000},
+  };
+
+  const vec3 red = {0.906, 0.369, 0.373};
+
+  //const vec3 normal = {0.0000 1.0000 0.0000};
+
+  vec3 u = {0, 1, 0};
+  if (dot(u, n) > 0.7071f) u = {0, 0, 1};
+  vec3 r = normalize(cross(u, n));
+  u = normalize(cross(n, r));
+
+  mat4x4 m = {
+    r.x, n.x, u.x, k.x,
+    r.y, n.y, u.y, k.y,
+    r.z, n.z, u.z, k.z,
+    0, 0, 0, 1
+  };
+  mat4x4 t = trans * m;
+
+  vec3 ap = screen_transform(t * position[0]);
+  vec3 bp = screen_transform(t * position[1]);
+  vec3 cp = screen_transform(t * position[3]);
+  vec3 dp = screen_transform(t * position[2]);
+
+  global_polygon_intensity(writer,
+                           color,
+                           isp_tsp_instruction_word::culling_mode::cull_if_negative);
+  quad_type_2(writer,
+              ap,
+              bp,
+              cp,
+              dp,
+              1.0f);
+
+  global_polygon_intensity(writer,
+                           red,
+                           isp_tsp_instruction_word::culling_mode::cull_if_positive);
+  quad_type_2(writer,
+              ap,
+              bp,
+              cp,
+              dp,
+              1.0f);
+}
diff --git a/src/platform/graphics_primitive.hpp b/src/platform/graphics_primitive.hpp
index d5a8551..7946d35 100644
--- a/src/platform/graphics_primitive.hpp
+++ b/src/platform/graphics_primitive.hpp
@@ -87,53 +87,22 @@ static inline void draw_axis(ta_parameter_writer& writer,
             z_axis);
 }
 
-static inline void draw_cube(ta_parameter_writer& writer,
-                             const mat4x4& trans,
-                             const vec3& color)
-{
-  static const vec3 position[] = {
-    {-1.0, -1.0, 1.0},
-    {-1.0, 1.0, 1.0},
-    {-1.0, -1.0, -1.0},
-    {-1.0, 1.0, -1.0},
-    {1.0, -1.0, 1.0},
-    {1.0, 1.0, 1.0},
-    {1.0, -1.0, -1.0},
-    {1.0, 1.0, -1.0},
-  };
-  const int position_length = (sizeof (position)) / (sizeof (position[0]));
+void draw_cube(ta_parameter_writer& writer,
+               const mat4x4& trans,
+               const vec3& color);
 
-  struct quad {
-    int a, b, c, d;
-  };
+void draw_icosphere(ta_parameter_writer& writer,
+                    const mat4x4& trans,
+                    const vec3& color);
 
-  static const quad quads[] = {
-    {0, 1, 3, 2},
-    {2, 3, 7, 6},
-    {6, 7, 5, 4},
-    {4, 5, 1, 0},
-    {2, 6, 4, 0},
-    {7, 3, 1, 5}
-  };
+void draw_plane(ta_parameter_writer& writer,
+                const vec3& k,
+                const vec3& n,
+                const mat4x4& trans,
+                const vec3& color);
 
-  vec3 cache[position_length];
-  for (int i = 0; i < position_length; i++) {
-    cache[i] = screen_transform(trans * position[i]);
-  }
-
-  global_polygon_intensity(writer, color);
-
-  for (int i = 0; i < 6; i++) {
-    const vec3& ap = cache[quads[i].a];
-    const vec3& bp = cache[quads[i].b];
-    const vec3& cp = cache[quads[i].c];
-    const vec3& dp = cache[quads[i].d];
-
-    quad_type_2(writer,
-                ap,
-                bp,
-                cp,
-                dp,
-                1.0f);
-  }
-}
+void draw_halfspace(ta_parameter_writer& writer,
+                    const vec3& k,
+                    const vec3& n,
+                    const mat4x4& trans,
+                    const vec3& color);
diff --git a/src/platform/ta_parameter_presets.hpp b/src/platform/ta_parameter_presets.hpp
index 160abd7..6ce92c4 100644
--- a/src/platform/ta_parameter_presets.hpp
+++ b/src/platform/ta_parameter_presets.hpp
@@ -9,7 +9,8 @@
 #include "math/float_types.hpp"
 
 static inline void global_polygon_intensity(ta_parameter_writer& writer,
-                                            const vec3& color)
+                                            const vec3& color,
+                                            uint32_t culling_mode = isp_tsp_instruction_word::culling_mode::no_culling)
 {
   const uint32_t parameter_control_word = para_control::para_type::polygon_or_modifier_volume
                                         | para_control::list_type::opaque
@@ -17,7 +18,7 @@ static inline void global_polygon_intensity(ta_parameter_writer& writer,
                                         ;
 
   const uint32_t isp_tsp_instruction_word = isp_tsp_instruction_word::depth_compare_mode::greater_or_equal
-                                          | isp_tsp_instruction_word::culling_mode::no_culling;
+                                          | culling_mode;
 
   const uint32_t tsp_instruction_word = tsp_instruction_word::texture_shading_instruction::decal
                                       | tsp_instruction_word::src_alpha_instr::one
@@ -97,6 +98,28 @@ static inline void quad_type_2(ta_parameter_writer& writer,
                                         base_intensity);
 }
 
+static inline void tri_type_2(ta_parameter_writer& writer,
+                              const vec3& ap,
+                              const vec3& bp,
+                              const vec3& cp,
+                              float base_intensity)
+{
+  writer.append<ta_vertex_parameter::polygon_type_2>() =
+    ta_vertex_parameter::polygon_type_2(polygon_vertex_parameter_control_word(false),
+                                        ap.x, ap.y, ap.z,
+                                        base_intensity);
+
+  writer.append<ta_vertex_parameter::polygon_type_2>() =
+    ta_vertex_parameter::polygon_type_2(polygon_vertex_parameter_control_word(false),
+                                        bp.x, bp.y, bp.z,
+                                        base_intensity);
+
+  writer.append<ta_vertex_parameter::polygon_type_2>() =
+    ta_vertex_parameter::polygon_type_2(polygon_vertex_parameter_control_word(true),
+                                        cp.x, cp.y, cp.z,
+                                        base_intensity);
+}
+
 static inline void quad_type_3(ta_parameter_writer& writer,
                                const vec3& ap, const vec2& at,
                                const vec3& bp, const vec2& bt,