/*
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOUSE. See the GNU
 * General Public License for more details.
 *
 * You should have recieved a copy of the GNU General Public License
 * along with this program; if not write to the Free Software
 * Foundation, inc., 59 Temple Place, Suite 330, Boston MA 02111-1307
 * USA
 */
package j3d;
import java.awt.*;
import java.awt.geom.*;
import java.awt.image.*;
import java.io.Serializable;
/**
 * Insert the type's description here.
 *
 * @author: Yuriy Mikhaylovskiy
 */

public class lib3D implements Serializable{

  public static float[] PL3T(_Point p0, _Point p1, _Point p2){
    /*float[] a = new float[3];
    float[] b = new float[3];
    float[] c = new float[3];
    float[] p = new float[4];
    a[0]=p1.x;
    a[1]=p1.y;
    a[2]=p1.z;
    b[0]=p2.x;
    b[1]=p2.y;
    b[2]=p2.z;
    c[0]=p3.x;
    c[1]=p3.y;
    c[2]=p3.z;
    for(int i=0; i<0; i++){
      b[i]-=a[i];
      c[i]-=a[i];
    }
    p[0]=b[1]*c[2]-c[1]*b[2];
    p[1]=-b[0]*c[2]+c[1]*b[2];
    p[2]=b[0]*c[1]-c[0]*b[2];
    p[3]=-a[0]*p[0]+a[1]*p[1]-a[2]*p[2];*/
    float[] p = new float[4];
    float a1 = (p1.y-p0.y)*(p2.z-p0.z);
    float a2 = (p1.z-p0.z)*(p2.x-p0.x);
    float a3 = (p1.x-p0.x)*(p2.y-p0.y);
    float a4 = (p1.y-p0.y)*(p2.x-p0.x);
    float a5 = (p1.z-p0.z)*(p2.y-p0.y);
    float a6 = (p1.x-p0.x)*(p2.z-p0.z);
    p[0]=a1-a5;
    p[1]=a2-a6;
    p[2]=a3-a4;
    p[3]=p0.y*a6+p0.x*a5+p0.z*a4-p0.z*a3-p0.y*a2-p0.x*a1;
    return p;
  }

  public static float[] PL3T_LINE(_Point p1, _Point p2, _Point p3, _Point t1, _Point t2){
    float[] p = PL3T(p1,p2,p3);
    float[] xyz = new float[3];
    if(t1.x==t2.x && t1.z==t2.z){
      xyz[0]=t1.x;
      xyz[1]=(-p[0]*t1.x-p[2]*t1.z-p[3])/p[1];
      xyz[2]=t1.z;
    }else if(t1.y==t2.y && t1.z==t2.z){
      xyz[0]=(-p[1]*t1.y-p[2]*t1.z-p[3])/p[0];
      xyz[1]=t1.y;
      xyz[2]=t1.z;
    }else if(t1.x==t2.x && t1.y==t2.y){
      xyz[0]=t1.x;
      xyz[1]=t1.y;
      xyz[2]=(-p[0]*t1.x-p[1]*t1.y-p[3])/p[2];
    }
    if(Float.isNaN(xyz[0]) || Float.isNaN(xyz[1]) || Float.isNaN(xyz[2])) {return null;}
    float xmin = Math.min(p1.x,p2.x);
    float xmax = Math.max(p1.x,p2.x);
    float ymin = Math.min(p1.y,p2.y);
    float ymax = Math.max(p1.y,p2.y);
    float zmin = Math.min(p1.z,p2.z);
    float zmax = Math.max(p1.z,p2.z);
    xmin = Math.min(xmin,p3.x);
    xmax = Math.max(xmax,p3.x);
    ymin = Math.min(ymin,p3.y);
    ymax = Math.max(ymax,p3.y);
    zmin = Math.min(zmin,p3.z);
    zmax = Math.max(zmax,p3.z);
    if(!(xmin<=xyz[0] && xmax>=xyz[0]) || !(ymin<=xyz[1] && ymax>=xyz[1]) || !(zmin<=xyz[2] && zmax>=xyz[2])) return null;
    GeneralPath gp_xy = new GeneralPath();
    gp_xy.reset();
    gp_xy.moveTo(p1.x,p1.y);
    gp_xy.lineTo(p2.x,p2.y);
    gp_xy.lineTo(p3.x,p3.y);
    gp_xy.closePath();
    if(gp_xy.contains(xyz[0],xyz[1])) return xyz;
    else if(Line2D.Float.ptLineDist(p1.x,p1.y,p2.x,p2.y,p3.x,p3.y)!=0 && xmin<=xyz[0] && xmax>=xyz[0] && ymin<=xyz[1] && ymax>=xyz[1]){
      if((p1.x==p2.x || p1.y==p2.y) && Line2D.Float.ptLineDist(p1.x,p1.y,p2.x,p2.y,xyz[0],xyz[1])==0)return xyz;
      if((p1.x==p3.x || p1.y==p3.y) && Line2D.Float.ptLineDist(p1.x,p1.y,p3.x,p3.y,xyz[0],xyz[1])==0)return xyz;
      if((p3.x==p2.x || p3.y==p2.y) && Line2D.Float.ptLineDist(p2.x,p2.y,p3.x,p3.y,xyz[0],xyz[1])==0)return xyz;
    }
    GeneralPath gp_xz = new GeneralPath();
    gp_xz.reset();
    gp_xz.moveTo(p1.x,p1.z);
    gp_xz.lineTo(p2.x,p2.z);
    gp_xz.lineTo(p3.x,p3.z);
    gp_xz.closePath();
    if(gp_xz.contains(xyz[0],xyz[2])) return xyz;
    else if(Line2D.Float.ptLineDist(p1.x,p1.z,p2.x,p2.z,p3.x,p3.z)!=0 && xmin<=xyz[0] && xmax>=xyz[0] && zmin<=xyz[2] && zmax>=xyz[2]){
      if((p1.x==p2.x || p1.z==p2.z) && Line2D.Float.ptLineDist(p1.x,p1.z,p2.x,p2.z,xyz[0],xyz[2])==0)return xyz;
      if((p1.x==p3.x || p1.z==p3.z) && Line2D.Float.ptLineDist(p1.x,p1.z,p3.x,p3.z,xyz[0],xyz[2])==0)return xyz;
      if((p3.x==p2.x || p3.z==p2.z) && Line2D.Float.ptLineDist(p2.x,p2.z,p3.x,p3.z,xyz[0],xyz[2])==0)return xyz;
    }
    GeneralPath gp_yz = new GeneralPath();
    gp_yz.reset();
    gp_yz.moveTo(p1.y,p1.z);
    gp_yz.lineTo(p2.y,p2.z);
    gp_yz.lineTo(p3.y,p3.z);
    gp_yz.closePath();
    if(gp_yz.contains(xyz[1],xyz[2])) return xyz;
    else if(Line2D.Float.ptLineDist(p1.y,p1.z,p2.y,p2.z,p3.y,p3.z)!=0 && ymin<=xyz[1] && ymax>=xyz[1] && zmin<=xyz[2] && zmax>=xyz[2]){
      if((p1.y==p2.y || p1.z==p2.z) && Line2D.Float.ptLineDist(p1.y,p1.z,p2.y,p2.z,xyz[1],xyz[2])==0)return xyz;
      if((p1.y==p3.y || p1.z==p3.z) && Line2D.Float.ptLineDist(p1.y,p1.z,p3.y,p3.z,xyz[1],xyz[2])==0)return xyz;
      if((p3.y==p2.y || p3.z==p2.z) && Line2D.Float.ptLineDist(p2.y,p2.z,p3.y,p3.z,xyz[1],xyz[2])==0)return xyz;
    }
    return null;
  }

  public static float[] PL3T_LINE2(_Point p1, _Point p2, _Point p3, _Point t1, _Point t2){
    float[] p = PL3T(p1,p2,p3);
    float[] xyz = new float[3];
    if(t1.x==t2.x && t1.z==t2.z){
      xyz[0]=t1.x;
      xyz[1]=(-p[0]*t1.x-p[2]*t1.z-p[3])/p[1];
      xyz[2]=t1.z;
    }else if(t1.y==t2.y && t1.z==t2.z){
      xyz[0]=(-p[1]*t1.y-p[2]*t1.z-p[3])/p[0];
      xyz[1]=t1.y;
      xyz[2]=t1.z;
    }else if(t1.x==t2.x && t1.y==t2.y){
      xyz[0]=t1.x;
      xyz[1]=t1.y;
      xyz[2]=(-p[0]*t1.x-p[1]*t1.y-p[3])/p[2];
    }
    if(Float.isNaN(xyz[0]) || Float.isNaN(xyz[1]) || Float.isNaN(xyz[2])) {return null;}
    float xmin = Math.min(p1.x,p2.x);
    float xmax = Math.max(p1.x,p2.x);
    float ymin = Math.min(p1.y,p2.y);
    float ymax = Math.max(p1.y,p2.y);
    float zmin = Math.min(p1.z,p2.z);
    float zmax = Math.max(p1.z,p2.z);
    xmin = Math.min(xmin,p3.x);
    xmax = Math.max(xmax,p3.x);
    ymin = Math.min(ymin,p3.y);
    ymax = Math.max(ymax,p3.y);
    zmin = Math.min(zmin,p3.z);
    zmax = Math.max(zmax,p3.z);
    if(!(xmin<=xyz[0] && xmax>=xyz[0]) || !(ymin<=xyz[1] && ymax>=xyz[1]) || !(zmin<=xyz[2] && zmax>=xyz[2])) return null;
    if(Line2D.Float.ptLineDist(p1.x,p1.y,p2.x,p2.y,p3.x,p3.y)!=0 && xmin<=xyz[0] && xmax>=xyz[0] && ymin<=xyz[1] && ymax>=xyz[1]){
      if((p1.x==p2.x || p1.y==p2.y) && Line2D.Float.ptLineDist(p1.x,p1.y,p2.x,p2.y,xyz[0],xyz[1])==0)return xyz;
      if((p1.x==p3.x || p1.y==p3.y) && Line2D.Float.ptLineDist(p1.x,p1.y,p3.x,p3.y,xyz[0],xyz[1])==0)return xyz;
      if((p3.x==p2.x || p3.y==p2.y) && Line2D.Float.ptLineDist(p2.x,p2.y,p3.x,p3.y,xyz[0],xyz[1])==0)return xyz;
      if(Line2D.Float.linesIntersect(p1.x,p1.y,p2.x,p2.y,p3.x,p3.y,xyz[0],xyz[1]))return null;
      if(Line2D.Float.linesIntersect(p1.x,p1.y,p3.x,p3.y,p2.x,p2.y,xyz[0],xyz[1]))return null;
      if(Line2D.Float.linesIntersect(p3.x,p3.y,p2.x,p2.y,p1.x,p1.y,xyz[0],xyz[1]))return null;
      return xyz;
    }
    if(Line2D.Float.ptLineDist(p1.x,p1.z,p2.x,p2.z,p3.x,p3.z)!=0 && xmin<=xyz[0] && xmax>=xyz[0] && zmin<=xyz[2] && zmax>=xyz[2]){
      if((p1.x==p2.x || p1.z==p2.z) && Line2D.Float.ptLineDist(p1.x,p1.z,p2.x,p2.z,xyz[0],xyz[2])==0)return xyz;
      if((p1.x==p3.x || p1.z==p3.z) && Line2D.Float.ptLineDist(p1.x,p1.z,p3.x,p3.z,xyz[0],xyz[2])==0)return xyz;
      if((p3.x==p2.x || p3.z==p2.z) && Line2D.Float.ptLineDist(p2.x,p2.z,p3.x,p3.z,xyz[0],xyz[2])==0)return xyz;
      if(Line2D.Float.linesIntersect(p1.x,p1.z,p2.x,p2.z,p3.x,p3.z,xyz[0],xyz[2]))return null;
      if(Line2D.Float.linesIntersect(p1.x,p1.z,p3.x,p3.z,p2.x,p2.z,xyz[0],xyz[2]))return null;
      if(Line2D.Float.linesIntersect(p3.x,p3.z,p2.x,p2.z,p1.x,p1.z,xyz[0],xyz[2]))return null;
      return xyz;
    }
    if(Line2D.Float.ptLineDist(p1.y,p1.z,p2.y,p2.z,p3.y,p3.z)!=0 && ymin<=xyz[1] && ymax>=xyz[1] && zmin<=xyz[2] && zmax>=xyz[2]){
      if((p1.y==p2.y || p1.z==p2.z) && Line2D.Float.ptLineDist(p1.y,p1.z,p2.y,p2.z,xyz[1],xyz[2])==0)return xyz;
      if((p1.y==p3.y || p1.z==p3.z) && Line2D.Float.ptLineDist(p1.y,p1.z,p3.y,p3.z,xyz[1],xyz[2])==0)return xyz;
      if((p3.y==p2.y || p3.z==p2.z) && Line2D.Float.ptLineDist(p2.y,p2.z,p3.y,p3.z,xyz[1],xyz[2])==0)return xyz;
      if(Line2D.Float.linesIntersect(p1.y,p1.z,p2.y,p2.z,p3.y,p3.z,xyz[1],xyz[2]))return null;
      if(Line2D.Float.linesIntersect(p1.y,p1.z,p3.y,p3.z,p2.y,p2.z,xyz[1],xyz[2]))return null;
      if(Line2D.Float.linesIntersect(p3.y,p3.z,p2.y,p2.z,p1.y,p1.z,xyz[1],xyz[2]))return null;
      return xyz;
    }
    return null;
  }

