moab
HomXform.cpp
Go to the documentation of this file.
00001 
00016 #include "moab/HomXform.hpp"
00017 #include <assert.h>
00018 
00019 namespace moab {
00020 
00021 HomCoord HomCoord::unitv[3] = {HomCoord(1,0,0), HomCoord(0,1,0), HomCoord(0,0,1)};
00022 HomCoord HomCoord::IDENTITY(1, 1, 1);
00023 
00024 int dum[] = {1, 0, 0, 0,
00025              0, 1, 0, 0,
00026              0, 0, 1, 0,
00027              0, 0, 0, 1};
00028 HomXform HomXform::IDENTITY(dum);
00029 
00030 void HomXform::three_pt_xform(const HomCoord &p1, const HomCoord &q1,
00031                               const HomCoord &p2, const HomCoord &q2,
00032                               const HomCoord &p3, const HomCoord &q3) 
00033 {
00034     // pmin and pmax are min and max bounding box corners which are mapped to
00035     // qmin and qmax, resp.  qmin and qmax are not necessarily min/max corners,
00036     // since the mapping can change the orientation of the box in the q reference
00037     // frame.  Re-interpreting the min/max bounding box corners does not change
00038     // the mapping.
00039 
00040     // change that: base on three points for now (figure out whether we can
00041     // just use two later); three points are assumed to define an orthogonal
00042     // system such that (p2-p1)%(p3-p1) = 0
00043   
00044     // use the three point rule to compute the mapping, from Mortensen, 
00045     // "Geometric Modeling".  If p1, p2, p3 and q1, q2, q3 are three points in
00046     // the two coordinate systems, the three pt rule is:
00047     //
00048     // v1 = p2 - p1
00049     // v2 = p3 - p1
00050     // v3 = v1 x v2
00051     // w1-w3 similar, with q1-q3
00052     // V = matrix with v1-v3 as rows
00053     // W similar, with w1-w3
00054     // R = V^-1 * W
00055     // t = q1 - p1 * W
00056     // Form transform matrix M from R, t
00057 
00058     // check to see whether unity transform applies
00059   if (p1 == q1 && p2 == q2 && p3 == q3) {
00060     *this = HomXform::IDENTITY;
00061     return;
00062   }
00063 
00064     // first, construct 3 pts from input
00065   HomCoord v1 = p2 - p1;
00066   assert(v1.i() != 0 || v1.j() != 0 || v1.k() != 0);
00067   HomCoord v2 = p3 - p1;
00068   HomCoord v3 = v1 * v2;
00069   
00070   if (v3.length_squared() == 0) {
00071       // 1d coordinate system; set one of v2's coordinates such that
00072       // it's orthogonal to v1
00073     if (v1.i() == 0) v2.set(1,0,0);
00074     else if (v1.j() == 0) v2.set(0,1,0);
00075     else if (v1.k() == 0) v2.set(0,0,1);
00076     else assert(false);
00077     v3 = v1 * v2;
00078     assert(v3.length_squared() != 0);
00079   }
00080     // assert to make sure they're each orthogonal
00081   assert(v1%v2 == 0 && v1%v3 == 0 && v2%v3 == 0);
00082   v1.normalize(); v2.normalize(); v3.normalize();
00083     // Make sure h is set to zero here, since it'll mess up things if it's one
00084   v1.homCoord[3] = v2.homCoord[3] = v3.homCoord[3] = 0;
00085 
00086   HomCoord w1 = q2 - q1;
00087   assert(w1.i() != 0 || w1.j() != 0 || w1.k() != 0);
00088   HomCoord w2 = q3 - q1;
00089   HomCoord w3 = w1 * w2;
00090   
00091   if (w3.length_squared() == 0) {
00092       // 1d coordinate system; set one of w2's coordinates such that
00093       // it's orthogonal to w1
00094     if (w1.i() == 0) w2.set(1,0,0);
00095     else if (w1.j() == 0) w2.set(0,1,0);
00096     else if (w1.k() == 0) w2.set(0,0,1);
00097     else assert(false);
00098     w3 = w1 * w2;
00099     assert(w3.length_squared() != 0);
00100   }
00101     // assert to make sure they're each orthogonal
00102   assert(w1%w2 == 0 && w1%w3 == 0 && w2%w3 == 0);
00103   w1.normalize(); w2.normalize(); w3.normalize();
00104     // Make sure h is set to zero here, since it'll mess up things if it's one
00105   w1.homCoord[3] = w2.homCoord[3] = w3.homCoord[3] = 0;
00106   
00107     // form v^-1 as transpose (ok for orthogonal vectors); put directly into
00108     // transform matrix, since it's eventually going to become R
00109   *this = HomXform(v1.i(), v2.i(), v3.i(), 0,
00110                    v1.j(), v2.j(), v3.j(), 0,
00111                    v1.k(), v2.k(), v3.k(), 0,
00112                    0, 0, 0, 1);
00113 
00114     // multiply by w to get R
00115   *this *= HomXform(w1.i(), w1.j(), w1.k(), 0,
00116                     w2.i(), w2.j(), w2.k(), 0,
00117                     w3.i(), w3.j(), w3.k(), 0,
00118                     0, 0, 0, 1);
00119   
00120     // compute t and put into last row
00121   HomCoord t = q1 - p1 * *this;
00122   (*this).XFORM(3,0) = t.i();
00123   (*this).XFORM(3,1) = t.j();
00124   (*this).XFORM(3,2) = t.k();
00125 
00126     // M should transform p to q
00127   assert((q1 == p1 * *this) &&
00128          (q2 == p2 * *this) &&
00129          (q3 == p3 * *this));
00130 }
00131   
00132 } // namespace moab
00133 
00134 
00135 #ifdef TEST
00136 
00137 using namespace moab;
00138 
00139 #include <iostream>
00140 
00141 // unit test for the HomCoord and HomXform classes
00142 //
00143 
00144 int test_get_set();
00145 
00146 int test_coord_operators();
00147 
00148 int test_xform_operators();
00149 
00150 int test_xform_functions();
00151 
00152 int test_coord_xform_operators();
00153 
00154 int main(int /*argc*/, char**/* argv[]*/)
00155 {
00156     // first test HomCoord
00157 
00158     // constructors
00159     // following shouldn't compile, since bare constructor is private
00160 //  HomCoord mycoord;
00161   
00162   int errors = 0;
00163 
00164   errors += test_get_set();
00165 
00166   errors += test_coord_operators();
00167 
00168   errors += test_xform_operators();
00169 
00170   errors += test_xform_functions();
00171   
00172   errors += test_coord_xform_operators();
00173 
00174   if (errors > 0)
00175     std::cout << errors << " errors found." << std::endl;
00176   else
00177     std::cout << "All tests passed." << std::endl;
00178     
00179   return errors;
00180 }
00181 
00182 int test_get_set() 
00183 {
00184   int errors = 0;
00185   
00186     // other constructors
00187   int coordsa[4] = {1, 2, 3, 1};
00188   int coordsb[4] = {4, 3, 2, 1};
00189       
00190   
00191   HomCoord coords1(coordsa);
00192   HomCoord coords2(4, 3, 2, 1);
00193   HomCoord coords3(4, 3, 2);
00194   HomCoord coords4(1, 1, 1, 1);
00195 
00196     // set
00197   coords2.set(coordsb);
00198   
00199     // hom_coord()
00200   for (int i = 0; i < 4; i++) {
00201     if (coords2.hom_coord()[i] != coordsb[i]) {
00202       std::cout << "Get test failed." << std::endl;
00203       errors++;
00204     }
00205   }
00206 
00207     // ijkh
00208   if (coords2.i() != coordsb[0] ||
00209       coords2.j() != coordsb[1] ||
00210       coords2.k() != coordsb[2] ||
00211       coords2.h() != coordsb[3]) {
00212     std::cout << "ijkh test failed." << std::endl;
00213     errors++;
00214   }
00215   
00216     // set
00217   coords2.set(3, 3, 3);
00218   
00219     // hom_coord()
00220   if (coords2.hom_coord()[0] != 3 || coords2.hom_coord()[1] != 3 ||
00221       coords2.hom_coord()[2] != 3 || coords2.hom_coord()[3] != 1) {
00222     std::cout << "Set (int) test failed." << std::endl;
00223     errors++;
00224   }
00225   
00226   return errors;
00227 }
00228 
00229 int test_coord_operators() 
00230 {
00231   int errors = 0;
00232   
00233   HomCoord coords1(1, 2, 3, 1);
00234   HomCoord coords2(4, 3, 2, 1);
00235   HomCoord coords3(4, 3, 2);
00236   HomCoord coords4(1, 1, 1, 1);
00237 
00238     // operator>=
00239   bool optest = (coords2 >= coords4 &&
00240                  coords2 >= coords3 &&
00241                  coords3 >= coords2);
00242   if (!optest) {
00243     std::cout << "Test failed for operator >=." << std::endl;
00244     errors++;
00245   }
00246 
00247   optest = (coords4 <= coords2 &&
00248             coords2 <= coords3 &&
00249             coords3 <= coords2);
00250   if (!optest) {
00251     std::cout << "Test failed for operator <=." << std::endl;
00252     errors++;
00253   }
00254 
00255     // operator>
00256   optest = (coords2 > coords4 &&
00257             !(coords2 > coords3) &&
00258             !(coords3 > coords2));
00259   if (!optest) {
00260     std::cout << "Test failed for operator >." << std::endl;
00261     errors++;
00262   }
00263 
00264   optest = (coords4 < coords2 &&
00265             !(coords2 < coords3) &&
00266             !(coords3 < coords2));
00267   if (!optest) {
00268     std::cout << "Test failed for operator <." << std::endl;
00269     errors++;
00270   }
00271 
00272     // operator[]
00273   for (int i = 0; i < 3; i++) {
00274     if (coords1[i] != coords2[3-i]) {
00275       std::cout << "Test failed for operator[]." << std::endl;
00276       errors++;
00277     }
00278   }
00279 
00280     // operator+
00281   HomCoord coords5(2*coords1[0], 2*coords1[1], 2*coords1[2]);
00282   HomCoord coords6 = coords1 + coords1;
00283   if (coords5 != coords6) {
00284       std::cout << "Test failed for operator+." << std::endl;
00285       errors++;
00286   }
00287   
00288     // operator-
00289   if (coords5-coords1 != coords1) {
00290       std::cout << "Test failed for operator-." << std::endl;
00291       errors++;
00292   }
00293 
00294   return errors;
00295 }
00296 
00297 int test_xform_constructors() 
00298 {
00299   int errors = 0;
00300   
00301     // integer constructor
00302   int test_int[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
00303   HomXform xform(test_int);
00304   for (int i = 0; i < 16; i++) {
00305     if (xform[i] != i) {
00306       std::cout << "HomXform integer array constructor failed." << std::endl;
00307       errors++;
00308     }
00309   }
00310 
00311   HomXform xform3(test_int[0], test_int[1], test_int[2], test_int[3],
00312                   test_int[4], test_int[5], test_int[6], test_int[7],
00313                   test_int[8], test_int[9], test_int[10], test_int[11],
00314                   test_int[12], test_int[13], test_int[14], test_int[15]);
00315   for (int i = 0; i < 16; i++) {
00316     if (xform3[i] != i) {
00317       std::cout << "HomXform integer constructor failed." << std::endl;
00318       errors++;
00319     }
00320   }
00321 
00322     // sample rotation, translation, and scaling matrices/vectors
00323   int rotate[] = {0, 1, 0, -1, 0, 0, 0, 0, 1};
00324   int translate[] = {4, 5, 6};
00325   int scale[] = {1, 1, 1};
00326   
00327     // construct an xform matrix based on those
00328   HomXform xform2(rotate, scale, translate);
00329   
00330     // test where those went in the xform
00331   if (!(xform2[XFORM_INDEX(0,0)] == rotate[0] &&
00332         xform2[XFORM_INDEX(0,1)] == rotate[1] &&
00333         xform2[XFORM_INDEX(0,2)] == rotate[2] &&
00334         xform2[XFORM_INDEX(1,0)] == rotate[3] &&
00335         xform2[XFORM_INDEX(1,1)] == rotate[4] &&
00336         xform2[XFORM_INDEX(1,2)] == rotate[5] &&
00337         xform2[XFORM_INDEX(2,0)] == rotate[6] &&
00338         xform2[XFORM_INDEX(2,1)] == rotate[7] &&
00339         xform2[XFORM_INDEX(2,2)] == rotate[8] &&
00340         xform2[XFORM_INDEX(3,0)] == translate[0] &&
00341         xform2[XFORM_INDEX(3,1)] == translate[1] &&
00342         xform2[XFORM_INDEX(3,2)] == translate[2] &&
00343         xform2[XFORM_INDEX(0,3)] == 0 &&
00344         xform2[XFORM_INDEX(1,3)] == 0 &&
00345         xform2[XFORM_INDEX(2,3)] == 0 &&
00346         xform2[XFORM_INDEX(3,3)] == 1)) {
00347       std::cout << "HomXform rotate, scale, translate constructor failed." << std::endl;
00348       errors++;
00349   }
00350 
00351   return errors;
00352 }
00353 
00354 int test_xform_operators() 
00355 {
00356   int errors = 0;
00357   
00358     // sample rotation, translation, and scaling matrices/vectors
00359   int rotate[] = {0, 1, 0, -1, 0, 0, 0, 0, 1};
00360   int translate[] = {4, 5, 6};
00361   int scale[] = {1, 1, 1};
00362   
00363     // construct an xform matrix based on those
00364   HomXform xform1(rotate, scale, translate);
00365   HomXform xform1a(rotate, scale, translate);
00366   
00367     // test operator==
00368   if (!(xform1 == xform1a)) {
00369     std::cout << "HomXform operator== failed." << std::endl;
00370     errors++;
00371   }
00372 
00373     // test operator!=
00374   xform1a[1] = 0;
00375   if (!(xform1 != xform1a)) {
00376     std::cout << "HomXform operator!= failed." << std::endl;
00377     errors++;
00378   }
00379 
00380     // test operator=
00381   HomXform xform1c = xform1;
00382   if (!(xform1c == xform1)) {
00383     std::cout << "HomXform operator= failed." << std::endl;
00384     errors++;
00385   }
00386 
00387   HomXform xform3 = xform1 * HomXform::IDENTITY;
00388   if (xform3 != xform1) {
00389     std::cout << "HomXform operator * failed." << std::endl;
00390     errors++;
00391   }
00392 
00393     // test operator*=
00394   xform3 *= HomXform::IDENTITY;
00395   if (xform3 != xform1) {
00396     std::cout << "HomXform operator *= failed." << std::endl;
00397     errors++;
00398   }
00399 
00400   return errors;
00401 }
00402 
00403 int test_xform_functions() 
00404 {
00405   int errors = 0;
00406 
00407     // HomCoord functions
00408     // length() and length_squared()
00409   HomCoord coord1(3, 4, 5);
00410   if (coord1.length_squared() != 50 ||
00411       coord1.length() != 7)   {
00412     std::cout << "HomCoord length() or length_squared() failed." << std::endl;
00413     errors++;
00414   }
00415 
00416     // normalize()
00417   coord1.normalize();
00418   HomCoord coord2(3, 0, 0);
00419   coord2.normalize();
00420   if (coord1.length_squared() != 0 ||
00421       coord2.length_squared() != 1)   {
00422     std::cout << "HomCoord normalize failed." << std::endl;
00423     errors++;
00424   }
00425   
00426     // sample rotation, translation, and scaling matrices/vectors
00427   int inv_int[] = {0, 1, 0, 0, -1, 0, 0, 0, 0, 0, 1, 0, 7, 5, 0, 1};
00428   
00429     // construct an xform matrix based on those
00430   HomXform xform1(inv_int);
00431 
00432   HomXform xform1_inv = xform1.inverse();
00433 
00434   HomXform xform2 = xform1 * xform1_inv;
00435   if (xform2 != HomXform::IDENTITY) {
00436     std::cout << "HomXform inverse failed." << std::endl;
00437     errors++;
00438   }
00439 
00440   return errors;
00441 }
00442 
00443 int test_coord_xform_operators() 
00444 {
00445   int errors = 0;
00446   
00447     // sample pt
00448   HomCoord test_pt(1, 2, 3);
00449   
00450   HomCoord test_pt2 = test_pt * HomXform::IDENTITY;
00451   if (test_pt2 != test_pt) {
00452     std::cout << "Coord-xform operator* failed." << std::endl;
00453     errors++;
00454   }
00455 
00456     // get an inverse transform quickly
00457   int rotate[] = {0, 1, 0, -1, 0, 0, 0, 0, 1};
00458   int translate[] = {4, 5, 6};
00459   int scale[] = {1, 1, 1};
00460   HomXform xform2(rotate, scale, translate);
00461 
00462     // operator*
00463   HomCoord ident(1, 1, 1, 1);
00464   HomCoord test_pt3 = ident * HomXform::IDENTITY;
00465   if (test_pt3 != ident) {
00466     std::cout << "Coord-xform operator* failed." << std::endl;
00467     errors++;
00468   }
00469     
00470     // operator/
00471   test_pt2 = (test_pt * xform2) / xform2;
00472   if (test_pt2 != test_pt) {
00473     std::cout << "Coord-xform operator/ failed." << std::endl;
00474     errors++;
00475   }
00476 
00477 
00478     // test three_pt_xform; use known transforms in most cases
00479   HomXform xform;
00480 
00481     // first test: i = j, j = -i, k = k, t = (7,5,0)
00482   xform.three_pt_xform(HomCoord(0,0,0,1), HomCoord(7,5,0,1),
00483                        HomCoord(0,3,0,1), HomCoord(4,5,0,1),
00484                        HomCoord(0,0,3,1), HomCoord(7,5,3,1));
00485 
00486   HomXform solution(0,1,0,0, -1,0,0,0,0,0,1,0,7,5,0,1);
00487   if (xform != solution) {
00488     std::cout << "Three-pt transform (general) test failed." << std::endl;
00489     errors++;
00490   }
00491   
00492     // now test 1d
00493   xform.three_pt_xform(HomCoord(0,0,0,1), HomCoord(7,5,0,1),
00494                        HomCoord(6,0,0,1), HomCoord(7,11,0,1),
00495                        HomCoord(0,0,0,1), HomCoord(7,5,0,1));
00496 
00497   solution = HomXform(0,1,0,0,1,0,0,0,0,0,-1,0,7,5,0,1);
00498   if (xform != solution) {
00499     std::cout << "Three-pt transform (1d) test failed." << std::endl;
00500     errors++;
00501   }
00502   
00503   
00504   return errors;
00505 }
00506   
00507 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines