//**************************************
// 3D POINTS AND VECTOR CLASSES
// Written by Jarek Rossignac,  June 2006
//*************************************
class pt { float x=0,y=0,z=0; 
  pt () {x=0;y=0;z=0;};
  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 add(pt P) {x += P.x; y += P.y; z += P.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 sub(pt P) {x -= P.x; y -= P.y; z -= P.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 div(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]; }
  
  void rotateXBy(float a, pt P) {float dy=y-P.y, dz=z-P.z, c=cos(a), s=sin(a); y=P.y+c*dy+s*dz; z=P.z-s*dy+c*dz; };
  void rotateYBy(float a, pt P) {float dz=z-P.z, dx=x-P.x, c=cos(a), s=sin(a); z=P.z+c*dz+s*dx; x=P.x-s*dz+c*dx; };
  void rotateZBy(float a, pt P) {float dx=x-P.x, dy=y-P.y, c=cos(a), s=sin(a); x=P.x+c*dx+s*dy; y=P.y-s*dx+c*dy; };
  
  void rotateXBy(float a) {float dy=y, dz=z, c=cos(a), s=sin(a); y=c*dy+s*dz; z=-s*dy+c*dz; };
  void rotateYBy(float a) {float dz=z, dx=x, c=cos(a), s=sin(a); z=c*dz+s*dx; x=-s*dz+c*dx; };
  void rotateZBy(float a) {float dx=x, dy=y, c=cos(a), s=sin(a); x=c*dx+s*dy; y=-s*dx+c*dy; };
  void applyMat(float m11,float m12, float m13, float m21, float m22, float m23, float m31, float m32, float m33)
  {
    float nx = m11*x+m12*y+m13*z;
    float ny = m21*x+m22*y+m23*z;
    float nz = m31*x+m32*y+m33*z;
    x=nx; y=ny; z=nz;
  }
  void transformPtToY(pt P)
  {
    pt ptStart = new pt(0, 0, 0);
    pt ptEnd = P;
   
    vec vecOrg = ptStart.vecTo(ptEnd);
    vecOrg.makeUnit();
   
    float a = vecOrg.x;
    float b = vecOrg.y;
    float c = vecOrg.z;
    float d = sqrt(b*b+c*c);
     
    applyMat(1.0, 0.0, 0.0, 0.0, c/d, -b/d, 0.0, b/d, c/d);
    applyMat(d, 0.0, -a, 0.0, 1.0, 0.0, a, 0.0, d);
    rotateXBy(PI/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);};
  vec cross(vec v) { return new vec((y*v.z - z*v.y), (z*v.x - x*v.z), (x*v.y - y*v.x)); }
  float dot(vec v) { return (x*v.x + y*v.y + z*v.z); }

  } ;
  
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); };