stlvec.h

矩阵、向量以及四元数的封装类

//========================================================================================
// Copyright (C) 2009 by Ma Zhi-Hao, Key Lab.
//          		The Academy of Equipment Command & Technology, PLA
//          		HuaiRou, Beijing, 101416, China
// File 		：STLvec.h
// Version	：1.0
// Email  	：[e-mail]
// Address  : 164, P.O.Box 3380, Beijing
//========================================================================================

/** \file
 * Vector
 */

#ifndef __STLMATH_VEC_H__
#define __STLMATH_VEC_H__

namespace shooter{ namespace math{

/** \class Vector<N>
 * A vector template class with N dimensions.
 */
template <size_t N>
struct Vector
{
private:
	/// Components
	double								vec[N];
	/// Size in bytes of the components
	static const size_t 	size;

public:
	/** \name Construct
	 * @{ */
	/**
	 * Default constructure.
	 * @remarks The vector is not initialized in this case.
	 */
	Vector() {}

	/**
	 * Construct a vector with given values array.
	 * @param v The values array used to set the components.
	 */
	Vector(const double *v)
	{
		memcpy(vec, v, size);
	}

	/**
	 * Construct a vector with given type compatible values array.
	 * @param v The values array with compatible type used to set the components.
	 */
	template <typename T>
	Vector(const T *v)
	{
		for (size_t i = 0; i < N; ++i)
			vec[i] = v[i];
	}

	/**
	 * Copy constructure. Construct a vector with another one.
	 * @param v A vector used to copy size_to new vector.
	 */
	Vector(const Vector &v)
	{
		memcpy(vec, v.vec, size);
	}
	/** @} */

	/** \name Assignment
	 * @{ */
	/**
	 * Assignment operator, assign another vector's values size_to this one.
	 * @param v Another vector used to set this one.
	 * @return Reference of this vector.
	 */
	inline Vector &operator =(const Vector &v)
	{
		memcpy(vec, v.vec, size);
		return *this;
	}

	/**
	 * Assignment operator, set the vector's values using a values array.
	 * @param v Posize_ter of the values array used to set this vector.
	 * @return Reference of this vector.
	 */
	inline Vector &operator =(const double *v)
	{
		memcpy(vec, v, size);
		return *this;
	}

	/**
	 * Assignment operator, set this vector's values using a type compatible values array.
	 * @param v Posize_ter of the type compatible values array used to set this vector.
	 * @return Reference of this vector.
	 */
	template <typename T>
	inline Vector &operator =(const T *v)
	{
		for (size_t i = 0; i < N; ++i)
			vec[i] = v;
		return *this;
	}

	/**
	 * Assignment operator, set all values of this vector equal to the given value.
	 * @param v Value use to set the vector.
	 * @return Reference of this vector.
	 */
	inline Vector &operator =(double v)
	{
		for (size_t i = 0; i < N; ++i)
			vec[i] = v;
		return *this;
	}

	/**
	 * Set the values of this vector with another one.
	 * @param v Reference of vector used to set this one.
	 * @return Reference of this vector.
	 */
	inline Vector &set(const Vector &v)
	{
		memcpy(vec, v.vec, size);
		return *this;
	}

	/**
	 * Set the values of this vector with a values array.
	 * @param v Pointer of the values array used to set the vector.
	 * @return Reference of this vector.
	 */
	inline Vector &set(const double *v)
	{
		memcpy(vec, v, size);
		return *this;
	}

	/**
	 * Set the values of this vector with a type compatible values array.
	 * @param v Pointer of the type compatible values array used to set the vector.
	 * @return Reference of this vector.
	 */
	template <typename T>
	inline Vector &set(const T *v)
	{
		for (size_t i = 0; i < N; ++i)
			vec[i] = v[i];
		return *this;
	}

	/**
	 * Set all the components of this vector equal to the given value.
	 * @param v Value used to set the vector.
	 * @return Reference of this vector.
	 */
	inline Vector &set(double v)
	{
		for (size_t i = 0; i < N; ++i)
			vec[i] = v;
		return *this;
	}
	/** @} */

