Vector3.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/Vector3.h>
00035 #include <cassert>
00036 #include <math.h>
00037 #include <string.h>
00038 
00039 namespace octomath {
00040 
00041   Vector3& Vector3::rotate_IP (double roll, double pitch, double yaw) {
00042     double x, y, z;
00043     // pitch (around y)
00044     x = (*this)(0); z = (*this)(2);
00045     (*this)(0) = (float) (z * sin(pitch) + x * cos(pitch));
00046     (*this)(2) = (float) (z * cos(pitch) - x * sin(pitch));
00047 
00048 
00049     // yaw (around z)
00050     x = (*this)(0); y = (*this)(1);
00051     (*this)(0) = (float) (x * cos(yaw) - y * sin(yaw));
00052     (*this)(1) = (float) (x * sin(yaw) + y * cos(yaw));
00053 
00054     // roll (around x)
00055     y = (*this)(1); z = (*this)(2);
00056     (*this)(1) = (float) (y * cos(roll) - z * sin(roll));
00057     (*this)(2) = (float) (y * sin(roll) + z * cos(roll));
00058 
00059     return *this;
00060   }
00061 
00062 //   void Vector3::read(unsigned char * src, unsigned int size){
00063 //     memcpy(&data[0],src, sizeof(double));
00064 //     memcpy(&data[1],src, sizeof(double));
00065 //     memcpy(&data[2],src, sizeof(double));
00066 //   }
00067 
00068 
00069   std::istream& Vector3::read(std::istream &s) {
00070     int temp;
00071     s >> temp; // should be 3
00072     for (unsigned int i=0; i<3; i++)
00073       s >> operator()(i);
00074     return s;
00075   }
00076 
00077 
00078   std::ostream& Vector3::write(std::ostream &s) const {
00079     s << 3;
00080     for (unsigned int i=0; i<3; i++)
00081       s << " " << operator()(i);
00082     return s;
00083   }
00084 
00085 
00086 
00087   std::istream& Vector3::readBinary(std::istream &s) {
00088     int temp;
00089     s.read((char*)&temp, sizeof(temp));
00090     double val = 0;
00091     for (unsigned int i=0; i<3; i++) {
00092       s.read((char*)&val, sizeof(val));
00093       operator()(i) = (float) val;
00094     }
00095     return s;
00096   }
00097 
00098 
00099   std::ostream& Vector3::writeBinary(std::ostream &s) const {
00100     int temp = 3;
00101     s.write((char*)&temp, sizeof(temp));
00102     double val = 0;
00103     for (unsigned int i=0; i<3; i++) {
00104       val = operator()(i);
00105       s.write((char*)&val, sizeof(val));
00106     }
00107     return s;
00108   }
00109 
00110 
00111   std::ostream& operator<<(std::ostream& out, octomath::Vector3 const& v) {
00112     return out << '(' << v.x() << ' ' << v.y() << ' ' << v.z() << ')';
00113   }
00114 
00115 }
00116 


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14