Pose6D.cpp
Go to the documentation of this file.
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 #include <octomap/math/Pose6D.h>
00035 #include <math.h>
00036 
00037 namespace octomath {
00038 
00039   Pose6D::Pose6D() {
00040   }
00041 
00042 
00043   Pose6D::Pose6D(const Vector3 &trans, const Quaternion &rot) :
00044     translation(trans),
00045     rotation(rot)
00046   { }
00047 
00048 
00049 
00050   Pose6D::Pose6D(float x, float y, float z, double roll, double pitch, double yaw) :
00051     translation(x, y, z),
00052     rotation(roll, pitch, yaw)
00053   { }
00054 
00055   Pose6D::~Pose6D() {
00056   }
00057 
00058   Pose6D& Pose6D::operator= (const Pose6D& other) {
00059     translation = other.trans();
00060     rotation = other.rot();
00061     return *this;
00062   }
00063 
00064 
00065   Pose6D Pose6D::inv() const {
00066     Pose6D result(*this);
00067     result.rot() = rot().inv().normalized();
00068     result.trans() = result.rot().rotate(-trans());
00069     return result;
00070   }
00071 
00072 
00073   Pose6D& Pose6D::inv_IP() {
00074     rot() = rot().inv().normalized();
00075     trans() = rot().rotate(-trans());
00076     return *this;
00077   }
00078 
00079 
00080   Vector3 Pose6D::transform (const Vector3 &v) const {
00081     Vector3 res = this->rot().rotate(v);
00082     res = res + this->trans();
00083     return res;
00084   }
00085 
00086   Pose6D Pose6D::operator* (const Pose6D& other) const {
00087     Quaternion rot_new   = rotation * other.rot();
00088     Vector3    trans_new = rotation.rotate(other.trans()) + trans();
00089     return Pose6D(trans_new, rot_new.normalized());
00090   }
00091 
00092   const Pose6D& Pose6D::operator*= (const Pose6D& other) {
00093     trans() += rotation.rotate(other.trans());
00094     rot() = rot() * other.rot();
00095     return *this;
00096   }
00097 
00098   double Pose6D::distance (const Pose6D &other) const {
00099     double dist_x = x() - other.x();
00100     double dist_y = y() - other.y();
00101     double dist_z = z() - other.z();
00102     return sqrt(dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
00103   }
00104 
00105   double Pose6D::transLength() const {
00106     return sqrt(x()*x() + y()*y() + z()*z());
00107   }
00108 
00109 
00110   bool Pose6D::operator==(const Pose6D& other) const {
00111     return translation == other.translation
00112       && rotation == other.rotation;
00113   }
00114 
00115   bool Pose6D::operator!=(const Pose6D& other) const {
00116     return !(*this == other);
00117   }
00118 
00119   std::istream& Pose6D::read(std::istream &s) {
00120     translation.read(s);
00121     rotation.read(s);
00122     return s;
00123   }
00124 
00125 
00126   std::ostream& Pose6D::write(std::ostream &s) const {
00127     translation.write(s);
00128     s << " ";
00129     rotation.write(s);
00130     return s;
00131   }
00132 
00133 
00134   std::istream& Pose6D::readBinary(std::istream &s) {
00135     translation.readBinary(s);
00136     rotation.readBinary(s);
00137     return s;
00138   }
00139 
00140 
00141   std::ostream& Pose6D::writeBinary(std::ostream &s) const {
00142     translation.writeBinary(s);
00143     rotation.writeBinary(s);
00144     return s;
00145   }
00146 
00147   std::ostream& operator<<(std::ostream& s, const Pose6D& p) {
00148     s << "(" << p.x() << " " << p.y() << " " << p.z()
00149       << ", " << p.rot().u() << " " << p.rot().x() << " " << p.rot().y() << " " << p.rot().z() << ")";
00150     return s;
00151   }
00152 
00153 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:50:59