//**************************************
// 3D POINTS AND VECTOR CLASSES
// Written by Jarek Rossignac,  June 2006
//*************************************
class pt { float x=0,y=0,z=0; 
  pt () {};
  pt (float px, float py, float pz) {x = px; y = py; z = pz;};
  pt make() {return(new pt(x,y,z));};
  void show(float r) { pushMatrix(); translate(x,y,z); sphere(r); popMatrix();}; 
  void show(int r) { pushMatrix(); translate(x,y,z); sphere(r); popMatrix();}; 
  void showLineTo (pt P) {line(x,y,z,P.x,P.y,P.z); }; 
  void setToPoint(pt P) { x = P.x; y = P.y; z = P.z;}; 
  void setTo(pt P) { x = P.x; y = P.y; z = P.z;}; 
  void setTo (float px, float py, float pz) {x = px; y = py; z = pz;}; 
  void setToMouse() { x = mouseX; y = mouseY; }; 
  void write() {println("("+x+","+y+","+z+")");};
  void addVec(vec V) {x += V.x; y += V.y; z += V.z;};
  void addScaledVec(float s, vec V) {x += s*V.x; y += s*V.y; z += s*V.z;};
  void moveTowards(float s, pt P) {x += s*(P.x-x); y += s*(P.y-y); z += s*(P.z-z);};
  void subVec(vec V) {x -= V.x; y -= V.y; z -= V.z;};
  void vert() {vertex(x,y,z);};
  void vertext(float u, float v) {vertex(x,y,z,u,v);};
  boolean isInWindow() {return(((x<0)||(x>width)||(y<0)||(y>height)));};
  void label(String s, vec D) {text(s, x+D.x, y+D.y, z+D.z);  };
  vec vecTo(pt P) {return(new vec(P.x-x,P.y-y,P.z-z)); };
  float disTo(pt P) {return(sqrt( sq(P.x-x)+sq(P.y-y)+sq(P.z-z) )); };
  vec vecToMid(pt P, pt Q) {return(new vec((P.x+Q.x)/2.0-x,(P.y+Q.y)/2.0-y,(P.z+Q.z)/2.0-z )); };
  vec vecToProp (pt B, pt D) {
      vec CB = this.vecTo(B); float LCB = CB.norm();
      vec CD = this.vecTo(D); float LCD = CD.norm();
      vec U = CB.make();
      vec V = CD.make(); V.sub(U); V.mul(LCB/(LCB+LCD));
      U.add(V);
      return(U);  
      };  
  void addPt(pt P) {x+=P.x; y+=P.y; z+=P.z;};
  void subPt(pt P) {x-=P.x; y-=P.y; z-=P.z; };
  void mul(float f) {x*=f; y*=f; z*=f;};
  void pers(float d) { y=d*y/(d+z); x=d*x/(d+z); z=d*z/(d+z); };
  void inverserPers(float d) { y=d*y/(d-z); x=d*x/(d-z); z=d*z/(d-z); };
  boolean coplanar (pt A, pt B, pt C) {return(abs(tetVol(this,A,B,C))<0.0001);};
  boolean cw (pt A, pt B, pt C) {return(tetVol(this,A,B,C)>0.0001);};
  void pick() { float[] mousePos=getMouse3D(); x=mousePos[0]; y=mousePos[1]; z=mousePos[2]; }
  void setToViewpoint() { float[] mousePos=getMouse3D(); x=mousePos[0]; y=mousePos[1]; z=mousePos[2]; }
  } ;
 
class vec { float x,y,z; 
  vec (float px, float py, float pz) {x = px; y = py; z = pz;};
  void setTo (float px, float py, float pz) {x = px; y = py; z = pz;}; 
  vec make() {return(new vec(x,y,z));};
  void setToVec(vec V) { x = V.x; y = V.y; z = V.z;}; 
  void setTo(vec V) { x = V.x; y = V.y; z = V.z;}; 
  void show (pt P) {line(P.x,P.y, P.z,P.x+x,P.y+y,P.z+z); }; 
  void add(vec V) {x += V.x; y += V.y; z += V.z;};
  void addScaled(float m, vec V) {x += m*V.x; y += m*V.y; z += m*V.z;};
  void sub(vec V) {x -= V.x; y -= V.y; z -= V.z;};
  void mul(float m) {x *= m; y *= m; z *= m;};
  void div(float m) {x /= m; y /= m; z /= m;};
  void write() {println("("+x+","+y+","+z+")");};
  float norm() {return(sqrt(sq(x)+sq(y)+sq(z)));}; 
  void makeUnit() {float n=this.norm(); if (n>0.0001) {this.div(n);};};
  void back() {x= -x; y= -y; z= -z;};
  boolean coplanar (vec V, vec W) {return(abs(mixed(this,V,W))<0.0001);};
  boolean cw (vec U, vec V, vec W) {return(mixed(this,V,W)>0.0001);};
  } ;
  
void showTriangle(pt A, pt B, pt C) {beginShape(); A.vert(); B.vert(); C.vert(); endShape(CLOSE);}
vec triNormalFromPts(pt A, pt B, pt C) {vec N = cross(A.vecTo(B),A.vecTo(C));  return(N); };
float tetVol (pt A, pt B, pt C, pt D) { return(dot(triNormalFromPts(A,B,C),A.vecTo(D))); };
vec midVec(vec U, vec V) {return(new vec((U.x+V.x)/2,(U.y+V.y)/2,(U.z+V.z)/2)); };
float dot(vec U, vec V) {return(U.x*V.x+U.y*V.y+U.z*V.z); };
vec cross(vec U, vec V) {return(new vec( U.y*V.z-U.z*V.y, U.z*V.x-U.x*V.z, U.x*V.y-U.y*V.x )); };
float mixed(vec U, vec V, vec W) {return(dot(cross(U,V),W)); };
pt midPt(pt A, pt B) {return(new pt((A.x+B.x)/2 , (A.y+B.y)/2, (A.z+B.z)/2 )); };
pt triCenterFromPts(pt A, pt B, pt C) {return(new pt((A.x+B.x+C.x)/3 , (A.y+B.y+C.y)/3, (A.z+B.z+C.z)/3 )); };
float tetVolume(pt E, pt A, pt B, pt C) {vec EA=E.vecTo(A); vec EB=E.vecTo(B); vec EC=E.vecTo(C); float v=mixed(EA,EB,EC); return(v/6); };
boolean rayHitTri(pt E, pt M, pt A, pt B, pt C) { boolean s, sA, sB, sC; 
  s=(tetVolume(E,A,B,C)>0);  sA=(tetVolume(E,M,B,C)>0); sB=(tetVolume(E,A,M,C)>0);  sC=(tetVolume(E,A,B,M)>0); 
  return( (s==sA) && (s==sB) && (s==sC) );};

float rayDistTriPlane(pt E, pt M, pt A, pt B, pt C) { 
   vec AE = A.vecTo(E); vec AC = A.vecTo(C); vec AB = A.vecTo(B); vec EM = E.vecTo(M); 
   float s = - mixed(AE,AC,AB) / mixed(EM,AC,AB);
   return(s); };