File cmodules/affine/vector3d.tcl from the latest check-in

my c_tclproc_nspace ::vector3d

my c_function {CTHULHU_INLINE int Vector3d_SamePoint(double x0, double y0, double z0, double x1, double y1, double z1)} {
  /*
  ** Return TRUE if x0,y0 is the same point as x1,y1
  */
  return floatCompare(x0,x1)==0 && floatCompare(y0,y1)==0 && floatCompare(z0,z1)==0;
}

my c_tclcmd ::vector3d::compare {
  VectorXYZ A, B;
  int c;
  if( objc!=7 ){
    Tcl_WrongNumArgs(interp, 1, objv, "X0 Y0 Z0 X1 Y1 Z1");
    return TCL_ERROR;
  }
  if( Odie_GetMatrixElementFromObj(interp, objv[1], A, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[2], A, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[3], A, Z_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[4], B, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[5], B, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[6], B, Z_IDX) ) return TCL_ERROR;
  c=VectorXYZ_SamePoint(A,B);
  Tcl_SetObjResult(interp, Tcl_NewBooleanObj(c));
  return TCL_OK;
} {
  aliases ::vector3d::=
}


my c_tclcmd ::vector3d::add {
  VectorXYZ A, B, P;
  Tcl_Obj *pResult;
  if( objc!=7 ){
    Tcl_WrongNumArgs(interp, 1, objv, "X0 Y0 Z0 X1 Y1 Z1");
    return TCL_ERROR;
  }
  if( Odie_GetMatrixElementFromObj(interp, objv[1], A, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[2], A, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[3], A, Z_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[4], B, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[5], B, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[6], B, Z_IDX) ) return TCL_ERROR;
  VectorXYZ_Add(P,A,B);
  pResult=Tcl_NewObj();
  Tcl_ListObjAppendElement(interp,pResult,Tcl_NewDoubleObj(P[X_IDX]));
  Tcl_ListObjAppendElement(interp,pResult,Tcl_NewDoubleObj(P[Y_IDX]));
  Tcl_ListObjAppendElement(interp,pResult,Tcl_NewDoubleObj(P[Z_IDX]));
  Tcl_SetObjResult(interp, pResult);
  return TCL_OK;
} {
  aliases ::vector3d::+
}

my c_tclcmd ::vector3d::subtract {
  VectorXYZ A, B, P;
  Tcl_Obj *pResult;
  if( objc!=7 ){
    Tcl_WrongNumArgs(interp, 1, objv, "X0 Y0 Z0 X1 Y1 Z1");
    return TCL_ERROR;
  }
  if( Odie_GetMatrixElementFromObj(interp, objv[1], A, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[2], A, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[3], A, Z_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[4], B, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[5], B, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[6], B, Z_IDX) ) return TCL_ERROR;
  VectorXYZ_Subtract(P,A,B);
  pResult=Tcl_NewObj();
  Tcl_ListObjAppendElement(interp,pResult,Tcl_NewDoubleObj(P[X_IDX]));
  Tcl_ListObjAppendElement(interp,pResult,Tcl_NewDoubleObj(P[Y_IDX]));
  Tcl_ListObjAppendElement(interp,pResult,Tcl_NewDoubleObj(P[Z_IDX]));
  Tcl_SetObjResult(interp, pResult);
  return TCL_OK;
} {
  aliases {::vector3d::-}
}

my c_tclcmd ::vector3d::orthagonal {
  VectorXYZ A, B;
  if( objc!=7 ){
    Tcl_WrongNumArgs(interp, 1, objv, "X0 Y0 Z0 X1 Y1 Z1");
    return TCL_ERROR;
  }
  if( Odie_GetMatrixElementFromObj(interp, objv[1], A, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[2], A, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[3], A, Z_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[4], B, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[5], B, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[6], B, Z_IDX) ) return TCL_ERROR;
  Tcl_SetObjResult(interp,Tcl_NewBooleanObj(VectorXYZ_IsOrthagonal(A,B)));
  return TCL_OK;
}

my c_tclcmd ::vector3d::distance {
  double result;
  VectorXYZ A, B;
  if( objc!=7 ){
    Tcl_WrongNumArgs(interp, 1, objv, "X0 Y0 Z0 X1 Y1 Z1");
    return TCL_ERROR;
  }
  if( Odie_GetMatrixElementFromObj(interp, objv[1], A, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[2], A, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[3], A, Z_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[4], B, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[5], B, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[6], B, Z_IDX) ) return TCL_ERROR;
  result=VectorXYZ_Distance(A,B);
  Tcl_SetObjResult(interp,Tcl_NewDoubleObj(result));
  return TCL_OK;
}

