Triangle clipping(processing sketch)

import java.util.Vector;

PVector[] triangle = new PVector[3];
PVector frustum_tl = new PVector(-1, -1); 
PVector frustum_br = new PVector(1, 1);
final float screen_scale = 0.25;


void setup() {
  size(640, 640);
  triangle[0] = new PVector(-1.0, -1.0);
  triangle[1] = new PVector(0.0, 1.0);
  triangle[2] = new PVector(1.0, 0.0);
}

PVector to_screen_coords(PVector p)
{
  PVector scale = new PVector((float)width/2*screen_scale, (float)height/2*screen_scale);
  PVector offset = new PVector((float)width/2, (float)height/2);
  return new PVector(p.x*scale.x + offset.x, p.y*scale.y + offset.y);
}

PVector to_normalized_coords(PVector p)
{
  PVector scale = new PVector((float)width/2*screen_scale, (float)height/2*screen_scale);
  PVector offset = new PVector((float)width/2, (float)height/2);
  return new PVector((p.x-offset.x)/scale.x , (p.y - offset.y)/scale.y);
}

PVector get_screen_center()
{
  return new PVector((float)width/2, (float)height/2);
}

void draw_point(int x, int y, float size)
{
  ellipse(x, y, size, size);
}

float t_horizontal(float x1, float x2, float N) 
{
  return (N - x1)/(x2 - x1);
}

float y_per_t(float y1, float y2, float t)
{
  return t*(y2-y1) + y1;
}

float t_vertical(float y1, float y2, float N) 
{
  return (N - y1)/(y2 - y1);
}

float x_per_t(float x1, float x2, float t)
{
  return t*(x2-x1) + x1;
}

/* Rolled-up clipping function using loops */
float[][] clip_triangle(float[][] triangle) {
  int       src_size = 3;
  int       dst_idx = 0;
  float[][] src = new float[8][2]; // max 7, + null
  float[][] dst = new float[8][2];
  src[0] = triangle[0];
  src[1] = triangle[1];
  src[2] = triangle[2];
  for (int p = 0; p < 4; p++) { // -X, -Y, X, Y
    dst_idx = 0;
    float N = p > 1 ? 1.0 : -1.0;
    int u = p % 2;              // X, Y, X, Y
    int v = (u + 1) % 2;        // Y, X, Y, X
    for (int s = 0; s < src_size; s++) {
      final float[] a = src[s];
      final float[] b = src[(s+1) % src_size];
      final Boolean a_in = a[u]*N <= N*N;
      final Boolean b_in = b[u]*N <= N*N;
      if(a_in || b_in)
      {
        if(a_in)
          dst[dst_idx++] = a;
        float t = (N - a[u])/(b[u] - a[u]);
        if(t > 0.0f && t < 1.0f) 
        {
          float[] c = new float[2];
          c[u] = N;
          c[v] = t*(b[v]-a[v]) + a[v];
          dst[dst_idx++] = c;
        }
      }
    }
    float[][] src_old = src;
    src = dst;
    dst = src_old;
    src_size = dst_idx;
    dst_idx = 0;
  }
  src[src_size] = null;
  return src;
}

float[][] to_float_array(PVector[] vs)
{
  float[][] out = new float[vs.length][2];
  for (int i = 0; i < vs.length; i++) {
    out[i][0] = vs[i].x;
    out[i][1] = vs[i].y;
  }
  return out;
}

PVector[] from_float_array(float[][] vs)
{
  Vector<PVector> out_v = new Vector<PVector>();
  for (int i = 0; i < vs.length; i++) {
    if(vs[i] == null)
      break;
    PVector v = new PVector();
    v.x = vs[i][0];
    v.y = vs[i][1];
    out_v.add(v);
  }
  PVector[] out = new PVector[out_v.size()]; 
  out_v.toArray(out);
  return out;
}

