octomap 1.5.0
include/octomap/math/Pose6D.h
Go to the documentation of this file.
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