Vector3.hh

/*
 * Copyright 2011 Nate Koenig & Andrew Howard
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
/* Desc: The world; all models are collected here
 * Author: Nate Koenig
 * Date: 3 Apr 2007
 */

#ifndef VECTOR3_HH
#define VECTOR3_HH

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

namespace gazebo
{
    namespace math
  {
    
    class Vector3
    {
      public: Vector3();
    
      public: Vector3( const double &x, const double &y, const double &z );
    
      public: Vector3( const Vector3 &pt );
    
      public: virtual ~Vector3();
    
      public: double Distance( const Vector3 &pt ) const;
    
      public: double GetLength() const;
    
      public: double GetSquaredLength() const;
    
      public: void Normalize();
    
      public: Vector3 Round();
    
      public: Vector3 GetRounded() const;
    
      public: inline void Set(double x = 0, double y =0 , double z = 0)
              {
                this->x = x;
                this->y = y;
                this->z = z;
              }
    
      public: Vector3 GetCrossProd(const Vector3 &pt) const;
    
      public: double GetDotProd(const Vector3 &pt) const;
    
      public: Vector3 GetAbs() const;
    
      public: Vector3 GetPerpendicular() const;
    
      public: static Vector3 GetNormal(const Vector3 &v1, const Vector3 &v2, 
                                       const Vector3 &v3);
    
      public: double GetDistToPlane(const Vector3 &_dir, 
                                    const Vector3 &_planeNormal, 
                                    double _d) const;
      public: double GetDistToLine(const Vector3 &_pt1, const Vector3 &_pt2);
    
      public: void SetToMax(const Vector3 & v);
    
      public: void SetToMin(const Vector3 & v);
    
      public: const Vector3 &operator=( const Vector3 &pt );
    
      public: const Vector3 &operator=( double value );
    
      public: Vector3 operator+( const Vector3 &pt ) const;
    
      public: const Vector3 &operator+=( const Vector3 &pt );
    
      public: inline Vector3 operator-( const Vector3 &pt ) const
              {
                return Vector3(this->x - pt.x, this->y - pt.y, this->z - pt.z);
              }
    
      public: const Vector3 &operator-=( const Vector3 &pt );
    
      public: const Vector3 operator/( const Vector3 &pt ) const;
    
      public: const Vector3 &operator/=( const Vector3 &pt );
    
      public: const Vector3 operator/( double v ) const;
    
      public: const Vector3 &operator/=( double v );
    
      public: const Vector3 operator*( const Vector3 &pt ) const;
    
      public: const Vector3 &operator*=( const Vector3 &pt );
    
      public: const Vector3 operator*( double v ) const;
    
      public: const Vector3 &operator*=( double v );
    
      public: bool operator==( const Vector3 &pt ) const;
    
      public: bool operator!=( const Vector3 &pt ) const;
    
      public: bool IsFinite() const;
    
      public: inline void Correct()
              {
                if (!finite(this->x))
                  this->x = 0;
                if (!finite(this->y))
                  this->y = 0;
                if (!finite(this->z))
                  this->z = 0;
              }
    
      public: double operator[](unsigned int index) const;

      public: void Round(int _precision);

      public: bool Equal( const Vector3 &_v ) const;

      public: double x;
    
      public: double y;
    
      public: double z;
    
      public: friend std::ostream &operator<<( std::ostream &out, const gazebo::math::Vector3 &pt )
      {
        out << pt.x << " " << pt.y << " " << pt.z;
    
        return out;
      }
    
      public: friend std::istream &operator>>( std::istream &in, gazebo::math::Vector3 &pt )
      {
        // Skip white spaces
        in.setf( std::ios_base::skipws );
        in >> pt.x >> pt.y >> pt.z;
        return in;
      }
    };
    
  }

}
#endif