  public static float[] PL3T_LINE1(_Point p1, _Point p2, _Point p3, _Point t1, _Point t2){
    float[] p = PL3T(p1,p2,p3);
    float[] xyz = new float[3];
    if(t1.x==t2.x && t1.z==t2.z){
      xyz[0]=t1.x;
      xyz[1]=(-p[0]*t1.x-p[2]*t1.z-p[3])/p[1];
      xyz[2]=t1.z;
    }else if(t1.y==t2.y && t1.z==t2.z){
      xyz[0]=(-p[1]*t1.y-p[2]*t1.z-p[3])/p[0];
      xyz[1]=t1.y;
      xyz[2]=t1.z;
    }else if(t1.x==t2.x && t1.y==t2.y){
      xyz[0]=t1.x;
      xyz[1]=t1.y;
      xyz[2]=(-p[0]*t1.x-p[1]*t1.y-p[3])/p[2];
    }
    float xmin = Math.min(p1.x,p2.x);
    float xmax = Math.max(p1.x,p2.x);
    float ymin = Math.min(p1.y,p2.y);
    float ymax = Math.max(p1.y,p2.y);
    float zmin = Math.min(p1.z,p2.z);
    float zmax = Math.max(p1.z,p2.z);
    xmin = Math.min(xmin,p3.x);
    xmax = Math.max(xmax,p3.x);
    ymin = Math.min(ymin,p3.y);
    ymax = Math.max(ymax,p3.y);
    zmin = Math.min(zmin,p3.z);
    zmax = Math.max(zmax,p3.z);
    float dx = Math.abs(xmax-xmin);
    float dy = Math.abs(ymax-ymin);
    float dz = Math.abs(zmax-zmin);
    double p1x,p1y,p2x,p2y,p3x,p3y,x1,y1,x2,y2,x3,y3;
    double sin,cos,r;
    if(((dx<dy && dy<dz) || (dx<dz && dz<dy)) && t1.x!=t2.x){
      p1x=p1.y-xyz[1];
      p1y=p1.z-xyz[2];
      p2x=p2.y-xyz[1];
      p2y=p2.z-xyz[2];
      p3x=p3.y-xyz[1];
      p3y=p3.z-xyz[2];
      r=Math.sqrt(p1x*p1x+p1y*p1y);
      cos=p1x/r;
      sin=p1y/r;
      x1=p1x*cos+p1y*sin;
      x2=p2x*cos+p2y*sin;
      x3=p3x*cos+p3y*sin;
      if((x1==x2 && x1==0) || (x1==x3 && x1==0) || (x3==x2 && x3==0)) return xyz;
      if((x1<=0 && x2<=0 && x3<=0) || (x1>=0 && x2>=0 && x3>=0)) return null;
      r=Math.sqrt(p2x*p2x+p2y*p2y);
      cos=p2x/r;
      sin=p2y/r;
      x1=p1x*cos+p1y*sin;
      x2=p2x*cos+p2y*sin;
      x3=p3x*cos+p3y*sin;
      if((x1==x2 && x1==0) || (x1==x3 && x1==0) || (x3==x2 && x3==0)) return xyz;
      if((x1<=0 && x2<=0 && x3<=0) || (x1>=0 && x2>=0 && x3>=0)) return null;
      r=Math.sqrt(p3x*p3x+p3y*p3y);
      cos=p3x/r;
      sin=p3y/r;
      x1=p1x*cos+p1y*sin;
      x2=p2x*cos+p2y*sin;
      x3=p3x*cos+p3y*sin;
      if((x1==x2 && x1==0) || (x1==x3 && x1==0) || (x3==x2 && x3==0)) return xyz;
      if((x1<=0 && x2<=0 && x3<=0) || (x1>=0 && x2>=0 && x3>=0)) return null;
    }else if(((dy<dx && dx<dz) || (dy<dz && dz<dx)) && t1.y!=t2.y){
      p1x=p1.y-xyz[0];
      p1y=p1.z-xyz[2];
      p2x=p2.y-xyz[0];
      p2y=p2.z-xyz[2];
      p3x=p3.y-xyz[0];
      p3y=p3.z-xyz[2];
      r=Math.sqrt(p1x*p1x+p1y*p1y);
      cos=p1x/r;
      sin=p1y/r;
      x1=p1x*cos+p1y*sin;
      x2=p2x*cos+p2y*sin;
      x3=p3x*cos+p3y*sin;
      if((x1==x2 && x1==0) || (x1==x3 && x1==0) || (x3==x2 && x3==0)) return xyz;
      if((x1<=0 && x2<=0 && x3<=0) || (x1>=0 && x2>=0 && x3>=0)) return null;
      r=Math.sqrt(p2x*p2x+p2y*p2y);
      cos=p2x/r;
      sin=p2y/r;
      x1=p1x*cos+p1y*sin;
      x2=p2x*cos+p2y*sin;
      x3=p3x*cos+p3y*sin;
      if((x1==x2 && x1==0) || (x1==x3 && x1==0) || (x3==x2 && x3==0)) return xyz;
      if((x1<=0 && x2<=0 && x3<=0) || (x1>=0 && x2>=0 && x3>=0)) return null;
      r=Math.sqrt(p3x*p3x+p3y*p3y);
      cos=p3x/r;
      sin=p3y/r;
      x1=p1x*cos+p1y*sin;
      x2=p2x*cos+p2y*sin;
      x3=p3x*cos+p3y*sin;
      if((x1==x2 && x1==0) || (x1==x3 && x1==0) || (x3==x2 && x3==0)) return xyz;
      if((x1<=0 && x2<=0 && x3<=0) || (x1>=0 && x2>=0 && x3>=0)) return null;
    }else if(((dz<dx && dx<dy) || (dz<dy && dy<dx)) && t1.z!=t2.z){
      p1x=p1.x-xyz[0];
      p1y=p1.y-xyz[1];
      p2x=p2.x-xyz[0];
      p2y=p2.y-xyz[1];
      p3x=p3.x-xyz[0];
      p3y=p3.y-xyz[1];
      r=Math.sqrt(p1x*p1x+p1y*p1y);
      cos=p1x/r;
      sin=p1y/r;
      x1=p1x*cos+p1y*sin;
      x2=p2x*cos+p2y*sin;
      x3=p3x*cos+p3y*sin;
      if((x1==x2 && x1==0) || (x1==x3 && x1==0) || (x3==x2 && x3==0)) return xyz;
      if((x1<=0 && x2<=0 && x3<=0) || (x1>=0 && x2>=0 && x3>=0)) return null;
      r=Math.sqrt(p2x*p2x+p2y*p2y);
      cos=p2x/r;
      sin=p2y/r;
      x1=p1x*cos+p1y*sin;
      x2=p2x*cos+p2y*sin;
      x3=p3x*cos+p3y*sin;
      if((x1==x2 && x1==0) || (x1==x3 && x1==0) || (x3==x2 && x3==0)) return xyz;
      if((x1<=0 && x2<=0 && x3<=0) || (x1>=0 && x2>=0 && x3>=0)) return null;
      r=Math.sqrt(p3x*p3x+p3y*p3y);
      cos=p3x/r;
      sin=p3y/r;
      x1=p1x*cos+p1y*sin;
      x2=p2x*cos+p2y*sin;
      x3=p3x*cos+p3y*sin;
      if((x1==x2 && x1==0) || (x1==x3 && x1==0) || (x3==x2 && x3==0)) return xyz;
      if((x1<=0 && x2<=0 && x3<=0) || (x1>=0 && x2>=0 && x3>=0)) return null;
    }
    return xyz;
  }

 
  //***************************************************************
  // Various Nurb methods
  //***************************************************************

  
  /**
   * Generates a _Point on a NURBS surface determined by the internal curve coordinates u & v
   * 
   * @param n		Number of control points in U direction
   * @param p		Curve segment degree in U direction
   * @param U		Knot vector U
   * @param m		Number of control points in V direction
   * @param q		Curve segment degree in V direction
   * @param V		Knot vector V
   * @param Pw		Array of Control points (weighted values used)
   * @param u		Internal surface coordinate for the generated point in u direction
   * @param v		Internal surface coordinate for the generated point in v direction
   * @return		A _Point in 3D space on the surface
   */
  
