matrix3.h

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

	 */
	void transpose(const Matrix3& other);

	/** Transpose this matrix.
	 */
	inline void transpose() {
		swap(_mat[0][1], _mat[1][0]);
		swap(_mat[0][2], _mat[2][0]);
		swap(_mat[1][2], _mat[2][1]);
	}

	//r[0]=r[a] r[1]=r[b] r[2]=r[c]
	inline void swapRow(int a,int b,int c,int sa=1,int sb=1, int sc=1) {
		double mat[3][3];
		mat[0][0]=_mat[a][0]*sa;
		mat[0][1]=_mat[a][1]*sa;
		mat[0][2]=_mat[a][2]*sa;
		mat[1][0]=_mat[b][0]*sb;
		mat[1][1]=_mat[b][1]*sb;
		mat[1][2]=_mat[b][2]*sb;
		mat[2][0]=_mat[c][0]*sc;
		mat[2][1]=_mat[c][1]*sc;
		mat[2][2]=_mat[c][2]*sc;
		memcpy(_mat,mat,sizeof(double[3][3]));
	}

	inline void swapCol(int a,int b,int c,int sa=1,int sb=1, int sc=1) {
		double mat[3][3];
		mat[0][0]=_mat[0][a]*sa;
		mat[1][0]=_mat[1][a]*sa;
		mat[2][0]=_mat[2][a]*sa;
		mat[0][1]=_mat[0][b]*sb;
		mat[1][1]=_mat[1][b]*sb;
		mat[2][1]=_mat[2][b]*sb;
		mat[0][2]=_mat[0][c]*sc;
		mat[1][2]=_mat[1][c]*sc;
		mat[2][2]=_mat[2][c]*sc;
		memcpy(_mat,mat,sizeof(double[3][3]));
	}

	/** Get the inverse of this matrix.
	 *
	 *  See invert(double).
	 */
	inline Matrix3 getInverse(double tolerance=1e-12) const { return inverse(*this, tolerance); }

	/** Get the transpose of this matrix.
	 */
	inline Matrix3 getTranspose() const {
		return Matrix3(_mat[0][0], _mat[1][0], _mat[2][0],
		               _mat[0][1], _mat[1][1], _mat[2][1],
		               _mat[0][2], _mat[1][2], _mat[2][2]);
	}

	/** Compute the determinant of this matrix.
	 */
	double determinant() const;

	/** Create a new identity matrix.
	 */
	inline static Matrix3 const &identity(void) { return IDENTITY; }

	/** Create a new scaling matrix.
	 */
	inline static Matrix3 scale(const Vector3& sv);

	/** Create a new scaling matrix.
	 */
	inline static Matrix3 scale(double sx, double sy, double sz);

	/** Create a new rotation matrix.
	 */
	inline static Matrix3 rotate(const Vector3& from, const Vector3& to);

	/** Create a new rotation matrix.
	 */
	inline static Matrix3 rotate(double angle, double x, double y, double z);
	/** Create a new rotation matrix.
	 */
	inline static Matrix3 rotate(double angle, const Vector3& axis);

	/** Create a new rotation matrix.
	 */
	inline static Matrix3 rotate(double angle1, const Vector3& axis1,
	                             double angle2, const Vector3& axis2,
	                             double angle3, const Vector3& axis3);

	/** Create a new rotation matrix.
	 */
	inline static Matrix3 rotate(double roll, double pitch, double yaw);

	/** Create a new rotation matrix.
	 */
	inline static Matrix3 rotate(const Quat& quat);

	/** Get the inverse of a matrix.
	 */
	inline static Matrix3 inverse(const Matrix3& matrix, double tolerance=1e-12);
	/** Get the tensor product of two vectors.
	 */
	inline static Matrix3 tensor(const Vector3&a, const Vector3 &b);
	
	/** Get the rotation angle and axis of this matrix.
	 */
	void getRotate(double angle, Vector3& axis) const;

	/** Get the Euler angles of this matrix.
	 */
	void getEulerAngles(double &roll, double &pitch, double &yaw);

	/** Multiply this matrix by a row vector (v*M).
	 */
	inline Vector3 preMult(const Vector3& v) const;

	/** Multiply this matrix by a column vector (M*v).
	 */
	inline Vector3 postMult(const Vector3& v) const;

	/** Multiply this matrix by a column vector (M*v).
	 */
	inline Vector3 operator* (const Vector3& v) const { return postMult(v); }
#ifndef SWIG
	/** Multiply a matrix by a row vector (v*M).
	 */
	inline friend Vector3 operator* (const Vector3& v, const Matrix3& m) { return m.preMult(v); }
