matrix3.h

GPS坐标转换软件与源程序 支持世界上大多数坐标框架下

/* Combat Simulator Project
 * Copyright (C) 2002, 2003, 2005 Mark Rose <[e-mail]>
 *
 * 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.
 */


/**
 * @file Matrix3.h
 *
 * A three-by-three matrix class.
 *
 * This source code was originally based on the Matrix class of
 * the OpenSceneGraph library, Copyright 1998-2003 Robert Osfield.
 * Source code from OpenSceneGraph is used here under the GNU General
 * Public License version 2 or later, as permitted under the
 * OpenSceneGraph Public License version 0.0 (exception 3) and the GNU
 * Lesser Public  License version 2 (clause 3).
 **/

#ifndef __CSPLIB_DATA_MATRIX3_H__
#define __CSPLIB_DATA_MATRIX3_H__



#include "Vector3.h"

#include <vector>
#include <cassert>
#include <cmath>
#include "math.h"

class Quat;
class CArchive;

template<class _Ty> inline
	void swap(_Ty& _X, _Ty& _Y)
	{_Ty _Tmp = _X;
	_X = _Y, _Y = _Tmp; }
/**
 * @brief A three-by-three matrix class using double-precision.
 *
 * @ingroup BaseTypes
 */
class  Matrix3
{
public: // BaseType

	/// String representation.
	std::string asString() const;

	/// Type representation.
	std::string typeString() const { return "Matrix3"; }

	/// Serialize from a Reader.
	void serialize(CArchive*);

	
	/** Extract the matrix values from XML character data.
	 *
	 *  The nine values should be separated by white-space
	 *  and arranged in order (0,0), (0,1), (0,2), (1,0), etc.
	 */
	void parseXML(const char* cdata);

	/// XML post processing.
	void convertXML() {}

public:

	/** Null matrix.
	 */
	static const Matrix3 ZERO;

	/** Identity matrix.
	 */
	static const Matrix3 IDENTITY;

	/** Default constructor.
	 *
	 *  @note The default constructor does @b not initialize the
	 *  matrix for the sake of efficiency.  If you do not explicitly
	 *  set the matrix, the elements will generally assume random
	 *  values.
	 */
	Matrix3() { } // for speed, do not initialize

	/** Copy constructor.
	 */
	Matrix3(const Matrix3& other) { set(other); }

	/** Construct and initialize a matrix from a double[9] array.
	 */
	explicit Matrix3(double const * const def) { set(def); }
	Matrix3(double a00, double a01, double a02,
	        double a10, double a11, double a12,
	        double a20, double a21, double a22) {
		set(a00, a01, a02,
		    a10, a11, a12,
		    a20, a21, a22);
	}

	/** Construct and initialize a matrix from three column vectors.
	 */
	Matrix3(const Vector3& col0, const Vector3& col1, const Vector3& col2);

	/** Destructor.
	 */
	~Matrix3() {}

	/** Compare two matrices.
	 *
	 *  Compares two matrices byte-by-byte.  The sign of the return value is
	 *  useless, since the byte comparisons are inequivalent to floating point
	 *  comparisions.  Used only to test for equality.
	 *
	 *  @returns 0 if equal, non-zero if unequal.
	 */
	int compare(const Matrix3& m) const { return memcmp(_mat, m._mat, sizeof(_mat)); }

	/** Compare two matrices for (byte) equality.
	 */
	bool operator == (const Matrix3& m) const { return compare(m)==0; }

	/** Compare two matrices for (byte) inequality.
	 */
	bool operator != (const Matrix3& m) const { return compare(m)!=0; }

#ifndef SWIG
	/** Get the value (reference) of a matrix element.
	 */
	inline double& operator()(int row_, int col_) { return _mat[row_][col_]; }

	/** Get the (const) value of a matrix element.
	 */
	inline double operator()(int row_, int col_) const { return _mat[row_][col_]; }
#endif // SWIG

	/** Get the value of a matrix element.
	 */
	inline double getElement(int row_, int col_) { return _mat[row_][col_]; }

	/** Set the value of a matrix element.
	 */
	inline void setElement(int row_, int col_, double value) { _mat[row_][col_]=value; }

	/** Return true if all elements are valid floating point numbers.
	 */
	inline bool valid() const { return !isNaN(); }

	/** Return true if any elements are NaN (not-a-number).
	 */
	bool isNaN() const;

#ifndef SWIG
	/** Copy operator.
	 */
	inline Matrix3& operator = (const Matrix3& other) {
		if (&other == this) return *this;
		set(reinterpret_cast<double const *>(other._mat));
		return *this;
	}
#endif // SWIG

	/** Set this matrix from another matrix.
	 */
	inline void set(const Matrix3& other) {
		set(reinterpret_cast<double const *>(other._mat));
	}

	/** Set this matrix from a double[9] array.
	 */
	inline void set(double const * src) {
		double *dst = reinterpret_cast<double*>(_mat);
		for (const double *end = src + 9; src != end; ) *dst++ = *src++;
	}

