moab
|
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