vector3.hh

2007年机器人足球世界杯3D仿真组亚军

/*
 *  Little Green BATS (2006)
 *
 *  Authors: 	Martin Klomp ([e-mail])
 *		Mart van de Sanden ([e-mail])
 *		Sander van Dijk ([e-mail])
 *		A. Bram Neijt ([e-mail])
 *		Matthijs Platje ([e-mail])
 *
 *  Date: 	September 14, 2006
 *
 *  Website:	http://www.littlegreenbats.nl
 *
 *  Comment:	Please feel free to contact us if you have any 
 *		problems or questions about the code.
 *
 *
 *  License: 	This program is free software; you can redistribute 
 *		it and/or modify it under the terms of the GNU General
 *		Public License as published by the Free Software 
 *		Foundation; either version 2 of the License, or (at 
 *		your option) any later version.
 *
 *   		This program is distributed in the hope that it will
 *		be useful, but WITHOUT ANY WARRANTY; without even the
 *		implied warranty of MERCHANTABILITY or FITNESS FOR A
 *		PARTICULAR PURPOSE.  See the GNU General Public
 *		License for more details.
 *
 *   		You should have received a copy of the GNU General
 *		Public License along with this program; if not, write
 *		to the Free Software Foundation, Inc., 59 Temple Place - 
 *		Suite 330, Boston, MA  02111-1307, USA.
 *
 */







#ifndef __INC_BATS_VECTOR3_HH_
#define __INC_BATS_VECTOR3_HH_

#include <cstring>
#include <cassert>
#include <ostream>
#include <cmath>

namespace bats {

  template <class T>
  class Matrix9;

  /** \brief 3 element vector
   *
   *  The Vector3 class is a three dimensional vector class (3 values).
   */
  template <class T>
  class Vector3
  {
    template <class K> friend std::ostream &operator<<(std::ostream &, Vector3<K> const &);

    friend class Matrix9<T>;

    T d_values[3];

    void destroy()
    {
    }

    void copy(Vector3<T> const &other)
    {
      memcpy(reinterpret_cast<char*>(d_values),
	     reinterpret_cast<char const *>(other.d_values),
	     sizeof(T)*3);
    }

    Vector3(double values[])
    {
      memcpy(reinterpret_cast<char*>(d_values),
	     reinterpret_cast<char const *>(values),
	     sizeof(T)*3);
    }
    
  public:

    Vector3(Vector3<T> const &other) { copy(other); }

    /**
     *  A vector initalizes to zero.
     */
    Vector3() { d_values[0] = d_values[1] = d_values[2] = 0; }

    /**
     *  Initializes the vector to (x,y,z).
     */
    Vector3(T x, T y, T z)
    {
      d_values[0] = x;
      d_values[1] = y;
      d_values[2] = z;
    }

    ~Vector3() { destroy(); }

    /**
     *  @returns a pointer to the array of values.
     */
    T const *ptr() const { return d_values; }

    /**
     *  @returns a pointer to the array of values. This is an auto
     *           conversion function and makes it posible to
     *           use the vector in places where you normaly would
     *           have used a array of 3 values.
     */
    operator T /*const*/ *() /*const*/ { return d_values; }

    Vector3 &operator=(Vector3 const &other)
    {
      if (this != &other) {
        destroy();
        copy(other);
      }
      return *this;
    }

    /*T &operator[](unsigned index)
    {
      assert(index < 3);
      return d_values[index];
    }*/

    /** Set member \a idx to \a value */
    void set(unsigned idx, T value) { d_values[idx] = value; }
    
    /** Set X-coordinate to \a value */
    void setX(T value) { d_values[0] = value; }

    /** Set Y-coordinate to \a value */
    void setY(T value) { d_values[1] = value; }

    /** Set Z-coordinate to \a value */
    void setZ(T value) { d_values[2] = value; }

    
    /** Get member \a idx */
    T get(unsigned idx) const
    {
      assert(idx < 3);
      return d_values[idx];
    }

    /** Get X-coordinate */
    T getX() const{ return d_values[0]; }

    /** Get Y-coordinate */
    T getY() const { return d_values[1]; }

    /** Get Z-coordinate */
    T getZ() const { return d_values[2]; }

    /** Calculate vector length
     *
     * \f$ |\vec V| = \sqrt{X^2 + Y^2 + Z^2} \f$
     */
    T length() const
    {
      return sqrt(d_values[0]*d_values[0] +
		  d_values[1]*d_values[1] + 
		  d_values[2]*d_values[2]);
    }

    /**
     *  @returns a normalized vector. A vector is normalized when its euclidian length is one.
     */
    Vector3<T> normalize() const
    {
      return (*this)/length();
    }

    /** Calculate the cross product of this vector with another vector
     *
     * \f$ \vec c = \vec a \times \vec b \f$ <BR>
     * \f$ c_0 = a_1 \cdot b_2 - a_2 \cdot a_1 \f$ <BR>
     * \f$ c_1 = a_2 \cdot b_0 - a_0 \cdot a_2 \f$ <BR>
     * \f$ c_2 = a_3 \cdot b_1 - a_1 \cdot a_0 \f$ 
     */
    Vector3<T> crossProduct(Vector3<T> const &other) const
    {
      Vector3<T> res(
        (d_values[1] * other.d_values[2] - d_values[2] * other.d_values[1]),
        (d_values[2] * other.d_values[0] - d_values[0] * other.d_values[2]),
        (d_values[0] * other.d_values[1] - d_values[1] * other.d_values[0])
      );

      return res;
    }

