rotate_translate_transform.cc
Go to the documentation of this file.00001 00002 00005 00006 00007 #include <art_map/rotate_translate_transform.h> 00008 #include <math.h> 00009 00014 void rotate_translate_transform::find_transform(const posetype& c1, 00015 const posetype& c2) { 00016 // ROTATION*(C1x,C1y)+(OffsetX,OffsetY)=(C2_X,C2_Y) 00017 // ROTATION=C2_TH-C1_TH 00018 00019 actual_transform.theta=c2.theta-c1.theta; 00020 00021 actual_cs=cosf(actual_transform.theta); 00022 actual_sn=sinf(actual_transform.theta); 00023 00024 float rotated_x=c1.x*actual_cs+c1.y*-actual_sn; 00025 float rotated_y=c1.x*actual_sn+c1.y*actual_cs; 00026 00027 float offset_x=c2.x-rotated_x; 00028 float offset_y=c2.y-rotated_y; 00029 00030 actual_transform.x=offset_x; 00031 actual_transform.y=offset_y; 00032 } 00033 00037 rotate_translate_transform::rotate_translate_transform() { 00038 actual_transform.x=actual_transform.y=actual_transform.theta=0; 00039 actual_cs=1; 00040 actual_sn=0; 00041 } 00042 00047 posetype rotate_translate_transform::apply_transform(const posetype& c1) const { 00048 00049 float rotated_x=c1.x*actual_cs+c1.y*-actual_sn; 00050 float rotated_y=c1.x*actual_sn+c1.y*actual_cs; 00051 00052 posetype c2; 00053 00054 c2.x=rotated_x+actual_transform.x; 00055 c2.y=rotated_y+actual_transform.y; 00056 c2.theta=c1.theta+actual_transform.theta; 00057 00058 return c2; 00059 } 00060 00065 posetype rotate_translate_transform::apply_inverse_transform(const posetype& c2) const{ 00066 00067 posetype c1; 00068 00069 float rotated_x=c2.x-actual_transform.x; 00070 float rotated_y=c2.y-actual_transform.y; 00071 00072 c1.theta=c2.theta-actual_transform.theta; 00073 00074 c1.x=rotated_x*actual_cs+rotated_y*actual_sn; 00075 c1.y=rotated_x*-actual_sn+rotated_y*actual_cs; 00076 00077 return c1; 00078 }