  public static _Point surfacePoint(int n, int p, float[] U, int m, int q, float[] V, _CtrlPoint[][] Pw, float u, float v) {
      int uspan, vspan, k,l;
      float[] Nu = new float[p+1];
      float[] Nv = new float[q+1];
      _CtrlPoint[] temp = new _CtrlPoint[q+1];
      _CtrlPoint Sw;
      _Point S;
      
      uspan = findSpan(n-1,p,u,U);
      Nu = basisFuns(uspan,u,p,U);
      vspan = findSpan(m-1,q,v,V);
      Nv = basisFuns(vspan,v,q,V);
      
      for (l=0; l<=q; l++) {

          temp[l] = new _CtrlPoint(0,0,0,0,Color.BLUE);
          
          for (k=0; k<=p; k++) {
              temp[l].x += Nu[k]*Pw[uspan-p+k][vspan-q+l].x;
              temp[l].y += Nu[k]*Pw[uspan-p+k][vspan-q+l].y;
              temp[l].z += Nu[k]*Pw[uspan-p+k][vspan-q+l].z;
              temp[l].w += Nu[k]*Pw[uspan-p+k][vspan-q+l].w;
          }
          
      }    
      
      // Project to 3D view
      
      Sw = new _CtrlPoint(0,0,0,0,Color.BLUE);
      
      for (l=0; l<=q; l++) {
          Sw.x += Nv[l]*temp[l].x;
          Sw.y += Nv[l]*temp[l].y;
          Sw.z += Nv[l]*temp[l].z;
          Sw.w += Nv[l]*temp[l].w;
      }
      
      S = new _Point(false);
      
      S.x = Sw.x / Sw.w;
      S.y = Sw.y / Sw.w;
      S.z = Sw.z / Sw.w;
      
      return S;
  }
  
  
  
  /**
   * Generates a _Point in space set by the internal curve coordinate u
   * 
   * @param n		Number of control points
   * @param p		Curve segment degree
   * @param U		Knot vector
   * @param Pw		Vector of Control points (weighted values used)
   * @param u		Internal curve coordinate for the generated point
   * @return		A _Point in 3D space on the curve
   */
  
