Vector3.cpp
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
4  *
5  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6  * All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #include <octomap/math/Vector3.h>
35 #include <cassert>
36 #include <math.h>
37 #include <string.h>
38 
39 namespace octomath {
40 
41  Vector3& Vector3::rotate_IP (double roll, double pitch, double yaw) {
42  double x, y, z;
43  // pitch (around y)
44  x = (*this)(0); z = (*this)(2);
45  (*this)(0) = (float) (z * sin(pitch) + x * cos(pitch));
46  (*this)(2) = (float) (z * cos(pitch) - x * sin(pitch));
47 
48 
49  // yaw (around z)
50  x = (*this)(0); y = (*this)(1);
51  (*this)(0) = (float) (x * cos(yaw) - y * sin(yaw));
52  (*this)(1) = (float) (x * sin(yaw) + y * cos(yaw));
53 
54  // roll (around x)
55  y = (*this)(1); z = (*this)(2);
56  (*this)(1) = (float) (y * cos(roll) - z * sin(roll));
57  (*this)(2) = (float) (y * sin(roll) + z * cos(roll));
58 
59  return *this;
60  }
61 
62 // void Vector3::read(unsigned char * src, unsigned int size){
63 // memcpy(&data[0],src, sizeof(double));
64 // memcpy(&data[1],src, sizeof(double));
65 // memcpy(&data[2],src, sizeof(double));
66 // }
67 
68 
69  std::istream& Vector3::read(std::istream &s) {
70  int temp;
71  s >> temp; // should be 3
72  for (unsigned int i=0; i<3; i++)
73  s >> operator()(i);
74  return s;
75  }
76 
77 
78  std::ostream& Vector3::write(std::ostream &s) const {
79  s << 3;
80  for (unsigned int i=0; i<3; i++)
81  s << " " << operator()(i);
82  return s;
83  }
84 
85 
86 
87  std::istream& Vector3::readBinary(std::istream &s) {
88  int temp;
89  s.read((char*)&temp, sizeof(temp));
90  double val = 0;
91  for (unsigned int i=0; i<3; i++) {
92  s.read((char*)&val, sizeof(val));
93  operator()(i) = (float) val;
94  }
95  return s;
96  }
97 
98 
99  std::ostream& Vector3::writeBinary(std::ostream &s) const {
100  int temp = 3;
101  s.write((char*)&temp, sizeof(temp));
102  double val = 0;
103  for (unsigned int i=0; i<3; i++) {
104  val = operator()(i);
105  s.write((char*)&val, sizeof(val));
106  }
107  return s;
108  }
109 
110 
111  std::ostream& operator<<(std::ostream& out, octomath::Vector3 const& v) {
112  return out << '(' << v.x() << ' ' << v.y() << ' ' << v.z() << ')';
113  }
114 
115 }
116 
std::ostream & operator<<(std::ostream &s, const Pose6D &p)
user friendly output in format (x y z, u x y z) which is (translation, rotation)
Definition: Pose6D.cpp:147
std::istream & read(std::istream &s)
Definition: Vector3.cpp:69
const float & operator()(unsigned int i) const
Definition: Vector3.h:122
std::ostream & write(std::ostream &s) const
Definition: Vector3.cpp:78
Vector3 & rotate_IP(double roll, double pitch, double yaw)
Definition: Vector3.cpp:41
std::istream & readBinary(std::istream &s)
Definition: Vector3.cpp:87
std::ostream & writeBinary(std::ostream &s) const
Definition: Vector3.cpp:99
float & y()
Definition: Vector3.h:136
float & x()
Definition: Vector3.h:131
This class represents a three-dimensional vector.
Definition: Vector3.h:50
float & z()
Definition: Vector3.h:141


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06