#endif // SWIG

	/** Get the diagonal elements of this matrix as a vector.
	 */
	inline Vector3 getScale() const { return Vector3(_mat[0][0],_mat[1][1],_mat[2][2]); }

	/** Get the trace of this matrix.
	 */
	inline double getTrace() const { return _mat[0][0] + _mat[1][1] + _mat[2][2]; }

	/** Matrix multipliation (M*M)
	 */
	void mult(const Matrix3&, const Matrix3&);

	/** Multiply this matrix by another matrix on the left.
	 */
	void preMult(const Matrix3&);

	/** Multiply this matrix by another matrix on the right.
	 */
	void postMult(const Matrix3&);

	/** Multiply this matrix by another matrix on the right.
	 */
	inline void operator *= (const Matrix3& other) {
		if (this == &other) {
			Matrix3 temp(other);
			postMult(temp);
		} else {
			postMult(other);
		}
	}

	/** Get the product of this matrix and another matrix.
	 */
	inline Matrix3 operator * (const Matrix3& m) const {
		Matrix3 r;
		r.mult(*this, m);
		return r;
	}

	/** Add another matrix to this matrix.
	 */
	inline const Matrix3& operator += (const Matrix3& rhs) {
		double *m0 = reinterpret_cast<double*>(_mat);
		double const *m1 = reinterpret_cast<double const*>(rhs._mat);
		for (int i=0; i<9; ++i) {
			m0[i] += m1[i];
		}
		return *this;
	}

	/** Subtract another matrix from this matrix.
	 */
	inline const Matrix3& operator -= (const Matrix3& rhs) {
		double *m0 = reinterpret_cast<double*>(_mat);
		double const *m1 = reinterpret_cast<double const*>(rhs._mat);
		for (int i=0; i<9; ++i) {
			m0[i] -= m1[i];
		}
		return *this;
	}

	/** Multiply this matrix by a scalar.
	 */
	inline const Matrix3& operator *= (const double rhs) {
		double *m0 = reinterpret_cast<double*>(_mat);
		for (int i=0; i<9; ++i) {
			m0[i] *= rhs;
		}
		return *this;
	}

	/** Divide this matrix by a scalar.
	 */
	inline const Matrix3& operator /= (const double rhs) {
		return *this *= (1.0/rhs);
	}

	/** Get the sum of this matrix and another matrix.
	 */
	inline Matrix3 operator + (const Matrix3& rhs) const {
		Matrix3 result;
		double *m0 = reinterpret_cast<double*>(result._mat);
		double const *m1 = reinterpret_cast<double const*>(_mat);
		double const *m2 = reinterpret_cast<double const*>(rhs._mat);
		for (int i=0; i<9; ++i) {
			m0[i] = m1[i] + m2[i];
		}
		return result;
	}

	/** Get the difference of this matrix and another matrix.
	 */
	inline Matrix3 operator - (const Matrix3& rhs) const {
		Matrix3 result;
		double *m0 = reinterpret_cast<double*>(result._mat);
		double const *m1 = reinterpret_cast<double const*>(_mat);
		double const *m2 = reinterpret_cast<double const*>(rhs._mat);
		for (int i=0; i<9; ++i) {
			m0[i] = m1[i] - m2[i];
		}
		return result;
	}

	/** Get the product of this matrix and a scalar.
	 */
	inline Matrix3 operator * (double rhs) const {
		Matrix3 result;
		double *m0 = reinterpret_cast<double*>(result._mat);
		double const *m1 = reinterpret_cast<double const*>(_mat);
		for (int i=0; i<9; ++i) {
			m0[i] = m1[i] * rhs;
		}
		return result;
	}

	/** Get the quotient of this matrix and a scalar.
	 */
	inline Matrix3 operator / (double rhs) const {
		return *this * (1.0/rhs);
	}

	/** Get this matrix with each element negated.
	 */
	inline Matrix3 operator - () const {
		return *this * (-1.0);
	}

#ifndef SWIG
	/** Multiply a matrix by a scalar on the left (s*M).
	 */
	inline friend Matrix3 operator * (double lhs, const Matrix3& rhs) {
		return rhs * lhs;
	}
#endif // SWIG


public:
	/** The matrix elements */
	double _mat[3][3];

};


inline Matrix3 Matrix3::scale(double sx, double sy, double sz) {
	return Matrix3(sx, 0.0, 0.0, 0.0, sy, 0.0, 0.0, 0.0, sz);
}

inline Matrix3 Matrix3::scale(const Vector3& v) {
	return Matrix3(v.x(), 0.0, 0.0, 0.0, v.y(), 0.0, 0.0, 0.0, v.z());
}

inline Matrix3 Matrix3::rotate(const Quat& q) {
	Matrix3 m;
	m.makeRotate(q);
	return m;
}

inline Matrix3 Matrix3::rotate(double roll, double pitch, double yaw) {
	Matrix3 m;
	m.makeRotate(roll, pitch, yaw);
	return m;
}

inline Matrix3 Matrix3::rotate(double angle, double x, double y, double z) {
	Matrix3 m;
	m.makeRotate(angle, x, y, z);
	return m;
}

inline Matrix3 Matrix3::rotate(double angle, const Vector3& axis) {
	Matrix3 m;
	m.makeRotate(angle, axis);
	return m;
}

inline Matrix3 Matrix3::rotate(double angle1, const Vector3& axis1,
	                           double angle2, const Vector3& axis2,
	                           double angle3, const Vector3& axis3)
{
	Matrix3 m;
	m.makeRotate(angle1, axis1, angle2, axis2, angle3, axis3);
	return m;
}

inline Matrix3 Matrix3::rotate(const Vector3& from, const Vector3& to) {
	Matrix3 m;
	m.makeRotate(from, to);
	return m;
}

inline Matrix3 Matrix3::inverse(const Matrix3& matrix, double tolerance) {
	Matrix3 m;
	m.invert(matrix, tolerance);
	return m;
}

inline Matrix3 Matrix3::tensor(const Vector3&a, const Vector3 &b) {
	return Matrix3(a.x()*b.x(),
	               a.x()*b.y(),
	               a.x()*b.z(),
	               a.y()*b.x(),
	               a.y()*b.y(),
	               a.y()*b.z(),
	               a.z()*b.x(),
	               a.z()*b.y(),
	               a.z()*b.z());
}

inline Vector3 Matrix3::postMult(const Vector3& v)  const {
	return Vector3((_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z()),
	               (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z()),
	               (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z()));
}

inline Vector3 Matrix3::preMult(const Vector3& v) const {
	return Vector3((_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z()),
	               (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z()),
	               (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z()));
}

 std::ostream &operator <<(std::ostream &o, Matrix3 const &m);





#endif // __CSPLIB_DATA_MATRIX3_H__