  public static _Point curvePoint(int n, int p, float[] U, _CtrlPoint[] Pw, float u) {
      int span,i;
      float[] N = new float[p+1];
      _Point C = new _Point(false);
      float Cw = 0;
      
      span = findSpan(n-1,p,u,U);
      N = basisFuns(span,u,p,U);
      
      for (i=0; i<=p; i++) {
          C.x += N[i]*Pw[span-p+i].x;
      	  C.y += N[i]*Pw[span-p+i].y;
          C.z += N[i]*Pw[span-p+i].z;
          Cw  += N[i]*Pw[span-p+i].w;
      }
      
      // Project to 3D view
      C.x = C.x / Cw;
      C.y = C.y / Cw;
      C.z = C.z / Cw;
      
      return C;
  }
  
  
  
/**
* This method determines the knot span index and returns it
* 
* @param n		set to nr_of_controlpoints -1
* @param p		Curve degree
* @param u		position on Nurb (usually between 0 and 1)
* @param U		Knot vector
* @return i 	Knot vector index for u
*/
  public static int findSpan(int n, int p, float u, float[] U) {
      int low, high, mid;
      
      if (u == U[n+1]) return(n);
      low = p;
      high = n+1;
      mid = (int)(low+high)/2;
      while (u < U[mid] || u >= U[mid+1]) {
          if (u < U[mid]) high = mid;
          	else low = mid;
          mid = (int)(low+high)/2;
      }
      return mid;
  }

  /**
   * This method computes the non-vanishing basis functions
   * @param i		Function index
   * @param u		Curve internal coordinate
   * @param p		Curve degree
   * @param U		Knot vector
   * @return		Basis function array
   */
  public static float[] basisFuns(int i, float u, int p, float[]U) {
      int j, r;
      float[] left = new float[p+1];
      float[] right = new float[p+1];
      float saved,temp;
      float[] N = new float[p+1];
      
      N[0] = 1.0f;
      for (j=1; j<=p; j++) {
          left[j] = u-U[i+1-j];
          right[j] = U[i+j]-u;
          saved = 0.0f;
          for(r=0; r<j; r++) {
              temp = N[r]/(right[r+1]+left[j-r]);
              N[r] = saved + right[r+1]*temp;
              saved = left[j-r]*temp;
          }
          N[j] = saved;
      }
      
      return N;
  }
  
  
  /**
   * This function calculates all non-zero basis functions and their derivatives
   * @param i		Basis function index
   * @param u		Point internal u coordinate
   * @param p		Curve degree
   * @param n		number of controlpoints
   * @param U		Knot vector
   * @return
   */
  public static float[][] dersBasisFuns(int i, float u, int p, int n, float[] U) {
      int j,k,r,s1,s2,rk,pk,j1,j2;

      float[][] ndu = new float[p+1][];
      for(j=0; j<=p; j++)
          ndu[j] = new float[p+1];

      float[][] ders = new float[p+1][];
      for(j=0; j<=n; j++)
          ders[j] = new float[p+1];

      float[] left = new float[p+1];
      float[] right = new float[p+1];
      float saved,temp,d;

      float[][] a = new float[p+1][];
      for(j=0; j<n+1; j++)
          a[j] = new float[n+1];

      ndu[0][0] = 1.0f;
      for(j=1; j<=p; j++) {
          left[j] = u - U[i+1-j];
          right[j] = U[i+j] - u;
          saved = 0.0f;
          
          for(r=0; r<j; r++) {
              // lower triangle
              ndu[j][r] = right[r+1] + left[j-r];
              temp = ndu[r][j-1]/ndu[j][r];
              // upper triangle
              ndu[r][j] = saved + right[r+1] * temp;
              saved = left[j-r] * temp;
          }
          
          ndu[j][j] = saved;
              
          }

      // Load the basis functions
      for(j=0; j<=p; j++)
          ders[0][j] = ndu[j][p];
      
      // This section computes the derivatives (Eq. [2.9])
      for(r=0; r<=p; r++) {
          s1 = 0;
          s2 = 1;
          a[0][0] = 1.0f;

          // Loop to compute kth derivative
          for(k=1; k<=n; k++) {
              d = 0.0f;
              rk = r - k;
              pk = p - k;

              if(r >= k) {
                  a[s2][0] = a[s1][0]/ndu[pk + 1][rk];
                  d = a[s2][0] * ndu[rk][pk];
              }
              
              if(rk >= -1)
                  j1 = 1;
              else
                  j1 = -rk;
              
              if(r - 1 <= pk)
                  j2 = k - 1;
              else
                  j2 = p - r;
              
              for(j = j1; j <= j2; j++) {
                  a[s2][j] = (a[s1][j] - a[s1][j-1]) / ndu[pk+1][rk+j];
                  d += a[s2][j] * ndu[rk+j][pk];
              }
              
              if(r <= pk) {
                  a[s2][k] = -a[s1][k-1] / ndu[pk+1][r];
                  d += a[s2][k] * ndu[r][pk];
              }
              
              ders[k][r] = d;

              // Switch rows
              j = s1;
              s1 = s2;
              s2 = j;
              
          }
      }
      
      // Multiply with the correct factors (eq [2.9])
      r = p;
      for (k=1; k<=n; k++) {
          for(j=0; j<=p; j++)
              ders[k][j] *= r;
          r *= (p-k);
      }

  return ders;
  }
  
  
  /**
   * This function computes the derivatives for a nurb curve. All derivatives from
   * 0 to d are calculated at point u on the curve. They are delivered as _CtrlPoints.
   * 
   * @param n		Number of controlpoints
   * @param p		Curve segment degree (order)
   * @param U		knot vector
   * @param P		Vector of controlpoints
   * @param u		position on curve which to calculate derivatives (usually 0 <= u <= 1)
   * @param d		highest derivative to calculate
   * @return		An array of derivatives from 0 to d, at point u
   */
  
  public static _CtrlPoint[] curveDerivsAlg1(int n, int p, float[] U,  _CtrlPoint[] P, float u, int d) {
      int du,k,span,j;
      _CtrlPoint[] CK;
      float[][] nders;
      
      CK = new _CtrlPoint[d+1];
      
      for (k=p+1; k<=d; k++) {
          CK[k] = new _CtrlPoint();
          CK[k].w = 0;
      }
      
      du = Math.min(d,p);
      span = findSpan(n-1,p,u,U);
      nders = dersBasisFuns(span,u,p,du,U);
      
      for (k=0; k<=du; k++) {
          CK[k] = new _CtrlPoint();
          CK[k].w = 0.0f;
          
          for (j=0; j<=p; j++) { 
              CK[k].x = CK[k].x + nders[k][j] * P[span-p+j].x;
              CK[k].y = CK[k].y + nders[k][j] * P[span-p+j].y;
              CK[k].z = CK[k].z + nders[k][j] * P[span-p+j].z;
              CK[k].w = CK[k].w + nders[k][j] * P[span-p+j].w;
          }
      }
      
      return CK;
  }
  
