/*****************************
 *  Vec class and functions  *
 *****************************/

class Vec {
  double x=0,y=0,z=0;
  
  Vec() {
  }

  Vec(double _x, double _y, double _z) {
    x=_x;
    y=_y;
    z=_z;
  }
  
  Vec(Vec _v) {
    x=_v.x; 
    y=_v.y; 
    z=_v.z;
  }
  
  void setVec(Vec _v) {
    x=_v.x; 
    y=_v.y; 
    z=_v.z;
  }
  
  void setVec(double _x, double _y, double _z) {
    x=_x;
    y=_y;
    z=_z;
  }
  
  double len() {
    return Math.sqrt(x*x+y*y+z*z);
  }
  
  double lenSQ() {
    return (x*x+y*y+z*z);
  }
  
  void gravitateTo(Vec _to, double _mgd, double _fFac) {
    double minGravDist2=_mgd*_mgd;
    double dx=_to.x-x;
    double dy=_to.y-y;
    double dz=_to.z-z;
    double d2=dx*dx+dy*dy+dz*dz;
    if (d2<minGravDist2) {d2=minGravDist2;}
    //double d=Math.sqrt(d2);
    double F=_fFac/d2;
    add(new Vec(dx*F,dy*F,dz*F));
  }

  void add(Vec _v) {
    x+=_v.x;
    y+=_v.y;
    z+=_v.z;
  }
  
  void add(double _x, double _y, double _z) {
    x+=_x;
    y+=_y;
    z+=_z;
  }

  void sub(Vec _v) {
    x-=_v.x;
    y-=_v.y;
    z-=_v.z;
  }

  void mul(double p) {
    x*=p;
    y*=p;
    z*=p;
  }
  
  void div(double p) {
    x/=p;
    y/=p;
    z/=p;
  }

  void negX() {
    x=-x;
  }

  void negY() {
    y=-y;
  }

  void negZ() {
    z=-z;
  }

  void neg() {
    negX();
    negY();
    negZ();
  }
  
  void normalize() {
    double l=len();
    if (l!=0) { // if not nullvector
      div(l);
    }
  }
  
  Vec getNormalized() {
    double l=len();
    if (l!=0) {
      Vec out=new Vec(this);
      out.div(l);
      return out;
    } else {
      return new Vec();
    }
  }
  
  void setLen(double ml) {
    Vec out=new Vec(this);
    double l=len();
    if (l!=0) {
      double fac=ml/len();
      x*=fac;
      y*=fac;
      z*=fac;
    }
  }
  
  void toAvLen(double ml) {
    Vec out=new Vec(this);
    double fac=ml/((ml+len())/2.0);
    x*=fac;
    y*=fac;
    z*=fac;
  }

  void rotX(double rd) {
    double SIN=Math.sin(rd); 
    double COS=Math.cos(rd);
    double yn=y*COS-z*SIN;
    double zn=z*COS+y*SIN;
    y=yn;
    z=zn;
  }

  void rotY(double rd) {
    double SIN=Math.sin(rd);
    double COS=Math.cos(rd); 
    double xn=x*COS-z*SIN; 
    double zn=z*COS+x*SIN;
    x=xn;
    z=zn;
  }

  void rotZ(double rd) {
    double SIN=Math.sin(rd);
    double COS=Math.cos(rd); 
    double xn=x*COS-y*SIN; 
    double yn=y*COS+x*SIN;
    x=xn;
    y=yn;
  }

  void rot(double xrd, double yrd, double zrd) {
    rotX(xrd);
    rotY(yrd);
    rotZ(zrd);
  }
  
  boolean isNullVec() {
    if (x==0&&y==0&&z==0) {return true;} else {return false;}
  }
  
  void constrX(double mi, double ma) {
    double ra=ma-mi;
    while (x<mi) x+=ra;
    while (x>=ma) x-=ra;
  }
  void constrY(double mi, double ma) {
    double ra=ma-mi;
    while (y<mi) y+=ra;
    while (y>=ma) y-=ra;
  }
}




Vec vecAdd(Vec a, Vec b) {
  Vec out=new Vec(a);
  out.add(b);
  return out;
}

Vec vecSub(Vec a, Vec b) {
  Vec out=new Vec(a);
  out.sub(b);
  return out;
}

Vec vecMul(Vec a, double b) {
  Vec out=new Vec(a);
  out.mul(b);
  return out;
}

Vec vecDiv(Vec a, double b) {
  Vec out=new Vec(a);
  out.div(b);
  return out;
}

double vecLen(Vec a) {
  return a.len();
}

Vec vecRotY(Vec a, double rd) {
  Vec out=new Vec(a);
  out.rotY(rd);
  return out;
}

Vec rndDirVec(double r) {
  Vec out=new Vec(0,0,r);
  out.rotX(random(-PI/2.0,PI));
  out.rotZ(random(0,2*PI));
  return out;
}

Vec rndPlusMinVec(double s) {
  Vec out=new Vec(-s+2*s*Math.random(),-s+2*s*Math.random(),-s+2*s*Math.random());
  return out;
}

void vecSwap(Vec v1, Vec v2) {
  Vec t=new Vec(v1);
  v1.setVec(v2);
  v2.setVec(t);
}

/*****************************
 * adapted 3d functions to Vec
 *****************************/

void vecTranslate(Vec v) {
  translate((float)v.x,(float)v.y,(float)v.z);
}

void vecVertex(Vec v) {
  vertex((float)v.x,(float)v.y,(float)v.z);
}

void vecLine(Vec a, Vec b) {
  line((float)a.x,(float)a.y,(float)a.z,(float)b.x,(float)b.y,(float)b.z);
}

void vecRect(Vec a, Vec b) {
  rect((float)a.x,(float)a.y,(float)b.x,(float)b.y);
}

void vecCamera(Vec eye, Vec cen, Vec upaxis) {
  camera((float)eye.x,(float)eye.y,(float)eye.z,(float)cen.x,(float)cen.y,(float)cen.z,(float)upaxis.x,(float)upaxis.y,(float)upaxis.z);
}