// Simple line math object
// I just find all this vector stuff invaluable - one call to updateLine and I know where I'm facing

static class Line{
  Point a, b;
  float vx, vy, dx, dy, lx, ly, rx, ry, length, invLength, theta, speed;
  // Constructor
  Line(Point a, Point b, float speed){
    this.a = a;
    this.b = b;
    this.speed = speed;
    updateLine();
  }
  // Refresh values of normals
  void updateLine(){
    vx = b.x - a.x;
    vy = b.y - a.y;
    invLength = Util.invSqrt(vx * vx + vy * vy);
    length = 1.0f/invLength;
    //len = sqrt(vx * vx + vy * vy);
    if(length > 0){
      dx = vx * invLength;
      dy = vy * invLength;
      //dx = vx/len;
      //dy = vy/len;
    } else {
      dx = 0.0f;
      dy = 0.0f;
    }
    rx = -dy;
    ry = dx;
    lx = dy;
    ly = -dx;
  }
  // Spin line around the axis of point a
  void rotateA(float angle){
    angle += getTheta();
    theta = angle;
    b.x = a.x + cos(angle) * length;
    b.y = a.y + sin(angle) * length;
    updateLine();
  }
  // Spin line around the axis of point b
  void rotateB(float angle){
    angle += getTheta();
    theta = angle;
    a.x = b.x + cos(angle) * length;
    a.y = b.y + sin(angle) * length;
    updateLine();
  }
  // Get the angle of the line
  float getTheta(){
    theta = atan2(vy, vx);
    return theta;
  }
  String toString(){
    return "a:("+a.x+","+a.y+") b:("+b.x+","+b.y+")";
  }
  void draw(PGraphics g){
    g.strokeWeight(5 *speed);
    g.stroke(255*speed);
    g.line(a.x, a.y, b.x, b.y);
  }
  // Return a point between a and b
  Point lerp(float t){
    return(new Point(a.x + (b.x - a.x) * t, a.y + (b.y - a.y) * t));
  }
  // Some methods I find easier to read as static:
  // Get the dot product from the left hand normal
  static float perP(Line a, Line b){
    return a.vx * b.vy - a.vy * b.vx;
  }
  // Get a normalised left hand dot product
  static float normalPerP(Line a, Line b){
    return a.dx * b.dy - a.dy * b.dx;
  }
  // Get the Point at which two lines intersect
  static Point intersectionPoint(Line a, Line b){
    Line c = new Line(new Point(0,0), new Point(b.a.x-a.a.x, b.a.y-a.a.y), 1);
    float t = perP(c, b)/perP(a, b);
    return new Point(a.a.x+a.vx*t, a.a.y+a.vy*t);
  }
  // Do two lines actually cross each other?
  static boolean intersects(Line a, Line b){
    Line c = new Line(new Point(0,0), new Point(b.a.x-a.a.x, b.a.y-a.a.y), 1);
    float perP0 = perP(c, b);
    float perP1 = perP(a, b);
    float t0 = perP0/perP1;
    float t1 = perP1/perP0;
    return t0 >= 0 && t0 <= 1 && t1 >= 0 && t1 <= 1;
  }
  // Return the dot product of two vectors (lines)
  static float dot(Line a, Line b){
    return a.vx * b.vx + a.vy * b.vy;
  }
}
static class Util{
  // interpolate between two angles using the value "t" as a multiplier (t is between 0 and 1)
  static float thetaLerp(float a, float b, float t){
    a += (abs(b-a) > PI) ? ((a < b) ? TWO_PI : -TWO_PI) : 0;
    return lerp(a, b, t);
  }
  // Original C code by Chris Lomont <http://www.lomont.org/Math/Papers/2003/InvSqrt.pdf>
  static float invSqrt(float x){
    float xhalf = 0.5f*x;
    int i = Float.floatToIntBits(x); // get bits for floating value
    i = 0x5f375a86- (i>>1); // gives initial guess y0
    x = Float.intBitsToFloat(i); // convert bits back to float
    x = x*(1.5f-xhalf*x*x); // Newton step, repeating increases accuracy
    return x;
  }
}