Pose3d.hh
00001 /* 00002 * Gazebo - Outdoor Multi-Robot Simulator 00003 * Copyright (C) 2003 00004 * Nate Koenig & Andrew Howard 00005 * 00006 * This program is free software; you can redistribute it and/or modify 00007 * it under the terms of the GNU General Public License as published by 00008 * the Free Software Foundation; either version 2 of the License, or 00009 * (at your option) any later version. 00010 * 00011 * This program is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 * GNU General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU General Public License 00017 * along with this program; if not, write to the Free Software 00018 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00019 * 00020 */ 00021 /* Desc: External interfaces for Gazebo 00022 * Author: Nate Koenig 00023 * Date: 03 Apr 2007 00024 * SVN: $Id: Pose3d.hh 123 2007-09-19 20:36:14Z natepak $ 00025 */ 00026 00027 #ifndef POSE3D_HH 00028 #define POSE3D_HH 00029 00030 #include <iostream> 00031 00032 #include "Vector3.hh" 00033 #include "Quatern.hh" 00034 00035 00036 namespace gazebo 00037 { 00038 00042 00043 00045 class Pose3d 00046 { 00048 public: Pose3d(); 00049 00053 public: Pose3d( const Vector3 &pos, const Quatern &rot); 00054 00057 public: Pose3d( const Pose3d &pose ); 00058 00060 public: virtual ~Pose3d(); 00061 00063 public: bool IsFinite() const; 00064 00068 public: Pose3d operator+(const Pose3d &pose) const; 00069 00073 public: const Pose3d &operator+=(const Pose3d &pose); 00074 00078 public: Pose3d operator-(const Pose3d &pose) const; 00079 00083 public: const Pose3d &operator-=(const Pose3d &pose); 00084 00088 public: Vector3 CoordPositionAdd(const Vector3 &pos) const; 00089 00093 public: Vector3 CoordPositionAdd(const Pose3d &pose) const; 00094 00098 public: Vector3 CoordPositionSub(const Pose3d &pose) const; 00099 00103 public: Quatern CoordRotationAdd(const Quatern &rot) const; 00104 00108 public: Quatern CoordRotationSub(const Quatern &rot) const; 00109 00112 public: Pose3d CoordPoseSolve(const Pose3d &b) const; 00113 00115 public: void Reset(); 00116 00118 public: Vector3 pos; 00119 00121 public: Quatern rot; 00122 00127 public: friend std::ostream &operator<<(std::ostream &out, const gazebo::Pose3d &pose) 00128 { 00129 out << "Pos[" << pose.pos << "] Rot[" << pose.rot << "]"; 00130 return out; 00131 00132 } 00133 }; 00134 00136 00137 } 00138 00139 #endif 00140