  /**
   * Calculates the curve derivative for a rational B-spline as an array
   * of _CtrlPoints where [0] is the curve point in space, [1] is the first 
   * derivative at that point and so on up to [d]
   * 
   * @param CKa		Derivatives for a normal B-spline as given by curveDerivsAlg1()
   * @param d		Max number of the derivative requested
   * @return		An array of _Points with derivative at point u in normal space
   */
  
  public static _Point[] ratCurveDerivs(_CtrlPoint[] CKa, int d) {
      int k,i;
      float vx,vy,vz,vw;
      _Point[] CK;
      
      CK = new _Point[d+1];
      for (i=0; i<d+1; i++)
          CK[i] = new _Point(false);
      
      for (k=0; k<=d; k++) {
          vx = CKa[k].x;
          vy = CKa[k].y;
          vz = CKa[k].z;

          for (i=1; i<=k; i++) {
              vx = vx - bin(k,i) * CKa[i].w * CK[k-i].x;
              vy = vy - bin(k,i) * CKa[i].w * CK[k-i].y;
              vz = vz - bin(k,i) * CKa[i].w * CK[k-i].z;
          }
          
          // Project to 3D space
          CK[k].x = vx / CKa[0].w;
          CK[k].y = vy / CKa[0].w;
          CK[k].z = vz / CKa[0].w;
      }
      
      return CK;
  }


//Compute logarithm of the gamma function
//Algorithm from 'Numerical Recipes in C, 2nd Edition' pg214.
private static float _gammaln(float xx)
{
 float x,y,tmp,ser;
 final float[] cof = {76.18009172947146f,-86.50532032291677f,
                         24.01409824083091f,-1.231739572450155f,
                         0.12086650973866179e-2f, -0.5395239384953e-5f};
 int j;
 y = x = xx;
 tmp = x + 5.5f;
 tmp -= (x+0.5f) * Math.log(tmp);
 ser = 1.000000000190015f;
 for (j=0; j<=5; j++) ser += cof[j]/++y;
 return -tmp+(float)Math.log(2.5066282746310005f*ser/x);
}

//computes ln(n!)
//Numerical Recipes in C
//Algorithm from 'Numerical Recipes in C, 2nd Edition' pg215.
private static float _factln(int n)
{
int ntop = 0;
float[] a = new float[101];

if (n <= 1) return 0.0f;
while (n > ntop)
{
 ++ntop;
 a[ntop] = _gammaln(ntop+1.0f);
}
return a[n];
}
  
  
  /**
   * This method calculates the binomial of (n over k)
   * 
   * @param n
   * @param k
   * @return	The binomial
   */
  private static float bin(int n, int k) {
      return (float)Math.floor(0.5+Math.exp(_factln(n)-_factln(k)-_factln(n-k)));  
      }

  
  /**
   * Projects a point onto a line (vector) and returns the projected position
   * 
   * @param start		Start point of vector
   * @param direction	The vector
   * @param point		The point to be projected
   * @return			The projected point
   */
  public static Vector3D pointToLine(Vector3D start, Vector3D direction, Vector3D point) {
      Vector3D result = new Vector3D();
      float a;
      
      result.sub(point,start);
      a = result.dot(result,direction);
      
      result.copy(direction);
      result.scale(a);
      result.add(start,result);
      
      return result;
  }
  
  
  
  
 //******************************************************************************* 
 // Other functions which perform various operations on Nurb curves and surfaces * 
 //*******************************************************************************
  
  /**
   * Creates one or more points which is the normal projection of the p_original point onto the c_target curve
   * 
   * @param p_original	Point to be projected
   * @param c_target		Curve the point is to be projected onto
   * @return				The projected point or points
   */
  public static _Point[] projectPointOntoCurve(_Point p_original, _NurbCurve c_target) {
      
      _Point[] new_point;
      
      return null;
  }
  
  
  /**
   * Creates one or more points which is the normal projection of the p_original point onto the s_target surface
   * 
   * @param p_original	Point to be projected
   * @param s_target		Surface the point is to be projected onto
   * @return				The projected point or points
   */
  public static _Point[] projectPointOntoSurface(_Point p_original, _NurbSurface s_target) {
      
      _Point[] new_point;
      
      return null;
  }

  
  /**
   * Creates one or more curves which is the normal projection of the c_original curve onto the s_target surface
   * 
   * @param c_original	Curve to be projected
   * @param s_target		Surface the curve is to be projected onto
   * @return				The projected curve or curves
   */
  public static _NurbCurve[] projectCurveOntoSurface(_NurbCurve c_original, _NurbSurface s_target) {
      
      _NurbCurve[] new_curve;
      
      return null;
  }

  
  /**
   * Creates two curves which are the result of breaking the c_target curve at the p_breaker point
   * 
   * @param c_target		Curve to be broken
   * @param p_breaker		Point (residing on curve) where the curve is to be broken
   * @return				Resulting curves after the curve has been broken
   */
  
  public static _NurbCurve[] breakCurve(_NurbCurve c_target, _Point p_breaker) {
      
      _NurbCurve[] new_curve;
      
      return null;
  }

  
  /**
   * Creates a curve which is the result of joining the connected c_original curves
   * 
   * @param c_original	Curves to be joined
   * @return			Resulting curve after joining
   */
  public static _NurbCurve joinCurves(_NurbCurve[] c_original) {
      
      _NurbCurve new_curve;
      
      return null;
  }

  
  /** 
   * Creates one or more points at the intersection points between two c_original curves
   * 
   * @param c_original	Curves to be intersected (always 2)
   * @return				Resulting point or points at intersection
   */
  public static _Point[] intersectCurves(_NurbCurve[] c_original) {
      
      _Point[] new_point;
      
      return null;
  }

  
  /**
   * Creates one or more curves at the intersection of two s_original surfaces
   * 
   * @param s_original	Surfaces to be intersected (always 2)
   * @return				Resulting curves after intersection
   */
  public static _NurbCurve[] intersectSurfaces(_NurbSurface[] s_original) {
      
      _NurbCurve[] new_curve;
      
      return null;
  }

  
  /**
   * Creates one or more points at the intersection of c_original curve to the s_original surface
   * @param c_original	Curve to be intersected with surface
   * @param s_original	Surface to be intersected with curve
   * @return				Resulting point or points at intersection
   */
  