	/** Set this matrix from a list of element values.
	 */
	void set(double a00, double a01, double a02,
	         double a10, double a11, double a12,
	         double a20, double a21, double a22) {
		_mat[0][0] = a00;
		_mat[0][1] = a01;
		_mat[0][2] = a02;
		_mat[1][0] = a10;
		_mat[1][1] = a11;
		_mat[1][2] = a12;
		_mat[2][0] = a20;
		_mat[2][1] = a21;
		_mat[2][2] = a22;
	}

	/** Get the matrix elements as a vector<double>.
	 */
	std::vector<double> getElements() const;

	/** Set this matrix from a vector<double>.
	 */
	void setElements(std::vector<double> const &v) const;

	/** Get a row vector of this matrix.
	 */
	Vector3 getRow(int i) {
		assert(i>=0 && i<3);
		return Vector3(_mat[i][0], _mat[i][1], _mat[i][2]);
	}

	/** Get a column vector of this matrix.
	 */
	Vector3 getCol(int i) {
		assert(i>=0 && i<3);
		return Vector3(_mat[0][i], _mat[1][i], _mat[2][i]);
	}

	/** Set a row of this matrix from a vector.
	 */
	void setRow(int i, const Vector3& v) {
		assert(i>=0 && i<3);
		_mat[i][0] = v.x();
		_mat[i][1] = v.y();
		_mat[i][2] = v.z();
	}

	/** Set a column of this matrix from a vector.
	 */
	void setCol(int i, const Vector3& v) {
		assert(i>=0 && i<3);
		_mat[0][i] = v.x();
		_mat[1][i] = v.y();
		_mat[2][i] = v.z();
	}

	/** Get a pointer to a row of this matrix.
	 */
	double * row(int i) { assert(i>=0 && i<3); return (double*)(_mat[i]); }
	
	/** Get a pointer to the first element of this matrix.
	 */
	double * ptr() { return (double *)_mat; }

	/** Get a const pointer to the first element of this matrix.
	 */
	double const * ptr() const { return (double const *)_mat; }

	/** Set this matrix equal to the identity matrix.
	 */
	inline void makeIdentity() { set(IDENTITY); }

	/** Set all matrix elements to zero.
	 */
	inline void makeZero() { set(ZERO); }

	/** Set this matrix to a scaling matrix.
	 *
	 *  The resulting matrix has the components
	 *  of the input vector along the diagonal,
	 *  with all off-diagonal elements equal to
	 *  zero.
	 *
	 *  @param v A vector specifying the scale factor for
	 *           each axis.
	 */
	inline void makeScale(const Vector3& v) {
		makeScale(v.x(), v.y(), v.z());
	}

	/** Set this matrix to a scaling matrix.
	 *
	 *  See makeScale(const Vector3&).
	 */
	void makeScale(double, double, double);

	/** Make a rotation matrix to transform one vector into another.
	 *
	 *  The resulting matrix will rotate the @c from vector
	 *  into the @c to vector.
	 */
	void makeRotate(const Vector3& from, const Vector3& to);

	/** Make a rotation matrix to rotate around a given axis.
	 *
	 *  @param angle The angle of rotation.
	 *  @param axis The axis of rotation.
	 */
	void makeRotate(double angle, const Vector3& axis);

	/** Make a rotation matrix to rotate around a given axis.
	 *
	 *  See makeRotate(double angle, const Vector3& axis).
	 */
	void makeRotate(double angle, double x, double y, double z);

	/** Make a rotation matrix from a quaternion.
	 */
	void makeRotate(const Quat&);

	/** Make a rotation matrix from euler angles.
	 *
	 *  @param roll the x-axis rotation.
	 *  @param pitch the y-axis rotation.
	 *  @param yaw the z-axis rotation.
	 */
	void makeRotate(double roll, double pitch, double yaw);
	void makeRotateNED(double roll, double pitch, double yaw);

	/** Make a rotation from combining three rotations.
	 *  return Cnb
	 *  @param angle1 The angle of the first rotation.
	 *  @param axis1 The axis of the first rotation.
	 *  @param angle2 The angle of the second rotation.
	 *  @param axis2 The axis of the second rotation.
	 *  @param angle3 The angle of the third rotation.
	 *  @param axis3 The axis of the third rotation.
	 */
	void makeRotate(double angle1, const Vector3& axis1,
	                double angle2, const Vector3& axis2,
	                double angle3, const Vector3& axis3);

	/** Construct the inverse of a matrix.
	 *
	 *  @param m The input matrix.
	 *  @param tolerance The minimum value of the deteriminant.
	 *  @returns false if the deteriminant less than the tolerance,
	 *           true otherwise.  If false, the matrix elements are
	 *           copied directly from the input matrx.
	 */
	bool invert(const Matrix3 &m, double tolerance=1e-12);

	/** Invert this matrix.
	 *
	 *  @param tolerance The minimum value of the deteriminant.
	 *  @returns false if the deteriminant less than the tolerance,
	 *           true otherwise.  If false, the matrix elements are
	 *           unchanged.
	 */
	inline bool invert(double tolerance=1e-12) {
		Matrix3 m(*this);
		return invert(m, tolerance);
	}

	/** Construct the transpose of a matrix.
	 *
	 *  @param other The input matrix to transpose.