	/** \name Components visit
	 * @{ */
	/**
	 * Visit the n'th component of this vector.
	 * @param n Zero based index of the component wanted.
	 * @return Reference of the queried component.
	 */
	inline double &operator [](size_t n)
	{
		return vec[n];
	}

	/**
	 * Query the n'th component's value of this vector.
	 * @param n Zero based index of the component wanted.
	 * @return Value of queried component.
	 */
	inline double operator [](size_t n) const
	{
		return vec[n];
	}

	/**
	 * Visit the n'th component of this vector.
	 * @param n Zero based index of the component wanted.
	 * @return Reference of the queried component.
	 */
	inline double &get(size_t n)
	{
		return vec[n];
	}

	/**
	 * Query the n'th component's value of this vector.
	 * @param n Zero based index of the component wanted.
	 * @return Value of queried component.
	 */
	inline double get(size_t n) const
	{
		return vec[n];
	}

	/**
	 * Query the vector as a pointer of the components.
	 * @return The address of the first component in the vector.
	 */
	inline const double * const get() const
	{
		return vec;
	}
	/** @} */

	/** \name Vector operation
	 * @{ */
	/**
	 * Query the norm (magnitude) of this vector.
	 * @return Norm (magnitude) of this vector.
	 */
	inline double getNorm() const
	{
		double ret = 0;
		for (size_t i = 0; i < N; ++i)
			ret += vec[i] * vec[i];
		return sqrt(ret);
	}

	/**
	 * Query the squared norm (magnitude) of this vector.
	 * @return Squared norm (magnitude) of this vector.
	 */
	inline double getSquaredNorm() const
	{
		double ret = 0;
		for (size_t i = 0; i < N; ++i)
			ret += vec[i] * vec[i];
		return ret;
	}

	/**
	 * Return the unit vector in the direction of this vector.
	 * @return The unit vector in the direction of this vector.
	 * @remarks Attempting to nomalize a zero-vector will result in a divided by
   * zero error. This is as it should be... fix the calling code.
	 */
	inline Vector getUnit() const
	{
		return (*this) / getNorm();
	}

	/**
	 * Normalize this vector.
	 * @return Reference of this vector.
	 */
	inline Vector &normalize()
	{
		double len = 0;
		for (size_t i = 0; i < N; ++i)
			len += vec[i] * vec[i];
		if (len < 1e-6)
			return *this;
		len = 1.0 / len;
		for (size_t i = 0; i < N; ++i)
			vec[i] *= len;
		return *this;
	}
	/** @} */

	/** \name operators
	 * @{ */
	/**
	 * Unary + operator
	 */
	inline Vector operator +() const
	{
		return *this;
	}

	/**
	 * Unary - operator
	 */
	inline Vector operator -() const
	{
		Vector v;
		for (size_t i = 0; i < N; ++i)
			v.vec[i] = -vec[i];
		return v;
	}

	/**
	 * Add another vector onto this one.
	 * @param v Reference of the vector will be added onto this one.
	 * @return Reference of this vector.
	 */
	inline Vector &operator +=(const Vector &v)
	{
		for (size_t i = 0; i < N; ++i)
			vec[i] += v.vec[i];
		return *this;
	}

	/**
	 * Subtract another vector from this one.
	 * @param v Reference of the vector will be subtracted from this one.
	 * @return Reference of this vector.
	 */
	inline Vector &operator -=(const Vector &v)
	{
		for (size_t i = 0; i < N; ++i)
			vec[i] -= v.vec[i];
		return *this;
	}

	/**
	 * Multiply this vector with a scalar.
	 * @param s The scalar.
	 * @return Reference of this vector.
	 */
	inline Vector &operator *=(double s)
	{
		for (size_t i = 0; i < N; ++i)
			vec[i] *= s;
		return *this;
	}

