class Vector
{
  float x,y,z;
  float nx,ny,nz;
  float mag;
  Vector(float _x, float _y, float _z)
  {
    x=_x;
    y=_y;
    z=_z;
    
    //calculate the normalised vector (vector of length 1) .. doesn't make sense for using vector for co-ords, but useful when an actual vector
    mag=sqrt(x*x+y*y+z*z);
    nx=x/mag;
    ny=y/mag;
    nz=z/mag;
  }
  
  Vector divide(float val)
  {
    return new Vector(x/val,y/val,z/val);
  }
}

class Vertex
{
  float x,y,z;
  float nx,ny,nz;
  float mag;
  color c;
  Vertex(float _x, float _y, float _z, color _c)
  {
    x=_x;
    y=_y;
    z=_z;
    c=_c;
    
    //calculate the normalised vector (vector of length 1) .. doesn't make sense for using vector for co-ords, but useful when an actual vector
    mag=sqrt(x*x+y*y+z*z);
    nx=x/mag;
    ny=y/mag;
    nz=z/mag;
  }
  
  Vector divide(float val)
  {
    return new Vector(x/val,y/val,z/val);
  }
}
float dist(Vector a, Vector b)
{
  float i=a.x-b.x;
  float j=a.y-b.y;
  float k=a.z-b.z;
  return sqrt(i*i+j*j+k*k);
}

float sqdist(Vector a, Vector b)
{
  float i=a.x-b.x;
  float j=a.y-b.y;
  float k=a.z-b.z;
  return (i*i+j*j+k*k);
}


Vector dir(Vertex a, Vertex b)
{
  return new Vector(a.x-b.x,a.y-b.y,a.z-b.z);
}

Vector dir(Vector a, Vector b)
{
  return new Vector(a.x-b.x,a.y-b.y,a.z-b.z);
}

Vector ndir(Vector a, Vector b)
{
  return new Vector(a.nx-b.nx,a.ny-b.ny,a.nz-b.nz);
}

float Dot(Vector a, Vector b)
{
  return(a.nx*b.nx + a.ny*b.ny + a.nz * b.nz);
}

float mDot(Vector a, Vector b)
{
  return(a.x*b.x + a.y*b.y + a.z * b.z);
}

Vector nadd(Vector a, Vector b)
{
  return new Vector(a.nx+b.nx,a.ny+b.ny,a.nz+b.nz);
}

Vector cross(Vector a, Vector b)
{
  return new Vector(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y-a.y*b.x);
}

Vector VtoV(Vertex a)
{
  return new Vector(a.x,a.y,a.z);
}

Vertex VtoV(Vector a)
{
  return new Vertex(a.x,a.y,a.z,0);
}

Vector mul(Vector a, float d)
{
  return new Vector(a.x*d,a.y*d,a.z*d);
}

Vector div(Vector a, float d)
{
  return new Vector(a.x/d,a.y/d,a.z/d);
}

/*Vector mul(Vector a, float m)
{
  float d=sqrt(a.x*a.x+a.y*a.y+a.z*a.z);
  return new Vector(a.nx*d*m,a.ny*d*m,a.nz*d*m);
}*/

Vector add(Vector a, Vector b)
{
  return new Vector(a.x+b.x,a.y+b.y,a.z+b.z);
}

Vector sub(Vector a, Vector b) // take a from b e.g. r=b-a;
{
  return new Vector(b.x-a.x,b.y-a.y,b.z-a.z);
}

Vector rotateX(Vector a,float ang)
{
  float mz=a.nz*cos(ang)+a.ny*sin(ang);
  float my=a.ny*cos(ang)-a.nz*sin(ang);
  return new Vector(a.nx,my,mz);
}
Vector rotateY(Vector a,float ang)
{
  float mz=a.nz*cos(ang)-a.nx*sin(ang);
  float mx=a.nx*cos(ang)+a.nz*sin(ang);
  return new Vector(mx,a.ny,mz);
}
Vector rotateZ(Vector a,float ang)
{
  float mx=a.nx*cos(ang)-a.ny*sin(ang);
  float my=a.ny*cos(ang)+a.nx*sin(ang);
  return new Vector(mx,my,a.nz);
}

/*
   Determine whether or not the line segment p1,p2
   Intersects the 3 vertex facet bounded by pa,pb,pc
   Return true/false and the intersection point p

   The equation of the line is p = p1 + mu (p2 - p1)
   The equation of the plane is a x + b y + c z + d = 0
                                n.x x + n.y y + n.z z + d = 0
*/




boolean intersect(Vector a, Vector b, Vector c, Vector LS, Vector LE )
{
/*  if(sqdist(LS,a)>sq(SCALE*5))
    return false;*/
  Vector n=cross(dir(a,b),dir(b,c));
/*  if(Dot(n,L)==0)
    return false;*/
  n=new Vector(n.nx,n.ny,n.nz);
  float d=-(n.nx*a.x+n.ny*a.y+n.nz*a.z);
  float denom=n.nx*(LE.x-LS.x)+n.ny*(LE.y-LS.y)+n.nz*(LE.z-LS.z);
//  println("Denom:"+denom);
  if(denom<0.001 && denom>-0.001)
    return false;
  float mu=(d+n.nx*LS.x+n.ny*LS.y+n.nz*LS.z)/denom;
  if(mu>1 || mu<-1)
    return false;
  Vector p=new Vector(LS.x-(mu*(LE.x-LS.x)),LS.y-(mu*(LE.y-LS.y)),LS.z-(mu*(LE.z-LS.z)));
  Vector pa1=dir(a,p);
  Vector pa2=dir(b,p);
  Vector pa3=dir(c,p);
  float a1=Dot(pa1,pa2);
  float a2=Dot(pa2,pa3);
  float a3=Dot(pa1,pa3);
  float total=acos(a1)+acos(a2)+acos(a3);
//  println("Total="+total);
  if(total<TWO_PI-0.01 || total>TWO_PI+0.01)
    return false;
  if(sqdist(p,a)+sqdist(p,b)+sqdist(p,c)>sqdist(a,b)+sqdist(b,c)+sqdist(a,c))
    return false;
//  if((total-TWO_PI)>0.00001)
//    return false;
  return true;
}

