Vector3.h

Go to the documentation of this file.

/*
 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
 * http://octomap.github.com/
 *
 * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
 * All rights reserved.
 * License: New BSD
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University of Freiburg nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef OCTOMATH_VECTOR3_H
#define OCTOMATH_VECTOR3_H

#include <iostream>
#include <math.h>


namespace octomath {

  class Vector3 {
  public:

    Vector3 () { data[0] = data[1] = data[2] = 0.0; }

    Vector3 (const Vector3& other) {
      data[0] = other(0);
      data[1] = other(1);
      data[2] = other(2);
    }

    Vector3 (float x, float y, float z) {
      data[0] = x;
      data[1] = y;
      data[2] = z;
    }


    /* inline Eigen3::Vector3f getVector3f() const { return Eigen3::Vector3f(data[0], data[1], data[2]) ; } */
    /* inline Eigen3::Vector4f& getVector4f() { return data; } */
    /* inline Eigen3::Vector4f getVector4f() const { return data; } */

    inline Vector3& operator= (const Vector3& other)  {
      data[0] = other(0);
      data[1] = other(1);
      data[2] = other(2);      
      return *this;
    }


    inline Vector3 cross (const Vector3& other) const 
    {
      //return (data.start<3> ().cross (other.data.start<3> ()));
      // \note should this be renamed?
      return Vector3(y()*other.z() - z()*other.y(),
                     z()*other.x() - x()*other.z(),
                     x()*other.y() - y()*other.x());
    }

    inline double dot (const Vector3& other) const 
    {
      return x()*other.x() + y()*other.y() + z()*other.z();
    }

    inline const float& operator() (unsigned int i) const
    {
      return data[i];
    }
    inline float& operator() (unsigned int i)
    {
      return data[i];
    }

    inline float& x() 
    {
      return operator()(0);
    }

    inline float& y() 
    {
      return operator()(1);
    }

    inline float& z() 
    {
      return operator()(2);
    }

    inline const float& x() const 
    {
      return operator()(0);
    }

    inline const float& y() const 
    {
      return operator()(1);
    }

    inline const float& z() const 
    {
      return operator()(2);
    }

    inline float& roll() 
    {
      return operator()(0);
    }

    inline float& pitch() 
    {
      return operator()(1);
    }

    inline float& yaw() 
    {
      return operator()(2);
    }

    inline const float& roll() const 
    {
      return operator()(0);
    }

    inline const float& pitch() const 
    {
      return operator()(1);
    }

    inline const float& yaw() const 
    {
      return operator()(2);
    }

    inline Vector3 operator- () const 
    {
      Vector3 result;
      result(0) = -data[0];
      result(1) = -data[1];
      result(2) = -data[2];
      return result;
    }

    inline Vector3 operator+ (const Vector3 &other) const 
    {
      Vector3 result(*this);
      result(0) += other(0);
      result(1) += other(1);
      result(2) += other(2);
      return result;
    }

    inline Vector3 operator*  (float x) const {
      Vector3 result(*this);
      result(0) *= x;
      result(1) *= x;
      result(2) *= x;
      return result;
    }

    inline Vector3 operator- (const Vector3 &other) const 
    {
      Vector3 result(*this);
      result(0) -= other(0);
      result(1) -= other(1);
      result(2) -= other(2);
      return result;
    }

    inline void operator+= (const Vector3 &other)
    {
      data[0] += other(0);
      data[1] += other(1);
      data[2] += other(2);      
    }

    inline void operator-= (const Vector3& other) {
      data[0] -= other(0);
      data[1] -= other(1);
      data[2] -= other(2);      
    }

    inline void operator/= (float x) {
      data[0] /= x;
      data[1] /= x;
      data[2] /= x;      
    }

    inline void operator*= (float x) {    
      data[0] *= x;
      data[1] *= x;
      data[2] *= x;      
    }

    inline bool operator== (const Vector3 &other) const {
      for (unsigned int i=0; i<3; i++) {
        if (operator()(i) != other(i)) 
          return false;
      }
      return true;
    }
        
    inline double norm () const {
      return sqrt(norm_sq());
    }

    inline double norm_sq() const {
      return (x()*x() + y()*y() + z()*z());
    }

    inline Vector3& normalize () {
      double len = norm();
      if (len > 0)
        *this /= (float) len;
      return *this;
    }

    inline Vector3 normalized () const {
      Vector3 result(*this);
      result.normalize ();
      return result;
    }

    inline double angleTo(const Vector3& other) const { 
      double dot_prod = this->dot(other);
      double len1 = this->norm();
      double len2 = other.norm();
      return acos(dot_prod / (len1*len2));
    }


    inline double distance (const Vector3& other) const {
      double dist_x = x() - other.x();
      double dist_y = y() - other.y();
      double dist_z = z() - other.z();
      return sqrt(dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
    }

    inline double distanceXY (const Vector3& other) const {
      double dist_x = x() - other.x();
      double dist_y = y() - other.y();
      return sqrt(dist_x*dist_x + dist_y*dist_y);
    }

    Vector3& rotate_IP (double roll, double pitch, double yaw);

    //    void read (unsigned char * src, unsigned int size);
    std::istream& read(std::istream &s);
    std::ostream& write(std::ostream &s) const;
    std::istream& readBinary(std::istream &s);
    std::ostream& writeBinary(std::ostream &s) const;


  protected:
    float data[3];

  };


  std::ostream& operator<<(std::ostream& out, octomath::Vector3 const& v);

}


#endif