  public static _Point[] intersectCurveWithSurface(_NurbCurve c_original, _NurbSurface s_original) {
      
      _Point[] new_point;
      
      return null;
  }

  
  /**
   * Creates a coon patch surface, defined by the three or four boundary curves
   * @param c_border Boundary curve
   * @return			Resulting surface
   */
  
  public static _NurbSurface createPatch(_NurbCurve[] c_border) {
      
      _NurbSurface new_patch;
      
      return null;
  }

  
  /**
   * Creates one or more points on the c_target curve, placed at equal interval
   * Selecting one point will put it in the middle of the curve.
   * Selecting zero points will give the endpoints of the curve.
   * 
   * @param c_target		Curve on which the points are to be determined
   * @param nr_of_points	Amount of points to be determined
   * @return				Resulting points
   */
  public static _Point[] createSpacedPoints(_NurbCurve c_target, int nr_of_points) {
      
      _Point[] new_point;
      
      return null;
  }
  
  
  /**
   * Calulates the intersection point of two lines represented by points and direction vectors
   * 
   * @param v_p1	Point 1
   * @param v_d1	Direction vector from this point
   * @param v_p2	Point 2
   * @param v_d2	Direction vector from this point
   * @return		The intersection point
   */
  public static Vector3D intersection(Vector3D p1, Vector3D v1, Vector3D p2, Vector3D v2) {
      float b,a;
      Vector3D coords = new Vector3D();
      Vector3D check = new Vector3D();

      // Solve in x/y plane
      b= (p2.y*v1.x - p1.y*v1.x - p2.x*v1.y + p1.x*v1.y) / (v2.x*v1.y - v2.y*v1.x);
      //a= (p2.x+b*v2.x-p1.x)/v1.x;

      // If failed, try y/x plane
      if (Float.isNaN(b)) {
        b= (p2.z*v1.y - p1.z*v1.y - p2.y*v1.z + p1.y*v1.z) / (v2.y*v1.z - v2.z*v1.y);
      	//a= (p2.y+b*v2.y-p1.y)/v1.y;
      }
      
      // If failed, try z/x plane
      if (Float.isNaN(b)) {
        b= (p2.x*v1.z - p1.x*v1.z - p2.z*v1.x + p1.z*v1.x) / (v2.z*v1.x - v2.x*v1.z);
        //a= (p2.z+b*v2.z-p1.z)/v1.z;
      }
      
      coords.copy(v2);
      coords.scale(b);
      coords.add(p2,coords);
      
      // Verify and return null if no intersection
/*      check.copy(v1);
      check.scale(a);
      check.add(p1,coords);
      if (coords.z != check.z) return null;
 */     
      // Intersect OK. Return coordinate
      return coords; 
  }

  /**
   * This method returns the shortest distance between a point and a line
   * 
   * @param p	Point
   * @param s	Start point of line
   * @param e	End point of line
   * @return	Shortest distance from point to line
   */
  
  public static float distancePoint2Line(_Point p, _Point s, _Point e) {
      Vector3D vp,vs,ve;
      
      vp = p.getVector();
      vs = s.getVector();
      ve = e.getVector();
      
      vp.sub(vp,vs);
      ve.sub(ve,vs);
      
      vs.cross(vp,ve);
      
      return vs.getLength() / ve.getLength();
  }
  
  
  public static void refineKnotVectCurve(int p, float[] U, _CtrlPoint[] Pw, float[] X, float[] Ubar, _CtrlPoint[] Qw ) {
      int i,j,k,l,ind;
      int n = Pw.length - 1;
      int r = X.length - 1;
      float alfa;
      
      int m = n+p+1;
      int a = lib3D.findSpan(n,p,X[0],U);
      int b = lib3D.findSpan(n,p,X[r],U) +1;

      for(j=0; j<=a-p; j++) Qw[j] = new _CtrlPoint(Pw[j]);
      for(j=b-1; j<=n; j++) Qw[j+r+1] = new _CtrlPoint(Pw[j]);
      for(j=0; j<=a; j++) Ubar[j] = U[j];
      for(j=b+p; j<=m; j++) Ubar[j+r+1] = U[j];
      
      i = b+p-1;
      k = b+p+r;
             
      for (j=r; j>=0; j--) {
          while (X[j] <= U[i] && i > a) {
              Qw[k-p-1] = new _CtrlPoint(Pw[i-p-1]);
              Ubar[k] = U[i];
              k = k - 1;
              i = i - 1;
          }
          Qw[k-p-1] = new _CtrlPoint(Qw[k-p]);
          for (l=1; l<=p; l++) {
              ind = k-p+l;
              alfa = Ubar[k+l] - X[j];
              if (Math.abs(alfa) == 0)
                  Qw[ind-1].setTo(Qw[ind]);
              else {
                  alfa = alfa / (Ubar[k+l] - U[i-p+l]);
                  Qw[ind-1].x = alfa * Qw[ind-1].x + (1.0f - alfa) * Qw[ind].x;
                  Qw[ind-1].y = alfa * Qw[ind-1].y + (1.0f - alfa) * Qw[ind].y;
                  Qw[ind-1].z = alfa * Qw[ind-1].z + (1.0f - alfa) * Qw[ind].z;
              }
          }
          Ubar[k] = X[j];
          k = k - 1;
      }
      
  }
  
