开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > 3D图形编程 > 查看源码
CyberXform.cc
资源名称：scanalyze-1.0.3_source_code.rar　[点击查看]
上传用户：kellyonhid
上传日期：2013-10-12
资源大小：932k
文件大小：18k
源码类别：

3D图形编程

开发平台：

Visual C++

CyberXform.cc：源码内容
  1. //############################################################
  2. // 
  3. // CyberXform.cc
  4. //
  5. // Kari Pulli
  6. // Fri Dec 11 11:40:42 CET 1998
  7. //
  8. // Class for transforming raw Cyberware data (the Digital 
  9. // Michelangelo scanner) into 3D, as well as projecting
  10. // 3D points into scans
  11. //
  12. //############################################################
  14. #include "CyberXform.h"
  15. #include "CyberCalib.h"
  16. #include <stdio.h>
  17. #include <iostream.h>
  18. #include <fstream.h>
  19. #ifdef WIN32
  20. # include "defines.h"
  21. #endif
  24. #define SHOW(X) cout << #X " = " << X << endl
  26. static CyberCalib cc;           // calibration data
  28.                                 // and some other calibration
  29.                                 // data that hasn't been integrated
  30.                                 // into CyberCalib
  31. // these numbers come from the CyTemplate-WAG8007.iv file
  32. // and describe the range of the raw data for y and z
  33. static int CorrRowOffset = 4;  
  34. static int CorrNumRows   = 236;
  35. static int CorrZOffset   = 60;
  36. static int CorrZSize     = 2880;
  37. static int otableSize = 9;
  38. // The calibration values and derived calibration constants used
  39. // by these macros are defined in CyberCalib.h
  40. // a <- dtssp 
  41. // b <- dtsep 
  42. // c <- offt 
  43. // minscrew <- -6
  44. // function ang = screw_to_angle(screw, a, b, c, minscrew)
  45. //   ang = angle(a, b, screw-minscrew+c) - angle(a, b, c);
  46. //  
  47. // function res = angle(a,b,c)
  48. //   % get the angle opposite to c in a triangle a,b,c
  49. //   res = acos((a*a+b*b-c*c)/(2*a*b));
  51. #define TURN_SCREW_TO_ANGLE(scr) 
  52. (acos((cc.t_h2-((scr)+cc.t_h1)*((scr)+cc.t_h1))*cc.t_h3) - cc.t_h4)
  54. #define NOD_SCREW_TO_ANGLE(scr) 
  55. (acos((cc.n_h2-((scr)+cc.n_h1)*((scr)+cc.n_h1))*cc.n_h3) - cc.n_h4)
  57. #define SCREW_TO_ANGLE(scr) 
  58. (acos((_h2-((scr)+_h1)*((scr)+_h1))*_h3) - _h4)
  60. #define ANGLE_TO_SCREW(ang) 
  61. (sqrtf(_h2 - cos(ang+_h4)/_h3) - _h1)
  63. static float corners[4][2] = {
  64.   { CorrRowOffset*2, CorrZOffset },
  65.   { (CorrRowOffset+CorrNumRows)*2, CorrZOffset },
  66.   { CorrRowOffset*2, CorrZOffset+CorrZSize },
  67.   { (CorrRowOffset+CorrNumRows)*2, CorrZOffset+CorrZSize }
  68. };
  71. // the answer is laser_ctr_in_laser_coords() = [0 18.9049 -1215.23]
  72. Pnt3
  73. CyberXform::laser_ctr_in_laser_coords(void)
  74. {
  75.   Pnt3 a(0, corners[0][0], corners[0][1]);
  76.   Pnt3 b(0, corners[1][0], corners[1][1]);
  77.   Pnt3 c(0, corners[2][0], corners[2][1]);
  78.   Pnt3 d(0, corners[3][0], corners[3][1]);
  79.   raw_to_laser(a);
  80.   raw_to_laser(b);
  81.   raw_to_laser(c);
  82.   raw_to_laser(d);
  83.   Pnt3 ans;
  84.   bool ok = lines_intersect(a,c,b,d, ans);
  85.   assert(ok);
  86.   return ans;
  87. }
  90. void
  91. CyberXform::bounds_for_circle_on_laser_in_raw(float ly, float lz, float r,
  92.       short ywin[2], short zwin[2])
  93. { 
  94.   Xform<double> laser_to_raw = raw_to_laser;
  95.   laser_to_raw.invert();
  97.   //SHOW("...");
  99.   // need to convert from laser coordinates back to raw
  100.   Pnt3 p(0, ly+r, lz);
  101.   //SHOW(p);
  102.   laser_to_raw(p);
  103.   //SHOW(p);
  104.   ywin[0] = ywin[1] = p[1];
  105.   zwin[0] = zwin[1] = p[2];
  106.   p.set(0, ly-r, lz);
  107.   //SHOW(p);
  108.   laser_to_raw(p);
  109.   //SHOW(p);
  110.   if      (ywin[0] > p[1]) ywin[0] = p[1];
  111.   else if (ywin[1] < p[1]) ywin[1] = p[1];
  112.   if      (zwin[0] > p[2]) zwin[0] = p[2];
  113.   else if (zwin[1] < p[2]) zwin[1] = p[2];
  114.   p.set(0, ly, lz+r);
  115.   //SHOW(p);
  116.   laser_to_raw(p);
  117.   //SHOW(p);
  118.   if      (ywin[0] > p[1]) ywin[0] = p[1];
  119.   else if (ywin[1] < p[1]) ywin[1] = p[1];
  120.   if      (zwin[0] > p[2]) zwin[0] = p[2];
  121.   else if (zwin[1] < p[2]) zwin[1] = p[2];
  122.   p.set(0, ly, lz-r);
  123.   //SHOW(p);
  124.   laser_to_raw(p);
  125.   //SHOW(p);
  126.   if      (ywin[0] > p[1]) ywin[0] = p[1];
  127.   else if (ywin[1] < p[1]) ywin[1] = p[1];
  128.   if      (zwin[0] > p[2]) zwin[0] = p[2];
  129.   else if (zwin[1] < p[2]) zwin[1] = p[2];
  130.   //SHOW(ywin[0]);
  131.   //SHOW(ywin[1]);
  132.   //SHOW(zwin[0]);
  133.   //SHOW(zwin[1]);
  134. }
  138. // with the current optical transform, the raw z grows toward
  139. // scanner, the laser z grows away from it
  140. // raw and laser y behave more nicely
  142. void
  143. CyberXform::setup(bool vscan, bool tup, int tconf, bool tright,
  144.   float hor_trans, float screw, float vert_trans)
  145. {
  146.   vertical_scan = vscan;
  147.   // the transformation goes as follows:
  148.   // first, apply the optical correction to the data
  149.   // (the raw data comes in two shorts, 
  150.   // y [8,479] and z [60,2939])
  151.   // then, apply the optical frame transform
  152.   // then, apply the nod,
  153.   // then, apply the turn,
  154.   // finally, translate
  155.   // only the nod is variable, all the rest can be
  156.   // collapsed (optical correction and opt. frame xform,
  157.   // turn and translate)
  159.   // get the optical transformation first
  160.   
  161.   // map the raw data to range [1,otableSize]
  162.   double scaley = float(otableSize-1) / (2*CorrNumRows-1);
  163.   double scalez = float(otableSize-1) / (CorrZSize-1);
  164.   raw_to_laser.identity();
  166.   // remove offset
  167.   raw_to_laser.translate(0.0,
  168.  - (2*CorrRowOffset),
  169.  - CorrZOffset);
  170.   // scale to [0..scale]
  171.   raw_to_laser.scale(1.0, scaley, scalez);
  172.   // move up by one
  173.   raw_to_laser.translate(0., 1., 1.);
  175.   // do the optical table correction
  176.   raw_to_laser *= Xform<double>(cc.A);
  178.   // do the laser-to-nod frame transformation
  179.   laser_to_scanaxis = Xform<double>(cc.opt_frm_v);
  181.   scanaxis_to_horz.identity();
  182.   if (vertical_scan) {
  183.     _h1 = cc.n_h1; _h2 = cc.n_h2; _h3 = cc.n_h3; _h4 = cc.n_h4;
  184.     axdir[0] = cc.axnod[0]; axdir[1] = cc.axnod[1]; axdir[2] = cc.axnod[2];
  185.     ax0[0]  = cc.axnod0[0]; ax0[1]  = cc.axnod0[1]; ax0[2]  = cc.axnod0[2];
  186.     scanaxis_to_horz.rot(TURN_SCREW_TO_ANGLE(screw),
  187.  cc.axturn[0], cc.axturn[1], cc.axturn[2],
  188.  cc.axturn0[0], cc.axturn0[1], cc.axturn0[2]);
  189.   } else {
  190.     _h1 = cc.t_h1; _h2 = cc.t_h2; _h3 = cc.t_h3; _h4 = cc.t_h4;
  191.     axdir[0] = cc.axturn[0];axdir[1] = cc.axturn[1];axdir[2] = cc.axturn[2];
  192.     ax0[0]  = cc.axturn0[0]; ax0[1] = cc.axturn0[1]; ax0[2] = cc.axturn0[2];
  193.     laser_to_scanaxis *= Xform<double>(cc.opt_frm_h);
  194.     laser_to_scanaxis.rot(NOD_SCREW_TO_ANGLE(screw),
  195.   cc.axnod[0], cc.axnod[1], cc.axnod[2],
  196.   cc.axnod0[0], cc.axnod0[1], cc.axnod0[2]);
  197.   }
  199.   // Add translation
  200.   if (!tright) tup = !tup;
  202.   if (tup)  {
  203.     if (tconf == 1)  {
  204.       scanaxis_to_horz.translate(hor_trans * cc.arm_up1[0],
  205.  hor_trans * cc.arm_up1[1],
  206.  hor_trans * cc.arm_up1[2] );
  207.     } else if (tconf == 2)  {
  208.       scanaxis_to_horz.translate(hor_trans * cc.arm_up2[0],
  209.  hor_trans * cc.arm_up2[1],
  210.  hor_trans * cc.arm_up2[2] );
  211.     } else if (tconf == 3)  {
  212.       scanaxis_to_horz.translate(hor_trans * cc.arm_up3[0],
  213.  hor_trans * cc.arm_up3[1],
  214.  hor_trans * cc.arm_up3[2] );
  215.     } else {
  216.       scanaxis_to_horz.translate(hor_trans * cc.arm_up4[0],
  217.  hor_trans * cc.arm_up4[1],
  218.  hor_trans * cc.arm_up4[2] );
  219.     }
  220.   } else {
  221.     if (tconf == 1)  {
  222.       scanaxis_to_horz.translate(hor_trans * cc.arm_down1[0],
  223.  hor_trans * cc.arm_down1[1],
  224.  hor_trans * cc.arm_down1[2] );
  225.     } else if (tconf == 2)  {
  226.       scanaxis_to_horz.translate(hor_trans * cc.arm_down2[0],
  227.  hor_trans * cc.arm_down2[1],
  228.  hor_trans * cc.arm_down2[2] );
  229.     } else if (tconf == 3)  {
  230.       scanaxis_to_horz.translate(hor_trans * cc.arm_down3[0],
  231.  hor_trans * cc.arm_down3[1],
  232.  hor_trans * cc.arm_down3[2] );
  233.     } else {
  234.       scanaxis_to_horz.translate(hor_trans * cc.arm_down4[0],
  235.  hor_trans * cc.arm_down4[1],
  236.  hor_trans * cc.arm_down4[2] );
  237.     }
  238.   }
  239.   scanaxis_to_horz.translate(0,0,vert_trans);
  241.   // precalculations for backprojection
  242.   laser_n = &((double *) laser_to_scanaxis)[0];
  243.   laser_t = &((double *) laser_to_scanaxis)[12];
  244.   n_dot_t = laser_n[0]*laser_t[0]+laser_n[1]*laser_t[1]
  245.     +laser_n[2]*laser_t[2];
  246.   // vector s gives the direction of the line that we
  247.   // get when we drop a perpendicular from a 3D point
  248.   // to the nod axis, and rotate it so it cuts the laser
  249.   // plane
  250.   // s = cross(n,nodaxis)
  251.   s[0] = laser_n[1]*axdir[2] - laser_n[2]*axdir[1];
  252.   s[1] = laser_n[2]*axdir[0] - laser_n[0]*axdir[2];
  253.   s[2] = laser_n[0]*axdir[1] - laser_n[1]*axdir[0];
  254.   double sinvlen = 1.0 / sqrt(s[0]*s[0]+s[1]*s[1]+s[2]*s[2]);
  255.   s[0] *= sinvlen; s[1] *= sinvlen; s[2] *= sinvlen; 
  257.   q[0] = s[1]*axdir[2] - s[2]*axdir[1];
  258.   q[1] = s[2]*axdir[0] - s[0]*axdir[2];
  259.   q[2] = s[0]*axdir[1] - s[1]*axdir[0];
  261.   n_dot_q = laser_n[0]*q[0]+laser_n[1]*q[1]+laser_n[2]*q[2];
  262.   s_dot_q = s[0]*q[0] + s[1]*q[1] + s[2]*q[2];
  264.   raw_to_scanaxis     = laser_to_scanaxis * raw_to_laser;
  266.   raw_to_scanaxis_inv = raw_to_scanaxis;
  267.   raw_to_scanaxis_inv.invert();
  268.   toYZ = (const double *) raw_to_scanaxis_inv;
  270.   // figure out some limits for testing whether a point
  271.   // can project to within the working volume
  272.   axislimit_min = 1.e33; axislimit_max = -axislimit_min;
  273.   axisdist_near = 1.e33; axisdist_far = -1.0;
  274.   for (int i=0; i<4; i++) {
  275.     Pnt3 p(0, corners[i][0], corners[i][1]);
  276.     raw_to_scanaxis(p);
  277.     // change to vector from a0 to p
  278.     p[0] -= ax0[0]; p[1] -= ax0[1]; p[2] -= ax0[2];
  279.     float tmp = p[0]*axdir[0] + p[1]*axdir[1] + p[2]*axdir[2];
  280.     if (tmp < axislimit_min) axislimit_min = tmp;
  281.     if (tmp > axislimit_max) axislimit_max = tmp;
  282.     // calculate also the squared distance to the axis
  283.     tmp = dot(p, p) - tmp*tmp; // pythagorean rule
  284.     assert(tmp > 0.0);
  285.     if (tmp < axisdist_near) axisdist_near = tmp;
  286.     if (tmp > axisdist_far ) axisdist_far  = tmp;
  287.   }
  288.   axisdist_near = sqrtf(axisdist_near);
  289.   axisdist_far  = sqrtf(axisdist_far);
  290.   /*
  291.   SHOW(vertical_scan);
  292.   Pnt3 lzr = laser_ctr_in_laser_coords();
  293.   laser_to_scanaxis(lzr);
  294.   Pnt3 t1 = lzr - Pnt3(ax0);
  295.   float t = dot(t1, Pnt3(axdir));
  296.   SHOW(sqrtf(t1.norm2() - t*t));
  297.   assert(0);
  298.   */
  299. }
  302. void 
  303. CyberXform::set_screw(float even_scr, float odd_scr)
  304. {
  305.   odd_xform = even_xform = raw_to_scanaxis;
  306.   even_xform.rot(SCREW_TO_ANGLE(even_scr),
  307.  axdir[0], axdir[1], axdir[2],
  308.  ax0[0], ax0[1], ax0[2]);
  309.   odd_xform.rot (SCREW_TO_ANGLE(odd_scr),
  310.  axdir[0], axdir[1], axdir[2],
  311.  ax0[0], ax0[1], ax0[2]);
  312.   even_xform *= scanaxis_to_horz;
  313.   odd_xform  *= scanaxis_to_horz;
  314.   even_xf = (const double *) even_xform;
  315.   odd_xf  = (const double *) odd_xform;
  316.   
  317.   Xform<double> blah = laser_to_scanaxis;
  318.   blah.rot(SCREW_TO_ANGLE(even_scr),
  319.    axdir[0], axdir[1], axdir[2],
  320.    ax0[0], ax0[1], ax0[2]);
  321.   blah *= scanaxis_to_horz;
  322.   Pnt3 lzr(0, 18.9, -1215.2);
  323.   blah(lzr);
  324.   laser_ctr = lzr;
  325. }
  328. void 
  329. CyberXform::set_screw(float even_scr)
  330. {
  331.   even_xform = raw_to_scanaxis;
  332.   even_xform.rot(SCREW_TO_ANGLE(even_scr),
  333.  axdir[0], axdir[1], axdir[2],
  334.  ax0[0], ax0[1], ax0[2]);
  335.   even_xform *= scanaxis_to_horz;
  337.   even_xf = (const double *) even_xform;
  338. }
  341. // use this only if you want to see how the scanhead
  342. // moves, e.g., for color camera stuff
  343. Xform<double>
  344. CyberXform::set_and_get_geometric_xform(float screw)
  345. {
  346.   Xform<double> xf = laser_to_scanaxis;
  347.   xf.rot(SCREW_TO_ANGLE(screw),
  348.  axdir[0], axdir[1], axdir[2],
  349.  ax0[0], ax0[1], ax0[2]);
  350.   xf *= scanaxis_to_horz;
  351.   return xf;
  352. }
  355. Pnt3
  356. CyberXform::apply_xform(short y, short z)
  357. {
  358.   if (y%2) {
  359.     double invw = 
  360.       1.0 / (odd_xf[7]*y+odd_xf[11]*z+odd_xf[15]);
  361.     return Pnt3((odd_xf[4]*y+odd_xf[8]*z+odd_xf[12])*invw,
  362. (odd_xf[5]*y+odd_xf[9]*z+odd_xf[13])*invw,
  363. (odd_xf[6]*y+odd_xf[10]*z+odd_xf[14])*invw);
  364.   } else {
  365.     double invw = 
  366.       1.0 / (even_xf[7]*y+even_xf[11]*z+even_xf[15]);
  367.     return Pnt3((even_xf[4]*y+even_xf[8]*z+even_xf[12])*invw,
  368. (even_xf[5]*y+even_xf[9]*z+even_xf[13])*invw,
  369. (even_xf[6]*y+even_xf[10]*z+even_xf[14])*invw);
  370.   }
  371. }
  373. void
  374. CyberXform::apply_xform(short y, short z, Pnt3 &p)
  375. {
  376.   if (y%2) {
  377.     double invw = 
  378.       1.0 / (odd_xf[7]*y+odd_xf[11]*z+odd_xf[15]);
  379.     p.set((odd_xf[4]*y+odd_xf[8]*z+odd_xf[12])*invw,
  380.   (odd_xf[5]*y+odd_xf[9]*z+odd_xf[13])*invw,
  381.   (odd_xf[6]*y+odd_xf[10]*z+odd_xf[14])*invw);
  382.   } else {
  383.     double invw = 
  384.       1.0 / (even_xf[7]*y+even_xf[11]*z+even_xf[15]);
  385.     p.set((even_xf[4]*y+even_xf[8]*z+even_xf[12])*invw,
  386.   (even_xf[5]*y+even_xf[9]*z+even_xf[13])*invw,
  387.   (even_xf[6]*y+even_xf[10]*z+even_xf[14])*invw);
  388.   }
  389. }
  391. // used by CyberTurn...
  392. Pnt3
  393. CyberXform::apply_raw_to_laser(short y, short z)
  394. {
  395.   Pnt3 p(0,y,z);
  396.   raw_to_laser(p);
  397.   return p;
  398. }
  401. // move a raw data point to the same coordinates as
  402. // the scan axis, project it to it, get a float
  403. // that expresses the point's position on axis
  404. float
  405. CyberXform::axis_project(short y, short z)
  406. {
  407.   Pnt3 p(0,y,z);
  408.   raw_to_scanaxis(p);
  409.   return ((p[0]-ax0[0])*axdir[0] +
  410.   (p[1]-ax0[1])*axdir[1] +
  411.   (p[2]-ax0[2])*axdir[2]);
  412. }
  415. static double axis_proj_pos;
  416. static double scan_angle;
  418. // return the scr,y,z raw coordinates that would have
  419. // exactly scanned p_in
  420. // return false if the point is not within the viewing frustum
  421. // (can be in front or back of the actual working volume,
  422. //  but not on the side)
  423. bool
  424. CyberXform::back_project(const Pnt3 &p_in, Pnt3 &p_out,
  425.  bool check_frustum)
  426. {
  427.   // the sequence of events:
  428.   // * remove the transformation from horizontal translation
  429.   //   until the scan axis
  430.   // * project the point p onto the scan axis (call that pa)
  431.   // * can already check whether in working volume
  432.   //   (too far left or right, too far, too close,
  433.   //    although close one's can be used for carving)
  434.   // * find the intersection of the laser plane with p
  435.   //   think about rotating not the plane but p
  436.   //   find where they intersect, from that get the 
  437.   //   y,z coordinates, and also the rotation angle,
  438.   //   from which get the screw length
  439.   // * calculate the rotation intersection by first
  440.   //   going from pa to laser plane,
  441.   //   then continue along the plane so far that
  442.   //   the distance from pa becomes the same as |p-pa|.
  444.   float *p = &p_out[0];
  445.   // * remove the horizontal translation, maybe also turn
  446.   scanaxis_to_horz.apply_inv(p_in, p);
  448.   // * project the point p onto the scan axis (call that pa)
  449.   axis_proj_pos = ((p[0]-ax0[0])*axdir[0] +
  450.    (p[1]-ax0[1])*axdir[1] +
  451.    (p[2]-ax0[2])*axdir[2]);
  452.   // within frustum?
  453.   if (check_frustum && 
  454.       (axis_proj_pos < axislimit_min ||
  455.        axis_proj_pos > axislimit_max)) 
  456.     return false;
  457.   double pa[3],r[3];
  458.   pa[0] = axis_proj_pos*axdir[0]+ax0[0];
  459.   pa[1] = axis_proj_pos*axdir[1]+ax0[1];
  460.   pa[2] = axis_proj_pos*axdir[2]+ax0[2];
  461.   
  462.   // r is pa to p
  463.   r[0] = p[0]-pa[0];
  464.   r[1] = p[1]-pa[1];
  465.   r[2] = p[2]-pa[2];
  466.   double r2 = r[0]*r[0]+r[1]*r[1]+r[2]*r[2];
  468.   // find the intersection with laser plane
  469.   // alpha = n.t - n.pa (is also distance of pa from laser plane)
  470.   double alpha = (n_dot_t - (laser_n[0]*pa[0]+
  471.      laser_n[1]*pa[1]+
  472.      laser_n[2]*pa[2])) / n_dot_q;
  474.   double alpha2 = alpha*alpha;
  476.   if (r2 < alpha2) {
  477.     cerr << "Really weird input to backproject!" << endl;
  478.     cerr << "(too close to scan axis)" << endl;
  479.     return false;
  480.   }
  482.   double beta;
  483.   double tmp = sqrtf(alpha2*s_dot_q*s_dot_q - (alpha2-r2));
  484.   if (vertical_scan) beta = -alpha*s_dot_q - tmp;
  485.   else               beta = -alpha*s_dot_q + tmp;
  487.   // get the "rotated" point on the laser plane
  488.   double rp[3];
  489.   rp[0] = pa[0] + alpha*q[0] + beta*s[0];
  490.   rp[1] = pa[1] + alpha*q[1] + beta*s[1];
  491.   rp[2] = pa[2] + alpha*q[2] + beta*s[2];
  492.   
  493.   // get the y,z coords
  494.   // don't need to calculate x because it should become 0
  495.   tmp = 
  496.     1.0/(toYZ[3]*rp[0]+toYZ[7]*rp[1]+toYZ[11]*rp[2]+toYZ[15]);
  497.   p[1] =(toYZ[1]*rp[0]+toYZ[5]*rp[1]+toYZ[9]*rp[2]+toYZ[13])*tmp;
  498.   p[2] =(toYZ[2]*rp[0]+toYZ[6]*rp[1]+toYZ[10]*rp[2]+toYZ[14])*tmp;
  499.   
  500.   // get the screw
  501.   scan_angle = acos((alpha * (r[0]*q[0]+r[1]*q[1]+r[2]*q[2]) +
  502.      beta *  (r[0]*s[0]+r[1]*s[1]+r[2]*s[2]))/r2);
  503.   // try to deal with slightly negative angles
  504.   if (scan_angle > .75 * M_PI) scan_angle -= M_PI;
  506.   p[0] = ANGLE_TO_SCREW(scan_angle);
  507.   return true;
  508. }
  513. // return values
  514. // 0 not in frustum
  515. // 1 not fully in frustum
  516. // 2 possibly fully in frustum
  518. int
  519. CyberXform::sphere_status(const Pnt3 &ctr, float radius,
  520.   float &screw,
  521.   float ax_min, float ax_max,
  522.   short ywin[2], short zwin[2], 
  523.   float swin[2])
  524. {
  525.   // first, back project the center
  526.   Pnt3 p;
  527.   scanaxis_to_horz.apply_inv(ctr, p);
  529.   axis_proj_pos = ((p[0]-ax0[0])*axdir[0] +
  530.    (p[1]-ax0[1])*axdir[1] +
  531.    (p[2]-ax0[2])*axdir[2]);
  533.   // check whether in viewing frustum
  534.   if (axis_proj_pos < ax_min - radius ||
  535.       axis_proj_pos > ax_max + radius) {
  536.     //cout << "wide " << radius << endl;
  537.     return 0; // NOT_IN_FRUSTUM
  538.   }
  539.   double pa[3],r[3];
  540.   pa[0] = axis_proj_pos*axdir[0]+ax0[0];
  541.   pa[1] = axis_proj_pos*axdir[1]+ax0[1];
  542.   pa[2] = axis_proj_pos*axdir[2]+ax0[2];
  543.   
  544.   // r is pa to p
  545.   r[0] = p[0]-pa[0];
  546.   r[1] = p[1]-pa[1];
  547.   r[2] = p[2]-pa[2];
  548.   double r2 = r[0]*r[0]+r[1]*r[1]+r[2]*r[2];
  549.   double dist_axis = sqrt(r2);
  551.   if (dist_axis > axisdist_far + radius) {
  552.     //cout << "far " << radius << endl;
  553.     return 0; // NOT_IN_FRUSTUM
  554.   }
  556.   // check whether can be fully in viewing frustum
  557.   if (axis_proj_pos > ax_min + radius &&
  558.       axis_proj_pos < ax_max - radius) {
  559.     // can't be fully in frustum
  560.     return 1;
  561.   }
  562.   if (dist_axis > axisdist_far - radius) {
  563.     // can't be fully in frustum
  564.     return 1;
  565.   }
  567.   Pnt3 bp;
  568.   back_project(ctr, bp, false);
  569.   // how much can we rotate laser plane and still hit sphere?
  570.   // BUGBUG: not accurate at all...
  572.   float alpha = asin(radius / dist_axis);
  573.   swin[0] = scan_angle - alpha;
  574.   swin[1] = scan_angle + alpha;
  576.   if (swin[0] < 0 || swin[1] > M_PI * .75) return 1;
  578.   // the corners of the working volume in laser coordinates
  579.   // are
  580.   // (-2.5, 142.4)
  581.   // (143.2, 140.9
  582.   // (-.3, 1)
  583.   // (130.3, .4)
  584.   // Bbox: [(-2.5, .4); (143.2, 142.4)]
  586.   // convert the y,z to laser coordinates, in mm
  587.   Pnt3 lzr(0,bp[1],bp[2]);
  588.   raw_to_laser(lzr);
  590.   // could the sphere be fully within the working volume?
  591.   bounds_for_circle_on_laser_in_raw(lzr[1], lzr[2], radius,
  592.     ywin, zwin);
  594.   if (ywin[0] <  CorrRowOffset*2 ||
  595.       ywin[1] >= (CorrRowOffset+CorrNumRows)*2) {
  596.     // can't be fully in frustum
  597.     return 1;
  598.   }
  600.   if (zwin[0] <  CorrZOffset) {
  601.     //zwin[1] >= (CorrZOffset+CorrZSize)) {
  602.     // can't be fully in frustum
  603.     return 1;
  604.   }
  606.   swin[0] = ANGLE_TO_SCREW(swin[0]);
  607.   swin[1] = ANGLE_TO_SCREW(swin[1]);
  609.   // could be fully within frustum (for space carving purposes)
  610.   screw = bp[0];
  612.   return 2;
  613. }
  617. #if 0
  619. void
  620. main(void)
  621. {
  622.   CyberXform xf;
  623.   xf.setup(1, 1, 1, 1, 10, 10);
  624.   xf.set_nod(10,20);
  625.   SHOW(xf.even_xform);
  626.   SHOW(xf.odd_xform);
  627. }
  628. #endif