00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <geometry/hom_transform.h>
00025 #include <geometry/hom_point.h>
00026 #include <utils/time/tracker.h>
00027
00028 #include <iostream>
00029 #include <cmath>
00030
00031 using namespace fawkes;
00032 using namespace std;
00033
00034 int
00035 main(int argc, char **argv)
00036 {
00037 TimeTracker *tt = new TimeTracker();
00038 unsigned int loop_count = 0;
00039 unsigned int ttc_trans = tt->add_class("Tra");
00040 unsigned int ttc_rot = tt->add_class("Rot");
00041 unsigned int ttc_inv = tt->add_class("Inv");
00042
00043 HomTransform ht;
00044 for (loop_count = 0; loop_count < 10; ++loop_count) {
00045 tt->ping_start(ttc_trans);
00046 ht.trans(1, 2, 3);
00047 tt->ping_end(ttc_trans);
00048
00049 tt->ping_start(ttc_rot);
00050 ht.rotate_x(M_PI_2);
00051 tt->ping_end(ttc_rot);
00052
00053 tt->ping_start(ttc_trans);
00054 ht.trans(1, 2, 3);
00055 tt->ping_end(ttc_trans);
00056
00057 tt->ping_start(ttc_rot);
00058 ht.rotate_y(23);
00059 tt->ping_end(ttc_rot);
00060
00061 tt->ping_start(ttc_trans);
00062 ht.trans(1, 2, 3);
00063 tt->ping_end(ttc_trans);
00064
00065 tt->ping_start(ttc_rot);
00066 ht.rotate_z(M_PI_2);
00067 tt->ping_end(ttc_rot);
00068
00069 tt->ping_start(ttc_inv);
00070 ht.invert();
00071 tt->ping_end(ttc_inv);
00072
00073 tt->ping_start(ttc_inv);
00074 ht.invert();
00075 tt->ping_end(ttc_inv);
00076 }
00077
00078 ht.print_info("HomTransform");
00079 HomPoint p0 = HomPoint(0.1f, 0.2f, 0.3f);
00080 cout << "0: " << p0 << endl << endl << endl;
00081
00082 HomPoint p = ht * p0;
00083 cout << "p: " << p << endl << endl << endl;
00084
00085 ht.invert().print_info("HomTransform inverted");
00086 p0 = ht * p;
00087 cout << "0': " << p0 << endl << endl << endl;
00088
00089 ht.invert().print_info("HomTransform");
00090 tt->print_to_stdout();
00091
00092 ht *= ht;
00093
00094 delete tt;
00095 }
00096
00097
00098