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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:41:51