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/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 }