stlvec.cpp

来自「矩阵、向量以及四元数的封装类」· C++ 代码 · 共 49 行

//========================================================================================
// Copyright (C) 2009 by Ma Zhi-Hao, Key Lab.
//          		The Academy of Equipment Command & Technology, PLA
//          		HuaiRou, Beijing, 101416, China
// File 		：STLvec.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{

Vector<3> &Vector<3>::rotate(const Matrix<3, 3> &m)
{
  double x1 = x, y1 = y;
  x = m.m00 * x1 + m.m01 * y1 + m.m02 * z;
  y = m.m10 * x1 + m.m11 * y1 + m.m12 * z;
  z = m.m20 * x1 + m.m21 * y1 + m.m22 * z;
  return *this;
}

Vector<3> Vector<3>::getRotate(const Matrix<3, 3> &m) const
{
  return Vector<3>(m.m00 * x + m.m01 * y + m.m02 * z,
                 m.m10 * x + m.m11 * y + m.m12 * z,
                 m.m20 * x + m.m21 * y + m.m22 * z );
}

Vector<3> &Vector<3>::rotate(const Quaternion &q)
{
  Quaternion p(0, *this);
  p = (q % p) % q.getConjugate();
  x = p.x; y = p.y; z = p.z;
  return *this;
}

Vector<3> Vector<3>::getRotate(const Quaternion &q) const
{
  Quaternion p(0, *this);
  p = (q % p) % q.getConjugate();
  return Vector<3>(p.x, p.y, p.z);
}

}}