    /** Calculate the dot product of this vector with another vector
     *
     * \f$ \vec c = \vec a \cdot \vec b \f$ <BR>
     * \f$ c_i = a_i \cdot b_i \f$
     */
    T dotProduct(Vector3<T> const &other) const
    {
      return (d_values[0]*other.d_values[0] +
	      d_values[1]*other.d_values[1] +
	      d_values[2]*other.d_values[2]);
    }

    /**
     *  Performs a dot (or inner) product.
     */
    T operator*(Vector3<T> const &other) const
    {
      return dotProduct(other);
    }
    
    /** Add another vector to this vector*/
    Vector3<T> operator+(Vector3 const &other) const
    {
      Vector3<T> res(
        d_values[0] + other.d_values[0],
        d_values[1] + other.d_values[1],
        d_values[2] + other.d_values[2]
      );

      return res;
    }

    /** Subtract another vector from this vector */
    Vector3<T> operator-(Vector3 const &other) const
    {
      Vector3<T> res(
        d_values[0] - other.d_values[0],
        d_values[1] - other.d_values[1],
        d_values[2] - other.d_values[2]
      );

      return res;
    }

    /** Multiply this vector with a scalar value */
    Vector3<T> operator*(T value) const
    {
      Vector3<T> res(
        d_values[0] * value,
        d_values[1] * value,
        d_values[2] * value
      );

      return res;
    }

    /** Divide this vector by a scalar value */
    Vector3<T> operator/(T value) const
    {
      Vector3<T> res(
        d_values[0] / value,
        d_values[1] / value,
        d_values[2] / value
      );

      return res;
    }

    /**
     *  @returns the negative version of the vector.
     */
    Vector3<T> operator-()
    {
      Vector3<T> res(
        -d_values[0],
        -d_values[1],
        -d_values[2]
      );

      return res;
    }

    /**
     *  Adds other to this one.
     */
    Vector3<T> &operator+=(Vector3 const &other)
    {
      d_values[0] += other.d_values[0];
      d_values[1] += other.d_values[1];
      d_values[2] += other.d_values[2];

      return *this;
    }

    /**
     *  Substracts other from this one.
     */
    Vector3<T> &operator-=(Vector3 const &other)
    {
      d_values[0] -= other.d_values[0];
      d_values[1] -= other.d_values[1];
      d_values[2] -= other.d_values[2];

      return *this;
    }

    /** Divide this vector by a scalar value */
    Vector3<T> &operator/=(T value)
    {
      d_values[0] /= value;
      d_values[1] /= value;
      d_values[2] /= value;

      return *this;
    }

    /** Multiply this vector by a scalar value */
    Vector3<T> &operator*=(T value)
    {
      d_values[0] *= value;
      d_values[1] *= value;
      d_values[2] *= value;

      return *this;
    }

    /** Compare this vector to another vector */
    bool operator==(Vector3 const &other) const
    {
      return d_values[0] == other.d_values[0] &&
             d_values[1] == other.d_values[1] &&
             d_values[2] == other.d_values[2];
    }
    
    bool operator!=(Vector3 const &other) const
    {
      return !(*this == other);
    }

    bool operator<(Vector3 const& other) const
    {
      return length() < other.length();
    }
    
    /** Calculate angle between this vector and another vector
     *
     * \f$ cos(\alpha) = {{\vec a \cdot \vec b} \over {| \vec a | | \vec b | }}\f$ <BR>
     *
     */
    double angle(Vector3 &other) const
    {
      double dp = dotProduct(other);
      return acos(dp / (length() * other.length()));
    }

    /**
     *  Rotates the vector around the Z-ax by an angle of angle.
     *
     *  Might not be used anymore.
     */
    Vector3<T> rotateZ(double angle)
    {
      Matrix9<T> rotMat;
      rotMat.set(0,0, cos(angle));
      rotMat.set(0,1, sin(angle));
      rotMat.set(1,0, -sin(angle));
      rotMat.set(1,1, cos(angle));
      rotMat.set(2, 2, 1.0);
      Vector3<T> res;
      Matrix9<T>::mul(res, rotMat, *this);
      return res;
    }

    /**
     *  @returns false is one of the values is NaN.
     */
    bool isValid()
    {
    	return !std::isnan(d_values[0]) && !std::isnan(d_values[1]) && !std::isnan(d_values[2]);
    }
  };

  /**
   *  A vector of doubles.
   */
  typedef Vector3<double> Vector3D;

  /**
   *  A vector of floats.
   */
  typedef Vector3<float>  Vector3F;

  template <class K>
  std::ostream &operator<<(std::ostream &_os, Vector3<K> const &_vect)
  {
    return _os << "(" << _vect.d_values[0] << "," << _vect.d_values[1] << "," << _vect.d_values[2] << ")";
  }

};

#endif // __INC_BATS_VECTOR3_HH_