INTRODUCTION
Overview
Download and Install
Quick Start
Documentation
Publications
NONFRAMEWORK CODE
Driver Interfaces
Drivers
Libraries
Utilities
FRAMEWORK CODE
Interfaces
Components
Libraries
Utilities
Full Software Listings
DEVELOPER
Tutorials
Examples
Dev Guide
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-2009 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 <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 inline void getInverseTransform( double tX, double tY, double tTheta, 00238 double &invTX, double &invTY, double &invTTheta ) 00239 { 00240 double st = sin(-tTheta); 00241 double ct = cos(-tTheta); 00242 00243 invTX = -( ct*tX - st*tY ); 00244 invTY = -( st*tX + ct*tY ); 00245 invTTheta = -tTheta; 00246 } 00247 } 00248 00249 #endif