	/**
	 * Divide this vector by a scalar.
	 * @param s The scalar.
	 * @return Reference of this vector.
	 */
	inline Vector &operator /=(double s)
	{
		s = 1.0 / s;
		for (size_t i = 0; i < N; ++i)
			vec[i] *= s;
		return *this;
	}

	/**
	 * Add two vectors.
	 * @param v1 Reference of the first vector.
	 * @param v2 Reference of the second vector.
	 * @return Vector hold on the result.
	 */
	friend inline Vector operator +(const Vector &v1, const Vector &v2)
	{
		Vector v;
		for (size_t i = 0; i < N; ++i)
			v.vec[i] = v1.vec[i] + v2.vec[i];
		return v;
	}

	/**
	 * Subtract two vectors.
	 * @param v1 Reference of the first vector.
	 * @param v2 Reference of the second vector.
	 * @return Vector hold on the result.
	 */
	friend inline Vector operator -(const Vector &v1, const Vector &v2)
	{
		Vector &v;
		for (size_t i = 0; i < N; ++i)
			v.vec[i] = v1.vec[i] - v2.vec[i];
		return v;
	}

	/**
	 * Dot multiply two vectors.
	 * @param v1 Reference of the first vector.
	 * @param v2 Reference of the second vector.
	 * @return Dot product of the two vectors.
	 */
	friend inline double operator *(const Vector &v1, const Vector &v2)
	{
		double ret = 0;
		for (size_t i = 0; i < N; ++i)
			ret += v1.vec[i] * v2.vec[i];
		return ret;
	}

	/**
	 * Multiply a vector with a scalar.
	 * @param v1 Reference of the vector.
	 * @param s Reference of the scalar.
	 * @return Vector hold on the result.
	 */
	friend inline Vector operator *(const Vector &v1, double s)
	{
		Vector v;
		for (size_t i = 0; i < N; ++i)
			v.vec[i] = v1.vec[i] * s;
		return v;
	}

	/**
	 * Multiply a vector with a scalar.
	 * @param s Reference of the scalar.
	 * @param v1 Reference of the vector.
	 * @return Vector hold on the result.
	 */
	friend inline Vector operator *(double s, const Vector &v1)
	{
		Vector v;
		for (size_t i = 0; i < N; ++i)
			v.vec[i] = s * v1.vec[i];
		return v;
	}

	/**
	 * Divide a vector by a scalar.
	 * @param v1 Reference of the vector.
	 * @param s Reference of the scalar.
	 * @return Vector hold on the result.
	 */
	friend inline Vector operator /(const Vector &v1, double s)
	{
		Vector v;
		s = 1.0 / s;
		for (size_t i = 0; i < N; ++i)
			v.vec[i] = v1.vec[i] * s;
		return v;
	}

	/**
	 * Check if the two vectors are equal.
	 * @param v1 Reference of the first vector.
	 * @param v2 Reference of the second vector.
	 * @return True if the two vector are equal, else return false.
	 */
	friend inline bool operator ==(const Vector &v1, const Vector &v2)
	{
		for (size_t i = 0; i < N; ++i)
			if (v1.vec[i] != v2.vec[i])
				return false;
		return true;
	}

	/**
	 * Check if the two vectors are not equal.
	 * @param v1 Reference of the first vector.
	 * @param v2 Reference of the second vector.
	 * @return True if the two vectors are not equal, else return false.
	 */
	friend inline bool operator !=(const Vector &v1, const Vector &v2)
	{
		for (size_t i = 0; i < N; ++i)
			if (v1.vec[i] != v2.vec[i])
				return true;
		return false;
	}
	/** @} */
};

template <size_t N>
const size_t Vector<N>::size = sizeof(double) * N;

/** \class Vector<2>
 * A specified template class for 2D vector.
 */
template <>
struct Vector<2>
{
public:
	/// Components
	union
	{
		struct
		{
			/// The x component
			double				x;
			/// The y component
			double				y;
		};
		/// Components array
		double					vec[2];
	};

	/** \name Construct
	 * @{ */
	/**
	 * Default constructure.