my c_tclcmd ::vector3d::distanceSq {
  double result;
  VectorXYZ A, B;
  if( objc!=7 ){
    Tcl_WrongNumArgs(interp, 1, objv, "X0 Y0 Z0 X1 Y1 Z1");
    return TCL_ERROR;
  }
  if( Odie_GetMatrixElementFromObj(interp, objv[1], A, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[2], A, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[3], A, Z_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[4], B, X_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[5], B, Y_IDX) ) return TCL_ERROR;
  if( Odie_GetMatrixElementFromObj(interp, objv[6], B, Z_IDX) ) return TCL_ERROR;
  result=VectorXYZ_Distance_squared(A,B);
  Tcl_SetObjResult(interp,Tcl_NewDoubleObj(result));
  return TCL_OK;
}

my c_tclcmd  ::vector3d::cube_corners_base {
  double center_x, center_y, base_z,size_x,size_y,size_z,scale_x,scale_y,nx=100.0,ny=0.0;
  Tcl_Obj *pResult,*topZ,*botZ;
  Tcl_Obj *cornerX,*cornerY;

  if( objc!=7 && objc != 9 ){
    Tcl_WrongNumArgs(interp, 1, objv, "center_x center_y base_z size_x size_y size_z ?nx ny?");
    return TCL_ERROR;
  }

  if(Tcl_GetDoubleFromObj(interp,objv[1],&center_x)) return TCL_ERROR;
  if(Tcl_GetDoubleFromObj(interp,objv[2],&center_y)) return TCL_ERROR;
  if(Tcl_GetDoubleFromObj(interp,objv[3],&base_z)) return TCL_ERROR;

  if(Tcl_GetDoubleFromObj(interp,objv[4],&size_x)) return TCL_ERROR;
  if(Tcl_GetDoubleFromObj(interp,objv[5],&size_y)) return TCL_ERROR;
  if(Tcl_GetDoubleFromObj(interp,objv[6],&size_z)) return TCL_ERROR;
  scale_x=size_x*0.5;
  scale_y=size_y*0.5;
  topZ=Tcl_NewDoubleObj(base_z+size_z);
  botZ=Tcl_NewDoubleObj(base_z);
  if(objc==9) {
    if(Tcl_GetDoubleFromObj(interp,objv[7],&nx)) return TCL_ERROR;
    if(Tcl_GetDoubleFromObj(interp,objv[8],&ny)) return TCL_ERROR;
    if(fabs(ny)>0.001) {
      double theta;
      double matA[6] = {1.0,0.0,0.0,1.0,0.0,0.0};

      theta=atan2(ny,nx);
      matA[0]=cos(theta);
      matA[1]=sin(theta);
      matA[2]=-sin(theta);
      matA[3]=cos(theta);
      matA[4]=0.0;
      matA[5]=0.0;

      pResult=Tcl_NewObj();
      cornerX=Tcl_NewDoubleObj(matA[0]*scale_x+matA[1]*scale_y+matA[4]+center_x);
      cornerY=Tcl_NewDoubleObj(matA[2]*scale_x+matA[3]*scale_y+matA[5]+center_y);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,botZ);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,topZ);

      cornerX=Tcl_NewDoubleObj(matA[0]*scale_x*-1.0+matA[1]*scale_y+matA[4]+center_x);
      cornerY=Tcl_NewDoubleObj(matA[2]*scale_x*-1.0+matA[3]*scale_y+matA[5]+center_y);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,botZ);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,topZ);

      cornerX=Tcl_NewDoubleObj(matA[0]*scale_x+matA[1]*scale_y*-1.0+matA[4]+center_x);
      cornerY=Tcl_NewDoubleObj(matA[2]*scale_x+matA[3]*scale_y*-1.0+matA[5]+center_y);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,botZ);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,topZ);

      cornerX=Tcl_NewDoubleObj(matA[0]*scale_x*-1.0+matA[1]*scale_y*-1.0+matA[4]+center_x);
      cornerY=Tcl_NewDoubleObj(matA[2]*scale_x*-1.0+matA[3]*scale_y*-1.0+matA[5]+center_y);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,botZ);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,topZ);
      Tcl_SetObjResult(interp, pResult);
      return TCL_OK;
    }
  }
  pResult=Tcl_NewObj();
  cornerX=Tcl_NewDoubleObj(center_x-scale_x);
  cornerY=Tcl_NewDoubleObj(center_y-scale_y);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,botZ);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,topZ);

  cornerX=Tcl_NewDoubleObj(center_x+scale_x);
  cornerY=Tcl_NewDoubleObj(center_y-scale_y);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,botZ);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,topZ);

  cornerX=Tcl_NewDoubleObj(center_x-scale_x);
  cornerY=Tcl_NewDoubleObj(center_y+scale_y);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,botZ);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,topZ);

  cornerX=Tcl_NewDoubleObj(center_x+scale_x);
  cornerY=Tcl_NewDoubleObj(center_y+scale_y);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,botZ);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,topZ);

  Tcl_SetObjResult(interp, pResult);
  return TCL_OK;
}

