vec screwAxisDirection = new vec(1,0,0);
pt screwAxisPoint = new pt(0,0,0);

class pose { 
  pt O = new pt(0,0,0);  vec U = new vec(0,1,0); vec I = new vec(1,0,0);  vec J = new vec(0,1,0); vec K = new vec(0,0,1);
  pose() {};
  pose(vec T, vec U) {                        // create from K and up-vector
    vec UxT=cross(U,T); float n=UxT.norm();  
    if (n<0.0001) {if (printall) {println("Pose from direction: T and U are parallel.");};}
    else { K.setTo(T); K.makeUnit(); I.setTo(UxT); I.makeUnit();  J.setTo(cross(K,I)); };
    };
  void reset() { O.setTo(0,0,0); I.setTo(1,0,0); J.setTo(0,1,0); K.setTo(0,0,1);  }
  void setTo(pose P) { O.setTo(P.O); I.setTo(P.I); J.setTo(P.J); K.setTo(P.K); };
  void showT() { pt V=O.make(); beginShape(TRIANGLES); V.vert(); V.addVec(I); V.vert(); V=O.make(); V.addVec(K); V.vert();  endShape();}
  void show() {stroke(10); I.show(O); J.show(O); noStroke(); O.show(.1); 
    pt V=O.make(); beginShape(TRIANGLES); V.vert(); V.addVec(I); V.vert(); V=O.make(); V.addVec(K); V.vert();  endShape();}
  void write() {print("O="); O.write(); print("I="); I.write();print("J="); J.write();print("K="); K.write();  println();};
  void Tx(float d) {O.addScaledVec(d,I);}
  void Ty(float d) {O.addScaledVec(d,J);}
  void Tz(float d) {O.addScaledVec(d,K);}
  void Rx(float a) {
    float c=cos(a), s=sin(a); 
    vec cJ=J.make(); cJ.mul(c);    vec msJ=J.make(); msJ.mul(-s); 
    vec sK=K.make(); sK.mul(s);    vec cK=K.make(); cK.mul(c); 
    J.setTo(sum(cJ,sK));           K.setTo(sum(msJ,cK));
    }
  void Ry(float a) {
    float c=cos(a), s=sin(a); 
    vec cK=K.make(); cK.mul(c);   vec msK=K.make(); msK.mul(-s); 
    vec sI=I.make(); sI.mul(s);   vec cI=I.make(); cI.mul(c); 
    K.setTo(sum(cK,sI));           I.setTo(sum(msK,cI));
    }
  void Rz(float a) {
    float c=cos(a), s=sin(a); 
    vec cI=I.make(); cI.mul(c);    vec msI=I.make(); msI.mul(-s); 
    vec sJ=J.make(); sJ.mul(s);    vec cJ=J.make(); cJ.mul(c); 
    I.setTo(sum(cI,sJ));           J.setTo(sum(msI,cJ));
    }
  vec transform(vec V) {vec R=new vec(0,0,0);  R.addScaled(V.x,I); R.addScaled(V.y,J); R.addScaled(V.z,K); return(R); };
  pt transform(pt P) {pt R=O.make();  R.addScaledVec(P.x,I); R.addScaledVec(P.y,J); R.addScaledVec(P.z,K); return(R); };
  vec inverse(vec V) {vec R=new vec(dot(V,I),dot(V,J),dot(V,K));  return(R); };
  pt inverse(pt P) {vec V=O.vecTo(P); pt R= new pt(dot(V,I),dot(V,J),dot(V,K));  return(R); };
  void screw(pose P0, float s, pose P1) {
    pt O0=P0.O;   pt O1=P1.O; vec dO=O0.vecTo(O1);
    vec I0=P0.I;   vec I1=P1.I; vec dI=dif(I1,I0); 
    vec J0=P0.J;   vec J1=P1.J; vec dJ=dif(J1,J0); 
    vec K0=P0.K;   vec K1=P1.K; vec dK=dif(K1,K0); 
     vec T=cross(dK,dI); T.add(cross(dI,dJ)); T.add(cross(dJ,dK));  
     if (T.norm()<0.0001) {this.setTo(P0); O.setTo(interpolate(P0.O,s,P1.O)); }  // pure translation
     else {      // screw
       T.makeUnit(); // screw axis direction
       vec Ni=cross(T,I0); float ni=Ni.norm();  // projections of the 3 basis vectors on plane normal to T
       vec Nj=cross(T,J0); float nj=Nj.norm();  
       vec Nk=cross(T,K0); float nk=Nk.norm();
       vec N0=Ni.make(); vec N1=cross(T,I1); // initial and final projected vector
       if (nj>ni) {N0=Nj; N1=cross(T,J1);}; 
       if ((nk>ni)&&(nk>nj)) {N0=Nk; N1=cross(T,K1);}; // pick the longest
       S =  new pose(T,N0);  // pose used to build transformed self                            
       vec nN1=S.inverse(N1);                            
       float b = atan2(-nN1.x,nN1.y); 
       float d = dot(T,dO); 
       pt P = midPt(O0,O1); vec H = cross(T,dO); H.mul(1.0/tan(b/2)/2); P.addVec(H); 
       S.O.setTo(P); 
       pt nO = S.inverse(O);  vec nI = S.inverse(I); vec nJ = S.inverse(J); vec nK = S.inverse(K);  // transforms self by inverse of S
       S.Rz(s*b); S.Tz(s*d);  // roatates and translates S
       O.setTo(S.transform(nO));  I.setTo(S.transform(nI));  J.setTo(S.transform(nJ)); K.setTo(S.transform(nK));  // transforms self by S
       screwAxisPoint.setTo(P);  screwAxisDirection.setTo(T); screwAxisDirection.mul(10*b/PI); // set global variables for displaying axis
       };
    };
  }
  
pose screw(pose P0, float s, pose P1) {pose P = new pose(); P.setTo(P0); P.screw(P0,s,P1); return(P); }

vec rotate(vec U, float b, vec K) {
  vec R = new vec(0,0,0);
  vec I=cross(U,K); float n=I.norm(); 
  if (n<0.0001) {R.setTo(U);}
  else {I.makeUnit(); vec J=cross(K,I); float x=dot(U,I); float y=dot(U,J); float z=dot(U,K); };
  return(R);
  };