  public static int[] degreeElevateCurve(int p, float[] U, _CtrlPoint[] Pw, int t, float[] Uh, _CtrlPoint[] Qw) {
      int i,j,k,mpi,mh,nh,kind,r,a,b,cind,mul,oldr,lbz,rbz,save,s,first,last,kj,tr;
      float inv,ua,ub,numer,den,bet,alf,gam;
      int[] out;

      _CtrlPoint[] bpts = new _CtrlPoint[p+1];
      _CtrlPoint[] ebpts = new _CtrlPoint[p+t+1];
      _CtrlPoint[] Nextbpts = new _CtrlPoint[p-1];
      int n = Pw.length - 1;
      int m = n+p+1;
      int ph = p + t;
      int ph2 = ph / 2;
      float[][] bezalfs = new float[ph+1][p+1];
      for (i=0; i<=ph; i++)
          bezalfs[i] = new float[p+1];
      float[] alfs = new float[p-1];
      
      // Compute bezalfs coefficients
      bezalfs[0][0] = 1.0f;
      bezalfs[ph][p] = 1.0f;
      for (i=1; i<=ph2; i++) {
          inv = 1.0f / bin(ph,i);
          mpi = Math.min(p,i);
          for (j=Math.max(0,i-t); j<=mpi; j++) 
              bezalfs[i][j] = inv * bin(p,j) * bin(t,i-j);
      }
      for (i=ph2+1; i<=ph-1; i++) {
          mpi = Math.min(p,i);
          for (j=Math.max(0,i-t); j<=mpi; j++)
              bezalfs[i][j] = bezalfs[ph-i][p-j];
      }
      
      // Initialize other values
      mh = ph;
      kind = ph +1;
      r = -1;
      a = p;
      b = p+1;
      cind = 1;
      ua = U[0];
      Qw[0] = new _CtrlPoint(Pw[0]);
      for (i=0; i<=ph; i++)
          Uh[i] = ua;
      
      // Initialize first Bezier segment
      for (i=0; i<=p; i++)
          bpts[i] = new _CtrlPoint(Pw[i]);
      
      // Main loop
      while (b < m) {
          i = b;
          while (b < m && U[b] == U[b+1])
              b = b +1;
          mul = b-i+1;
          mh = mh + mul + t;
          ub = U[b];
          oldr = r;
          r = p - mul;
          // Insert kont u(b) r times
          if (oldr > 0)
              lbz = (oldr+2)/2;
          else
              lbz = 1;
          if (r > 0)
              rbz = ph-(r+1)/2;
          else
              rbz = ph;
          if (r > 0) {
              // Insert knot to bezier segment
              numer = ub - ua;
              for (k=p; k>mul; k--)
                  alfs[k-mul-1] = numer/(U[a+k]-ua);
              for (j=1; j<=r; j++) {
                  save = r-j;
                  s = mul+j;
                  for (k=p; k>=s; k--) {
                      bpts[k].x = alfs[k-s] * bpts[k].x + (1.0f - alfs[k-s]) * bpts[k-1].x;
                      bpts[k].y = alfs[k-s] * bpts[k].y + (1.0f - alfs[k-s]) * bpts[k-1].y;
                      bpts[k].z = alfs[k-s] * bpts[k].z + (1.0f - alfs[k-s]) * bpts[k-1].z;
                  }
                  Nextbpts[save] = new _CtrlPoint(bpts[p]);
              }
          } // End of insert knot

     for (i=lbz; i<=ph; i++) {
          ebpts[i] = new _CtrlPoint(0,0,0);
          mpi = Math.min(p,i);
          for (j=Math.max(0,i-t); j<=mpi; j++) {
              ebpts[i].x = ebpts[i].x + bezalfs[i][j] * bpts[j].x;
              ebpts[i].y = ebpts[i].y + bezalfs[i][j] * bpts[j].y;
              ebpts[i].z = ebpts[i].z + bezalfs[i][j] * bpts[j].z;
              //ebpts[i].w = ebpts[i].w + bezalfs[i][j] * bpts[j].w;
          }
      }  // End of degree elevating Bezier
          
      if (oldr > 1) {
          // Must remove knot u = U[a] oldr times
          first = kind - 2;
          last = kind;
          den = ub - ua;
          bet = (ub-Uh[kind-1])/den;
          for (tr=1; tr<oldr; tr++) {
              // Knot removal loop
              i = first;
              j = last;
              kj = j-kind+1;
              while (j-i > tr) {
                  // Loop and compute the new control points for one removal step
                  if (i < cind) {
                      alf = (ub-Uh[i])/(ua-Uh[i]);
                      Qw[i].x = alf*Qw[i].x + (1.0f-alf)*Qw[i-1].x;
                      Qw[i].y = alf*Qw[i].y + (1.0f-alf)*Qw[i-1].y;
                      Qw[i].z = alf*Qw[i].z + (1.0f-alf)*Qw[i-1].z;
                  }
                  if (j >= lbz) {
                      if (j-tr <= kind-ph+oldr) {
                          gam = (ub-Uh[j-tr])/den;
                          ebpts[kj].x = gam*ebpts[kj].x + (1.0f-gam)*ebpts[kj+1].x;
                          ebpts[kj].y = gam*ebpts[kj].y + (1.0f-gam)*ebpts[kj+1].y;
                          ebpts[kj].z = gam*ebpts[kj].z + (1.0f-gam)*ebpts[kj+1].z;
                      }
                      else {
                          ebpts[kj].x = bet*ebpts[kj].x + (1.0f-bet)*ebpts[kj+1].x;
                          ebpts[kj].y = bet*ebpts[kj].y + (1.0f-bet)*ebpts[kj+1].y;
                          ebpts[kj].z = bet*ebpts[kj].z + (1.0f-bet)*ebpts[kj+1].z;
                      }
                  }
                  i = i+1;
                  j = j-1;
                  kj = kj-1;
              }
              first = first -1;
              last = last +1;
          }
      } // End of removing knot u = U[a]

      if (a != p)
          for (i=0; i<ph-oldr; i++) {
              Uh[kind] = ua;
              kind = kind +1;
          }
      
      for (j=lbz; j<=rbz; j++) {
          Qw[cind] = new _CtrlPoint(ebpts[j]);
          cind = cind +1;
      }

      if (b < m) {
          // Set up for next pass though loop
          for (j=0; j<r; j++)
              bpts[j] = new _CtrlPoint(Nextbpts[j]);
          for (j=r; j<=p; j++)
              bpts[j] = new _CtrlPoint(Pw[b-p+j]);
          a = b;
          b = b+1;
          ua = ub;
      }
      else {
          // End knot
          for (i=0; i<=ph; i++) 
              Uh[kind+i] = ub;
      }
      } // End of while loop (b < m)
      
  nh = mh - ph -1;    
      
  // Return knot array length, control point array length and new degree
  out = new int[3];    
  out[0] = mh +1;
  out[1] = nh +1;
  out[2] = ph;
  return out;
  }
  
}