determining the position of a point based on signal strength from 4-detectors with vectors

signal_strength_with_vectors.pde

// see the output of this sketch here:
// http://www.flickr.com/photos/unavoidablegrain/6584746313/

// our starting points
PVector p1 = new PVector(240, 90);
PVector p2 = new PVector(417, 152);
PVector p3 = new PVector(55, 211);
PVector p4 = new PVector(250, 400);

// vectors that will hold center-to-starting point
PVector v1;
PVector v2;
PVector v3;
PVector v4;

// vectors that will hold normalized direction
// to each starting point
PVector n1;
PVector n2;
PVector n3;
PVector n4;

// our center point
PVector center = new PVector();

// signal strength to each point
float s1 = 61;
float s2 = 73;
float s3 = 30;
float s4 = 20;


void setup() {
  size(500, 500);
  center.x = (p1.x + p2.x + p3.x + p4.x)/4;
  center.y = (p1.y + p2.y + p3.y + p4.y)/4;

  v1 = PVector.sub(p1, center);
  v2 = PVector.sub(p2, center);
  v3 = PVector.sub(p3, center);
  v4 = PVector.sub(p4, center);

  smooth();
}

void draw() {
  background(255);

  // draw four points
  fill(200);
  noStroke();
  ellipse(p1.x, p1.y, 10, 10);
  ellipse(p2.x, p2.y, 10, 10);
  ellipse(p3.x, p3.y, 10, 10);
  ellipse(p4.x, p4.y, 10, 10);

  // display vectors from center
  //  to original points
  stroke(0);
  strokeWeight(1);
  drawFromCenter(v1);
  drawFromCenter(v2);
  drawFromCenter(v3);
  drawFromCenter(v4);

  // normalize is destructive
  // so we work on a different vector
  n1 = new PVector(v1.x, v1.y);
  n2 = new PVector(v2.x, v2.y);
  n3 = new PVector(v3.x, v3.y);
  n4 = new PVector(v4.x, v4.y);

  // normalize to reduce vectors to direction
  n1.normalize();
  n2.normalize();
  n3.normalize();
  n4.normalize();

  // scale them based on signal strength
  n1.mult(s1);
  n2.mult(s2);
  n3.mult(s3);
  n4.mult(s4);


  // draw the scaled normalized vectors
  strokeWeight(4);
  stroke(255, 0, 0);
  fill(255, 0, 0);
  drawFromCenter(n1);
  drawFromCenter(n2);
  drawFromCenter(n3);
  drawFromCenter(n4);

  strokeWeight(2);
  stroke(0, 255, 0);
  
  // draw vectors as you add them up
  // to find the final position
  drawFromVector(n1, center);
  
  PVector finalPosition = PVector.add(n1, center);
  drawFromVector(n2, finalPosition);
  
  finalPosition = PVector.add(n2, finalPosition);
  drawFromVector(n3, finalPosition);
  
  finalPosition = PVector.add(n3, finalPosition);
  drawFromVector(n4,finalPosition);
  
  // draw the final position
  finalPosition.add(n4);
  fill(0,255,0);
  ellipse(finalPosition.x, finalPosition.y + 5, 10,10);
 

  fill(255);
  stroke(0);
  ellipse(center.x, center.y, 10, 10);
}

void drawFromCenter(PVector v) {
  drawFromVector(v, center);
}

// don't worry about how this works,
// the magic is in heading2D(), which is built-in
// it's just for drawing
void drawFromVector(PVector v, PVector o) {
  int arrowsize = 6;

  PVector endPoint = PVector.add(v,o);

  pushMatrix();
    translate(o.x, o.y);
    rotate(v.heading2D());
    float len = v.mag();

    line(0, 0, len, 0);
    line(len, 0, len-arrowsize, +arrowsize/2);
    line(len, 0, len-arrowsize, -arrowsize/2);
  popMatrix();
}