/* Unrolled clipping function */
PVector[] clip_triangle_unrolled(PVector[] p)
{
  Vector<PVector> c = new Vector<PVector>();

  // Y = -1.0
  for(int i=0; i<p.length; i++)
  {
    PVector a, b;
    a = p[i];
    b = p[(i+1)%p.length ];
    if(a.y >= -1.0)
    {
      c.add(a);
      float t = t_vertical(a.y, b.y, -1.0);
      if(t>0 && t<1.0)
      {
        float x = x_per_t(a.x, b.x, t);
        c.add(new PVector(x,-1.0));
      }
    } else if(b.y > -1.0)
    {
      float t = t_vertical(a.y, b.y, -1.0);
      if(t>0 && t<1.0)
      {
        float x = x_per_t(a.x, b.x, t);
        c.add(new PVector(x,-1.0));
      }
    }
  }
 
  // Y = 1.0
  Vector<PVector> c2 = new Vector<PVector>();
  for(int i=0; i<c.size(); i++)
  {
    PVector a, b;
    a = c.get(i);
    b = c.get((i+1)%c.size());
    if(a.y <= 1.0)
    {
      c2.add(a);
      float t = t_vertical(a.y, b.y, 1.0);
      if(t>0 && t<1.0)
      {
        float x = x_per_t(a.x, b.x, t);
        c2.add(new PVector(x,1.0));
      }
    } else if(b.y < 1.0)
    {
      float t = t_vertical(a.y, b.y, 1.0);
      if(t>0 && t<1.0)
      {
        float x = x_per_t(a.x, b.x, t);
        c2.add(new PVector(x,1.0));
      }
    }
  }

  // X = -1.0
  c = new Vector<PVector>();
  for(int i=0; i<c2.size(); i++)
  {
    PVector a, b;
    a = c2.get(i);
    b = c2.get((i+1)%c2.size());
    if(a.x >= -1.0)
    {
      c.add(a);
      float t = t_horizontal(a.x, b.x, -1.0);
      if(t>0 && t<1.0)
      {
        float y = y_per_t(a.y, b.y, t);
        c.add(new PVector(-1.0, y));
      }
    } else if(b.x > -1.0)
    {
      float t = t_horizontal(a.x, b.x, -1.0);
      if(t>0 && t<1.0)
      {
        float y = y_per_t(a.y, b.y, t);
        c.add(new PVector(-1.0, y));
      }
    }
  }

  // X = 1.0
  c2 = new Vector<PVector>();
  for(int i=0; i<c.size(); i++)
  {
    PVector a, b;
    a = c.get(i);
    b = c.get((i+1)%c.size());
    if(a.x <= 1.0)
    {
      c2.add(a);
      float t = t_horizontal(a.x, b.x, 1.0);
      if(t>0 && t<1.0)
      {
        float y = y_per_t(a.y, b.y, t);
        c2.add(new PVector(1.0, y));
      }
    } else if(b.x < 1.0)
    {
      float t = t_horizontal(a.x, b.x, 1.0);
      if(t>0 && t<1.0)
      {
        float y = y_per_t(a.y, b.y, t);
        c2.add(new PVector(1.0, y));
      }
    }
  }

  PVector[] out_arr = new PVector[c2.size()];
  c2.toArray(out_arr);

  return out_arr;
}

void draw_polygon(PVector[] p)
{
  for(int i=0; i<p.length; i++)
  {
    PVector a, b;
    a = to_screen_coords(p[i]);
    b = to_screen_coords(p[(i+1)%p.length ]);
    fill(255, 0, 0);
    stroke(255, 0, 0);
    line(a.x, a.y, b.x, b.y);
    draw_point((int)a.x, (int)a.y, 5);
    text(i,(int)a.x, (int)a.y);
    draw_point((int)b.x, (int)b.y, 5);
  }
}

void draw_triangle()
{
  PVector[] ts = {to_screen_coords(triangle[0]),
                  to_screen_coords(triangle[1]),
                  to_screen_coords(triangle[2])};

  stroke(255);
  line(ts[0].x, ts[0].y, ts[1].x, ts[1].y);
  line(ts[1].x, ts[1].y, ts[2].x, ts[2].y);
  line(ts[2].x, ts[2].y, ts[0].x, ts[0].y);
  
  float point_size = 5.0;
  fill(255);
  draw_point((int)ts[0].x, (int)ts[0].y, point_size);
  draw_point((int)ts[1].x, (int)ts[1].y, point_size);
  draw_point((int)ts[2].x, (int)ts[2].y, point_size);
}

void draw_frustum()
{
  PVector tl = to_screen_coords(frustum_tl);
  PVector br = to_screen_coords(frustum_br);
  float width = br.x - tl.x;
  float height = br.y - tl.y; 
  noFill();
  stroke(255);
  rect(tl.x, tl.y, width, height);
}

PVector grabbed = null;

PVector get_closest_point()
{
  PVector pointer_n = to_normalized_coords(new PVector(mouseX, mouseY));
  float epsilon = 0.1;
  for (int i=0; i < 3; i++) {
    float dx = abs(triangle[i].x - pointer_n.x);
    float dy = abs(triangle[i].y - pointer_n.y);
    if((sqrt(dx*dx + dy*dy)) < epsilon)
    {
      return triangle[i];
    }
  }
  
  return null;
}

void draw() { 
  background(51);
  
  if(mousePressed == true && grabbed == null)
  {
    grabbed = get_closest_point();
  }

  if(grabbed != null)
  {
    PVector pointer_n = to_normalized_coords(new PVector(mouseX, mouseY));
    grabbed.x = pointer_n.x;
    grabbed.y = pointer_n.y;  
  }

  if(mousePressed == false)
  {
    grabbed = null;
  }
  
  draw_triangle();
  draw_frustum();

  // unrolled original version
  draw_polygon(clip_triangle_unrolled(triangle));

  // rolled-up version
  //draw_polygon(from_float_array(clip_triangle(to_float_array(triangle))));
}