Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
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
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
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
00063
00064
00065
00066
00067
00068
00069 std::istream& Vector3::read(std::istream &s) {
00070 int temp;
00071 s >> temp;
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