stlquat.cpp

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

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

Quaternion::Quaternion(const Matrix<3, 3> &mat)
{
  set(mat);
}

Quaternion &Quaternion::operator =(const Matrix<3, 3> &mat)
{
  return set(mat);
}

Quaternion &Quaternion::set(const Matrix<3, 3> &m)
{
  double tr, s1;
  int i;

  tr = m.m00 + m.m11 + m.m22 + 1.0;

  // check the diagonal
  if (tr > 0.0)
  {
    s1 = 0.5 / sqrt(tr);
    s = 0.25 / s1;
    x = -(m.m21 - m.m12) * s1;
    y = -(m.m02 - m.m20) * s1;
    z = -(m.m10 - m.m01) * s1;
  }
  else
  {
    // diagonal is negative
    i = 1;
    if (m.m11 > m.m00)
      i = 2;
    if ((i == 1 && m.m22 > m.m00) || (i == 2 && m.m22 > m.m11))
      i = 3;
    switch (i)
    {
    case 1:
      s1 = sqrt((m.m00 - (m.m11 + m.m22)) + 1.0);

      x = -s1 * 0.5;

      if (s1 != 0.0)
        s1 = 0.5 / s1;

      y = -(m.m01 + m.m10) * s1;
      z = -(m.m02 + m.m20) * s1;
      s = (m.m12 - m.m21) * s1;
      break;
    case 2:
      s1 = sqrt((m.m11 - (m.m22 + m.m00)) + 1.0);

      y = -0.5 * s1;

      if (s1 != 0.0)
        s1 = 0.5 / s1;

      x = -(m.m01 + m.m10) * s1;
      z = -(m.m12 + m.m21) * s1;
      s = (m.m02 - m.m20) * s1;

      break;
    case 3:
      s1 = sqrt((m.m22 - (m.m00 + m.m11)) + 1.0);

      z = -0.5 * s1;

      if (s1 != 0.0)
        s1 = 0.5 / s1;

      x = -(m.m02 + m.m20) * s1;
      y = -(m.m12 + m.m21) * s1;
      s = (m.m01 - m.m10) * s1;
      break;
    }
  }

  return *this;
}

Quaternion operator +(const Quaternion &q1, const Quaternion &q2)
{
	return Quaternion(q1.s + q2.s, q1.x + q2.x, q1.y + q2.y, q1.z + q2.z);
}

Quaternion operator -(const Quaternion &q1, const Quaternion &q2)
{
	return Quaternion(q1.s - q2.s, q1.x - q2.x, q1.y - q2.y, q1.z - q2.z);
}

Quaternion operator *(const Quaternion &q, double s)
{
	return Quaternion(q.s * s, q.x * s, q.y * s, q.z * s);
}

Quaternion operator *(double s, const Quaternion &q)
{
	return Quaternion(q.s * s, q.x * s, q.y * s, q.z * s);
}

Quaternion operator /(const Quaternion &q, double s)
{
	s = 1.0 / s;
	return Quaternion(q.s * s, q.x * s, q.y * s, q.z * s);
}

double operator *(const Quaternion &q1, const Quaternion &q2)
{
	return q1.s * q2.s + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
}

Quaternion operator %(const Quaternion &q1, const Quaternion &q2)
{
	return Quaternion(q1.s * q2.s - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, q1.y * q2.z
			- q1.z * q2.y + q1.s * q2.x + q1.x * q2.s, q1.z * q2.x - q1.x * q2.z + q1.s * q2.y
			+ q1.y * q2.s, q1.x * q2.y - q1.y * q2.x + q1.s * q2.z + q1.z * q2.s);
}

}}