stlmat.cpp

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

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

#include "STLsysdef.h"
#include "math/STLmat.h"
#include "math/STLquat.h"

namespace shooter{ namespace math{

Matrix<3,3>::Matrix(const Quaternion &quat)
{
  double q00 = quat.quat[0] * quat.quat[0];
  double q11 = quat.quat[1] * quat.quat[1];
  double q22 = quat.quat[2] * quat.quat[2];
  double q33 = quat.quat[3] * quat.quat[3];

  m00 = q00 + q11 - q22 - q33;
  m01 = 2 * (quat.quat[0] * quat.quat[3] + quat.quat[1] * quat.quat[2]);
  m02 = 2 * (quat.quat[1] * quat.quat[3] - quat.quat[0] * quat.quat[2]);
  m10 = 2 * (quat.quat[1] * quat.quat[2] - quat.quat[0] * quat.quat[3]);
  m11 = q00 - q11 + q22 - q33;
  m12 = 2 * (quat.quat[0] * quat.quat[1] + quat.quat[2] * quat.quat[3]);
  m20 = 2 * (quat.quat[0] * quat.quat[2] + quat.quat[1] * quat.quat[3]);
  m21 = 2 * (quat.quat[2] * quat.quat[3] - quat.quat[0] * quat.quat[1]);
  m22 = q00 - q11 - q22 + q33;
}

Matrix<3,3> &Matrix<3,3>::operator =(const Quaternion &quat)
{
  double q00 = quat.quat[0] * quat.quat[0];
  double q11 = quat.quat[1] * quat.quat[1];
  double q22 = quat.quat[2] * quat.quat[2];
  double q33 = quat.quat[3] * quat.quat[3];

  m00 = q00 + q11 - q22 - q33;
  m01 = 2 * (quat.quat[0] * quat.quat[3] + quat.quat[1] * quat.quat[2]);
  m02 = 2 * (quat.quat[1] * quat.quat[3] - quat.quat[0] * quat.quat[2]);
  m10 = 2 * (quat.quat[1] * quat.quat[2] - quat.quat[0] * quat.quat[3]);
  m11 = q00 - q11 + q22 - q33;
  m12 = 2 * (quat.quat[0] * quat.quat[1] + quat.quat[2] * quat.quat[3]);
  m20 = 2 * (quat.quat[0] * quat.quat[2] + quat.quat[1] * quat.quat[3]);
  m21 = 2 * (quat.quat[2] * quat.quat[3] - quat.quat[0] * quat.quat[1]);
  m22 = q00 - q11 - q22 + q33;

  return *this;
}

Matrix<3,3> &Matrix<3,3>::set(const Quaternion &quat)
{
  double q00 = quat.quat[0] * quat.quat[0];
  double q11 = quat.quat[1] * quat.quat[1];
  double q22 = quat.quat[2] * quat.quat[2];
  double q33 = quat.quat[3] * quat.quat[3];

  m00 = q00 + q11 - q22 - q33;
  m01 = 2 * (quat.quat[0] * quat.quat[3] + quat.quat[1] * quat.quat[2]);
  m02 = 2 * (quat.quat[1] * quat.quat[3] - quat.quat[0] * quat.quat[2]);
  m10 = 2 * (quat.quat[1] * quat.quat[2] - quat.quat[0] * quat.quat[3]);
  m11 = q00 - q11 + q22 - q33;
  m12 = 2 * (quat.quat[0] * quat.quat[1] + quat.quat[2] * quat.quat[3]);
  m20 = 2 * (quat.quat[0] * quat.quat[2] + quat.quat[1] * quat.quat[3]);
  m21 = 2 * (quat.quat[2] * quat.quat[3] - quat.quat[0] * quat.quat[1]);
  m22 = q00 - q11 - q22 + q33;

  return *this;
}

}}