orca-robotics
INTRODUCTION
Overview
Download and Install
Documentation
REPOSITORY
Interfaces
Drivers
Libraries
Utilities
Software Map
DEVELOPER
Dashboard
PEOPLE
Contributors
Users
Project
Download
Mailing lists
offset.h
00001 /* 00002 * Orca-Robotics Project: Components for robotics 00003 * http://orca-robotics.sf.net/ 00004 * Copyright (c) 2004-2008 Alex Brooks 00005 * 00006 * This distribution is licensed to you under the terms described in 00007 * the LICENSE file included in this distribution. 00008 * 00009 */ 00010 #ifndef ORCA_NAV_UTIL_OFFSET__H 00011 #define ORCA_NAV_UTIL_OFFSET__H 00012 00013 #include <iostream> 00014 #include <gbxsickacfr/gbxutilacfr/mathdefs.h> 00015 00016 // 00017 // A collection of small utility functions for doing 00018 // navigation-related things. 00019 // 00020 // @author Alex Brooks, Tobias Kaupp 00021 // 00022 00023 namespace hydronavutil { 00024 00026 // use orcaobj/mathdefs version. 00027 inline void normaliseAngle( double &angle ) 00028 { NORMALISE_ANGLE(angle); } 00029 00035 inline void addPoseOffset( double &poseX, 00036 double &poseY, 00037 double &poseT, 00038 double offsetX, 00039 double offsetY, 00040 double offsetT, 00041 bool normaliseHeading ) 00042 { 00043 // Add x and y in the local coordinate frame 00044 poseX += offsetX*std::cos(poseT) - offsetY*std::sin(poseT); 00045 poseY += offsetX*std::sin(poseT) + offsetY*std::cos(poseT); 00046 00047 // Add the heading change 00048 poseT = poseT + offsetT; 00049 00050 if ( normaliseHeading ) 00051 { 00052 // normalise to [-pi,pi). 00053 normaliseAngle( poseT ); 00054 } 00055 } 00056 00062 inline void addPoseOffset( double startX, 00063 double startY, 00064 double startT, 00065 double offsetX, 00066 double offsetY, 00067 double offsetT, 00068 double &resultX, 00069 double &resultY, 00070 double &resultT, 00071 bool normaliseHeading ) 00072 { 00073 // Add x and y in the local coordinate frame 00074 resultX = startX + offsetX*std::cos(startT) - offsetY*std::sin(startT); 00075 resultY = startY + offsetX*std::sin(startT) + offsetY*std::cos(startT); 00076 00077 // Add the heading change 00078 resultT = startT + offsetT; 00079 00080 if ( normaliseHeading ) 00081 { 00082 // normalise to [-pi,pi). 00083 normaliseAngle( resultT ); 00084 } 00085 } 00086 00090 inline void subtractFinalPoseOffset( double startX, 00091 double startY, 00092 double startT, 00093 double offsetX, 00094 double offsetY, 00095 double offsetT, 00096 double &resultX, 00097 double &resultY, 00098 double &resultT, 00099 bool normaliseHeading ) 00100 { 00101 double st = std::sin(startT-offsetT); 00102 double ct = std::cos(startT-offsetT); 00103 resultX = startX - offsetX*ct + offsetY*st; 00104 resultY = startY - offsetX*st - offsetY*ct; 00105 00106 resultT = startT-offsetT; 00107 00108 if ( normaliseHeading ) 00109 normaliseAngle( resultT ); 00110 } 00111 00115 inline void subtractFinalPoseOffset( double &poseX, 00116 double &poseY, 00117 double &poseT, 00118 double offsetX, 00119 double offsetY, 00120 double offsetT, 00121 bool normaliseHeading ) 00122 { 00123 double resX, resY, resT; 00124 subtractFinalPoseOffset( poseX, poseY, poseT, 00125 offsetX, offsetY, offsetT, 00126 resX, resY, resT, 00127 normaliseHeading ); 00128 poseX = resX; poseY = resY; poseT = resT; 00129 } 00130 00135 void subtractInitialOffset( double &totalOffsetX, 00136 double &totalOffsetY, 00137 double &totalOffsetTheta, 00138 double initialOffsetX, 00139 double initialOffsetY, 00140 double initialOffsetTheta ); 00141 00146 void subtractInitialOffset( double totalOffsetX, 00147 double totalOffsetY, 00148 double totalOffsetTheta, 00149 double initialOffsetX, 00150 double initialOffsetY, 00151 double initialOffsetTheta, 00152 double &resultX, 00153 double &resultY, 00154 double &resultTheta ); 00155 00160 inline void transformPoint2d( double xIn, 00161 double yIn, 00162 double offsetX, 00163 double offsetY, 00164 double offsetT, 00165 double &xOut, 00166 double &yOut) 00167 { 00168 xOut = std::cos(offsetT) * (xIn+offsetX) + std::sin(offsetT) * (yIn+offsetY); 00169 yOut = -std::sin(offsetT) * (xIn+offsetX) + std::cos(offsetT) * (yIn+offsetY); 00170 } 00171 00176 inline void transformPoint2d( double &x, 00177 double &y, 00178 double offsetX, 00179 double offsetY, 00180 double offsetT ) 00181 { 00182 double xNew, yNew; 00183 transformPoint2d( x, y, offsetX, offsetY, offsetT, xNew, yNew ); 00184 x=xNew; y=yNew; 00185 } 00186 00188 inline void addPoseOffset( double *pose, const double *offset, bool normaliseHeading ) 00189 { 00190 addPoseOffset( pose[0], pose[1], pose[2], 00191 offset[0], offset[1], offset[2], 00192 normaliseHeading ); 00193 } 00194 00196 inline void addPoseOffset( const double *start, const double *offset, double *result, bool normaliseHeading ) 00197 { 00198 addPoseOffset( start[0], start[1], start[2], 00199 offset[0], offset[1], offset[2], 00200 result[0], result[1], result[2], 00201 normaliseHeading ); 00202 } 00203 00205 inline void subtractFinalPoseOffset( double *pose, const double *offset, bool normaliseHeading ) 00206 { 00207 subtractFinalPoseOffset( pose[0], pose[1], pose[2], 00208 offset[0], offset[1], offset[2], 00209 normaliseHeading ); 00210 } 00211 00213 inline void subtractFinalPoseOffset( const double *start, const double *offset, double *result, bool normaliseHeading ) 00214 { 00215 subtractFinalPoseOffset( start[0], start[1], start[2], 00216 offset[0], offset[1], offset[2], 00217 result[0], result[1], result[2], 00218 normaliseHeading ); 00219 } 00220 00221 inline void subtractInitialOffset( double *totalOffset, 00222 const double *initialOffset ) 00223 { 00224 return subtractInitialOffset( totalOffset[0], totalOffset[1], totalOffset[2], 00225 initialOffset[0], initialOffset[1], initialOffset[2] ); 00226 } 00227 00228 inline void subtractInitialOffset( const double *totalOffset, 00229 const double *initialOffset, 00230 double *result ) 00231 { 00232 return subtractInitialOffset( totalOffset[0], totalOffset[1], totalOffset[2], 00233 initialOffset[0], initialOffset[1], initialOffset[2], 00234 result[0], result[1], result[2] ); 00235 } 00236 00237 00238 } 00239 00240 #endif
