Processing Point Tracking

//The code I've been using in Processing to track points
//Kudos if you can reverse engineer it.
//Mostly by Johnathon "zalo" Selstad

import cl.eye.*;
import oscP5.*;
import netP5.*;
import processing.serial.*;
import java.awt.*;

OscP5 oscP5;

NetAddress myBroadcastLocation;

class TPoint {
  public float x, y, z;
  TPoint (float x1, float y1, float z1) {
    x = x1;
    y = y1;
    z = z1;
  }
  PVector Pos() {
    return new PVector((x/z), (y/z));
  }
}

float[] track = new float[4];

// Camera Variables
int numCams;
CLCamera myCameras[] = new CLCamera[2];
PImage myImages[] = new PImage[2];
int cameraWidth = 320;
int cameraHeight = 240;
int cameraRate = 125;
int thresh = 40;
ArrayList<TPoint> Blobs = new ArrayList<TPoint>();

void DetectBlob(int x, int y) {
  Boolean found = true;
  for (TPoint Blub : Blobs) {
    if (dist(Blub.Pos().x, Blub.Pos().y, x, y)<sqrt(Blub.z/PI)*1.4+15) {
      found = false;
      Blub.x += x;
      Blub.y += y;
      Blub.z++;
    }
  }
  if (found) {
    Blobs.add(new TPoint(x, y, 1));
  }
}

void Vision(int i) {
  // Loop through every pixel in the image.
  for (int y = 1; y < myImages[i].height-1; y++) { // Skip top and bottom edges
    for (int x = 1; x < myImages[i].width-1; x++) { // Skip left and right edges
      int Pos = y*myImages[i].width + x;
      if (brightness(myImages[i].pixels[Pos])>thresh) {
        DetectBlob(x, y);
      }
    }
  }
  // State that there are changes to edgeImg.pixels[]
  myImages[i].updatePixels();
}


boolean setupCameras() {
  println("Getting number of cameras");
  // Checks available cameras
  numCams = CLCamera.cameraCount();
  println("Found " + numCams + " cameras");
  if (numCams == 0)  return false;
  // create cameras and start capture
  for (int i = 0; i < numCams; i++)
  {
    // Prints Unique Identifier per camera
    println("Camera " + (i+1) + " UUID " + CLCamera.cameraUUID(i));
    // New camera instance per camera
    myCameras[i] = new CLCamera(this);
    // ----------------------(i, CLEYE_GRAYSCALE/COLOR, CLEYE_QVGA/VGA, Framerate)
    myCameras[i].createCamera(i, CLCamera.CLEYE_GRAYSCALE, CLCamera.CLEYE_QVGA, cameraRate);
    // Starts camera captures
    myCameras[i].startCamera();
    //myCameras[i].setCameraParam(CLCamera.CLEYE_AUTO_GAIN, 1);
    //myCameras[i].setCameraParam(CLCamera.CLEYE_AUTO_EXPOSURE, 1);
    //myCameras[i].setCameraParam(CLCamera.CLEYE_AUTO_WHITEBALANCE, 1);
    myImages[i] = createImage(cameraWidth, cameraHeight, RGB);
  }
  // resize the output window
  size(320, 240);
  println("Complete Initializing Camera");
  return true;
}

void sendTrack() {
  OscMessage myOscMessage = new OscMessage("/trackingData");
  myOscMessage.add(track[0]);
  myOscMessage.add(track[1]);
  myOscMessage.add(track[2]);
  myOscMessage.add(track[3]);
  oscP5.send(myOscMessage, myBroadcastLocation);
  println("Sent Points - X1: "+floor(track[0])+" Y1: "+floor(track[1])+" X2: "+floor(track[2])+" Y2: "+floor(track[3]));
}

void oscEvent(OscMessage theOscMessage) {
  /* get and print the address pattern and the typetag of the received OscMessage */
  println("### received an osc message with addrpattern "+theOscMessage.addrPattern()+" and typetag "+theOscMessage.typetag());
  theOscMessage.print();
}

void setup() {
  //CLCamera.loadLibrary("C:/CLEyeMulticam.dll");
  frameRate(cameraRate);
  noFill();
  if (!setupCameras()) exit();

  oscP5 = new OscP5(this,12000);
  myBroadcastLocation = new NetAddress("127.0.0.1",8000);
}

void draw() {
  background(0);
  // Loops through available cameras and updates
  for (int i = 0; i < numCams; i++) {
    myCameras[i].getCameraFrame(myImages[i].pixels, (i==0) ? 1000 : 0);
    Vision(i);
    image(myImages[i], cameraWidth*i, 0);
    for (TPoint Blub : Blobs) {
      text("POINT: "+floor(Blub.Pos().x)+", "+floor(Blub.Pos().y), (Blub.Pos().x)-30, (Blub.Pos().y)-(Blub.z/30));
      stroke(200, 0, 0);
      ellipse(Blub.Pos().x+(cameraWidth*i), Blub.Pos().y, (sqrt(Blub.z/PI)*1)*2, (sqrt(Blub.z/PI))*2);
    }
    if(Blobs.size()==2){
      line(Blobs.get(0).Pos().x,Blobs.get(0).Pos().y,Blobs.get(1).Pos().x,Blobs.get(1).Pos().y);
      line(((Blobs.get(0).Pos().x+Blobs.get(1).Pos().x)/2),((Blobs.get(0).Pos().y+Blobs.get(1).Pos().y)/2),((Blobs.get(0).Pos().x+Blobs.get(1).Pos().x)/2)-((Blobs.get(0).Pos().y-Blobs.get(1).Pos().y)/2),((Blobs.get(0).Pos().y+Blobs.get(1).Pos().y)/2)+((Blobs.get(0).Pos().x-Blobs.get(1).Pos().x)/2));
      line(((Blobs.get(0).Pos().x+Blobs.get(1).Pos().x)/2),((Blobs.get(0).Pos().y+Blobs.get(1).Pos().y)/2),((Blobs.get(0).Pos().x+Blobs.get(1).Pos().x)/2)+((Blobs.get(0).Pos().y-Blobs.get(1).Pos().y)/2),((Blobs.get(0).Pos().y+Blobs.get(1).Pos().y)/2)-((Blobs.get(0).Pos().x-Blobs.get(1).Pos().x)/2  ));
      track[0] = Blobs.get(0).Pos().x;
      track[1] = Blobs.get(0).Pos().y;
      track[2] = Blobs.get(1).Pos().x;
      track[3] = Blobs.get(1).Pos().y;
      sendTrack();
    }
    Blobs = new ArrayList<TPoint>();
  }
  stroke(255);
}