my c_tclcmd  ::vector3d::cube_corners_center {
  double center_x, center_y, center_z,size_x,size_y,size_z,scale_x,scale_y,scale_z,nx=100.0,ny=0.0;
  Tcl_Obj *pResult,*topZ,*botZ;
  Tcl_Obj *cornerX,*cornerY;

  if( objc!=7 && objc != 9 ){
    Tcl_WrongNumArgs(interp, 1, objv, "center_x center_y center_x size_x size_y size_z ?nx ny?");
    return TCL_ERROR;
  }

  if(Tcl_GetDoubleFromObj(interp,objv[1],&center_x)) return TCL_ERROR;
  if(Tcl_GetDoubleFromObj(interp,objv[2],&center_y)) return TCL_ERROR;
  if(Tcl_GetDoubleFromObj(interp,objv[3],&center_z)) return TCL_ERROR;

  if(Tcl_GetDoubleFromObj(interp,objv[4],&size_x)) return TCL_ERROR;
  if(Tcl_GetDoubleFromObj(interp,objv[5],&size_y)) return TCL_ERROR;
  if(Tcl_GetDoubleFromObj(interp,objv[6],&size_z)) return TCL_ERROR;
  scale_x=size_x*0.5;
  scale_y=size_y*0.5;
  scale_z=size_z*0.5;
  topZ=Tcl_NewDoubleObj(center_z+scale_z);
  botZ=Tcl_NewDoubleObj(center_z-scale_z);
  if(objc==9) {
    if(Tcl_GetDoubleFromObj(interp,objv[7],&nx)) return TCL_ERROR;
    if(Tcl_GetDoubleFromObj(interp,objv[8],&ny)) return TCL_ERROR;
    if(fabs(ny)>0.001) {
      double theta;
      double matA[6] = {1.0,0.0,0.0,1.0,0.0,0.0};

      theta=atan2(ny,nx);
      matA[0]=cos(theta);
      matA[1]=sin(theta);
      matA[2]=-sin(theta);
      matA[3]=cos(theta);
      matA[4]=0.0;
      matA[5]=0.0;

      pResult=Tcl_NewObj();
      cornerX=Tcl_NewDoubleObj(matA[0]*scale_x+matA[1]*scale_y+matA[4]+center_x);
      cornerY=Tcl_NewDoubleObj(matA[2]*scale_x+matA[3]*scale_y+matA[5]+center_y);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,botZ);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,topZ);

      cornerX=Tcl_NewDoubleObj(matA[0]*scale_x*-1.0+matA[1]*scale_y+matA[4]+center_x);
      cornerY=Tcl_NewDoubleObj(matA[2]*scale_x*-1.0+matA[3]*scale_y+matA[5]+center_y);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,botZ);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,topZ);

      cornerX=Tcl_NewDoubleObj(matA[0]*scale_x+matA[1]*scale_y*-1.0+matA[4]+center_x);
      cornerY=Tcl_NewDoubleObj(matA[2]*scale_x+matA[3]*scale_y*-1.0+matA[5]+center_y);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,botZ);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,topZ);

      cornerX=Tcl_NewDoubleObj(matA[0]*scale_x*-1.0+matA[1]*scale_y*-1.0+matA[4]+center_x);
      cornerY=Tcl_NewDoubleObj(matA[2]*scale_x*-1.0+matA[3]*scale_y*-1.0+matA[5]+center_y);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,botZ);
      Tcl_ListObjAppendElement(interp,pResult,cornerX);
      Tcl_ListObjAppendElement(interp,pResult,cornerY);
      Tcl_ListObjAppendElement(interp,pResult,topZ);
      Tcl_SetObjResult(interp, pResult);
      return TCL_OK;
    }
  }
  pResult=Tcl_NewObj();
  cornerX=Tcl_NewDoubleObj(center_x-scale_x);
  cornerY=Tcl_NewDoubleObj(center_y-scale_y);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,botZ);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,topZ);

  cornerX=Tcl_NewDoubleObj(center_x+scale_x);
  cornerY=Tcl_NewDoubleObj(center_y-scale_y);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,botZ);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,topZ);

  cornerX=Tcl_NewDoubleObj(center_x-scale_x);
  cornerY=Tcl_NewDoubleObj(center_y+scale_y);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,botZ);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,topZ);

  cornerX=Tcl_NewDoubleObj(center_x+scale_x);
  cornerY=Tcl_NewDoubleObj(center_y+scale_y);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,botZ);
  Tcl_ListObjAppendElement(interp,pResult,cornerX);
  Tcl_ListObjAppendElement(interp,pResult,cornerY);
  Tcl_ListObjAppendElement(interp,pResult,topZ);

  Tcl_SetObjResult(interp, pResult);
  return TCL_OK;
}