octomap 1.5.0
|
00001 // $Id: Pose6D.h 371 2012-05-02 15:52:02Z ahornung $ 00002 00011 /* 00012 * Copyright (c) 2009, K. M. Wurm, A. Hornung, University of Freiburg 00013 * All rights reserved. 00014 * 00015 * Redistribution and use in source and binary forms, with or without 00016 * modification, are permitted provided that the following conditions are met: 00017 * 00018 * * Redistributions of source code must retain the above copyright 00019 * notice, this list of conditions and the following disclaimer. 00020 * * Redistributions in binary form must reproduce the above copyright 00021 * notice, this list of conditions and the following disclaimer in the 00022 * documentation and/or other materials provided with the distribution. 00023 * * Neither the name of the University of Freiburg nor the names of its 00024 * contributors may be used to endorse or promote products derived from 00025 * this software without specific prior written permission. 00026 * 00027 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00028 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00029 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00030 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00031 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00032 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00033 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00034 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00035 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00036 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00037 * POSSIBILITY OF SUCH DAMAGE. 00038 */ 00039 00040 #ifndef OCTOMATH_POSE6D_H 00041 #define OCTOMATH_POSE6D_H 00042 00043 #include "Vector3.h" 00044 #include "Quaternion.h" 00045 00046 namespace octomath { 00047 00055 class Pose6D { 00056 public: 00057 00058 Pose6D(); 00059 ~Pose6D(); 00060 00066 Pose6D(const Vector3& trans, const Quaternion& rot); 00067 00075 Pose6D(float x, float y, float z, double roll, double pitch, double yaw); 00076 00077 Pose6D& operator= (const Pose6D& other); 00078 bool operator==(const Pose6D& other) const; 00079 bool operator!=(const Pose6D& other) const; 00080 00086 inline Vector3& trans() { return translation; } 00092 inline Quaternion& rot() { return rotation; } 00098 const Vector3& trans() const { return translation; } 00103 const Quaternion& rot() const { return rotation; } 00104 00105 00106 inline float& x() { return translation(0); } 00107 inline float& y() { return translation(1); } 00108 inline float& z() { return translation(2); } 00109 inline const float& x() const { return translation(0); } 00110 inline const float& y() const { return translation(1); } 00111 inline const float& z() const { return translation(2); } 00112 00113 inline double roll() const {return (rotation.toEuler())(0); } 00114 inline double pitch() const {return (rotation.toEuler())(1); } 00115 inline double yaw() const {return (rotation.toEuler())(2); } 00116 00125 Vector3 transform (const Vector3 &v) const; 00126 00133 Pose6D inv() const; 00134 00141 Pose6D& inv_IP(); 00142 00150 Pose6D operator* (const Pose6D &p) const; 00158 const Pose6D& operator*= (const Pose6D &p); 00159 00165 double distance(const Pose6D &other) const; 00166 00173 double transLength() const; 00174 00180 std::ostream& write(std::ostream &s) const; 00186 std::istream& read(std::istream &s); 00192 std::ostream& writeBinary(std::ostream &s) const; 00198 std::istream& readBinary(std::istream &s); 00199 00200 protected: 00201 Vector3 translation; 00202 Quaternion rotation; 00203 }; 00204 00206 std::ostream& operator<<(std::ostream& s, const Pose6D& p); 00207 00208 } 00209 00210 #endif