00001 /* 00002 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 00003 * http://octomap.github.com/ 00004 * 00005 * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 00006 * All rights reserved. 00007 * License: New BSD 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above copyright 00015 * notice, this list of conditions and the following disclaimer in the 00016 * documentation and/or other materials provided with the distribution. 00017 * * Neither the name of the University of Freiburg nor the names of its 00018 * contributors may be used to endorse or promote products derived from 00019 * this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00022 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00023 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00024 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00025 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00026 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00027 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00028 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00029 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00030 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 * POSSIBILITY OF SUCH DAMAGE. 00032 */ 00033 00034 #ifndef OCTOMATH_POSE6D_H 00035 #define OCTOMATH_POSE6D_H 00036 00037 #include "Vector3.h" 00038 #include "Quaternion.h" 00039 00040 namespace octomath { 00041 00049 class Pose6D { 00050 public: 00051 00052 Pose6D(); 00053 ~Pose6D(); 00054 00060 Pose6D(const Vector3& trans, const Quaternion& rot); 00061 00069 Pose6D(float x, float y, float z, double roll, double pitch, double yaw); 00070 00071 Pose6D& operator= (const Pose6D& other); 00072 bool operator==(const Pose6D& other) const; 00073 bool operator!=(const Pose6D& other) const; 00074 00080 inline Vector3& trans() { return translation; } 00086 inline Quaternion& rot() { return rotation; } 00092 const Vector3& trans() const { return translation; } 00097 const Quaternion& rot() const { return rotation; } 00098 00099 00100 inline float& x() { return translation(0); } 00101 inline float& y() { return translation(1); } 00102 inline float& z() { return translation(2); } 00103 inline const float& x() const { return translation(0); } 00104 inline const float& y() const { return translation(1); } 00105 inline const float& z() const { return translation(2); } 00106 00107 inline double roll() const {return (rotation.toEuler())(0); } 00108 inline double pitch() const {return (rotation.toEuler())(1); } 00109 inline double yaw() const {return (rotation.toEuler())(2); } 00110 00119 Vector3 transform (const Vector3 &v) const; 00120 00127 Pose6D inv() const; 00128 00135 Pose6D& inv_IP(); 00136 00144 Pose6D operator* (const Pose6D &p) const; 00152 const Pose6D& operator*= (const Pose6D &p); 00153 00159 double distance(const Pose6D &other) const; 00160 00167 double transLength() const; 00168 00174 std::ostream& write(std::ostream &s) const; 00180 std::istream& read(std::istream &s); 00186 std::ostream& writeBinary(std::ostream &s) const; 00192 std::istream& readBinary(std::istream &s); 00193 00194 protected: 00195 Vector3 translation; 00196 Quaternion rotation; 00197 }; 00198 00200 std::ostream& operator<<(std::ostream& s, const Pose6D& p); 00201 00202 } 00203 00204 #endif