Vector intersectPoint(Vector a, Vector b, Vector c, Vector LS, Vector LE )
{
  Vector n=cross(dir(a,b),dir(b,c));
  /*  if(Dot(n,L)==0)
    return false;*/
  n=new Vector(n.nx,n.ny,n.nz);
  float d=-(n.nx*a.x+n.ny*a.y+n.nz*a.z);
  float denom=n.nx*(LE.x-LS.x)+n.ny*(LE.y-LS.y)+n.nz*(LE.z-LS.z);
//  println("Denom:"+denom);
  if(denom<0.001 && denom>-0.001)
    return new Vector(0,0,0);
  float mu=(d+n.nx*LS.x+n.ny*LS.y+n.nz*LS.z)/denom;
  
  Vector p=new Vector(LS.x-(mu*(LE.x-LS.x)),LS.y-(mu*(LE.y-LS.y)),LS.z-(mu*(LE.z-LS.z)));
  Vector pa1=dir(a,p);
  Vector pa2=dir(b,p);
  Vector pa3=dir(c,p);
  float a1=Dot(pa1,pa2);
  float a2=Dot(pa2,pa3);
  float a3=Dot(pa1,pa3);
  float total=acos(a1)+acos(a2)+acos(a3);
//  println("Total="+total);
  if(total<TWO_PI-0.1 || total>TWO_PI+0.1)
    return new Vector(0,0,0);
  if(sqdist(p,a)+sqdist(p,b)+sqdist(p,c)>sqdist(a,b)+sqdist(b,c)+sqdist(a,c))
    return new Vector(0,0,0);
//  if((total-TWO_PI)>0.00001)
//    return false;
  return p;
}

float topnavg(float[] a, int num)
{
  if(a.length==0)
    return 0;
  if(num>=a.length)
  {
    float tot=0;
    for(int i=0;i<a.length;i++)
    {
      tot+=a[i];
    }
    return tot/(float)a.length;
  }
  else
  {
    float tot=0;
    a=sort(a);
    for(int i=0;i<num;i++)
    {
      tot+=a[a.length-1-i];
    }
    return tot/(float)num;
  }
}

Vector[] topn(Vector[] a, int num)
{
  if(a.length==0)
    return a;
  if(num>=a.length)
  {
    return a;
  }
  else
  {
    boolean[] used=new boolean[a.length];
    for(int i=0;i<a.length;i++)
      used[i]=false;
    float tmp=-1000;
    Vector[] res=new Vector[num];
    for(int j=0;j<num;j++)
    {
      tmp=-1000;
      int high=-1;
      for(int i=0;i<a.length;i++)
      {
//        println("if("+a[i].z+">"+tmp
        if(a[i].z>tmp && used[i]==false)
        {
          high=i;
          tmp=a[i].z;
        }
      }
      if(high==-1)
        println("FUCKED!");
      used[high]=true;
      res[j]=a[high];
    }
    return res;
  }

}

void translate(Vector v)
{
  translate(v.x,v.y,v.z);
}

void printVector(Vector v)
{
  println("Vector: "+v.x+" "+v.y+" "+v.z);
}

void printVector(String pre, Vector v)
{
  println(pre+"Vector: "+v.x+" "+v.y+" "+v.z);
}


Vector rotate(Vector v, Vector _axis,float ang)
{
  Vector axis=new Vector(_axis.nx,_axis.ny,_axis.nz);
  Vector vnorm=new Vector(v.nx,v.ny,v.nz);
/*Steps to rotate a vector:
-Calculate the Perpendicular component, multiply it by the cosine of the angle you are trying to rotate through
-Calculate the cross product between the vector you are trying to rotate about and the vector you are rotating, multiply it by the sine of the angle
-Add the results together
-Add the component of the vector you are trying to rotate that is parallel to the vector you are rotating about*/
//  printVector("axis",axis);
//  printVector("v",v);
  float _parallel=mDot(axis,v);
//  println("_parallel "+_parallel);
  Vector parallel=mul(axis,_parallel);
  Vector perp=sub(parallel,v);
/*  println("ang:"+ang);
  printVector("axis",axis);
  printVector("vnorm",vnorm);
  printVector("parallel",parallel);
  printVector("perp",perp);*/
  Vector Cross=cross(v,axis);
/*  Cross.x=Cross.nx;
  Cross.y=Cross.ny;
  Cross.z=Cross.nz;  */
  
  Vector res=add(parallel,add(mul(Cross,sin(-ang)),mul(perp,cos(-ang))));
  
  
/*  Vector res=perp;
  res=mul(res,cos(ang));
  Vector tmp=mul(Cross,sin(ang));
  res=add(res,tmp);
  res=add(res,perp);*/
  

//  printVector("res",res);
  return res;

}