vrmaths.cpp

#include "vrmaths.h"

const Vector3 Vector3::ZERO(0.0f, 0.0f, 0.0f);
const Matrix4 Matrix4::IDENTITY(
  1, 0, 0, 0,
  0, 1, 0, 0,
  0, 0, 1, 0,
  0, 0, 0, 1);

float Quaternion::Norm() const
{
  return w*w + x*x + y*y + z*z;
}

float Quaternion::normalise(void)
{
  float len = Norm();
  float factor = 1.0f / sqrt(len);
  *this = *this * factor;
  return len;
}

Quaternion Quaternion::Inverse() const
{
  float fNorm = w*w + x*x + y*y + z*z;
  if (fNorm > 0.0)
  {
    float fInvNorm = 1.0f / fNorm;
    return Quaternion(w*fInvNorm, -x*fInvNorm, -y*fInvNorm, -z*fInvNorm);
  }
  else
  {
    return Quaternion(0.0f, 0.0f, 0.0f, 0.0f);
  }
}

void Quaternion::FromRotationMatrix(const Matrix3& kRot)
{
  // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
  // article "Quaternion Calculus and Fast Animation".

  float fTrace = kRot[0][0] + kRot[1][1] + kRot[2][2];
  float fRoot;

  if (fTrace > 0.0)
  {
    // |w| > 1/2, may as well choose w > 1/2
    fRoot = sqrt(fTrace + 1.0f);  // 2w
    w = 0.5f*fRoot;
    fRoot = 0.5f / fRoot;  // 1/(4w)
    x = (kRot[2][1] - kRot[1][2])*fRoot;
    y = (kRot[0][2] - kRot[2][0])*fRoot;
    z = (kRot[1][0] - kRot[0][1])*fRoot;
  }
  else
  {
    // |w| <= 1/2
    static size_t s_iNext[3] = { 1, 2, 0 };
    size_t i = 0;
    if (kRot[1][1] > kRot[0][0])
      i = 1;
    if (kRot[2][2] > kRot[i][i])
      i = 2;
    size_t j = s_iNext[i];
    size_t k = s_iNext[j];

    fRoot = sqrt(kRot[i][i] - kRot[j][j] - kRot[k][k] + 1.0f);
    float* apkQuat[3] = { &x, &y, &z };
    *apkQuat[i] = 0.5f*fRoot;
    fRoot = 0.5f / fRoot;
    w = (kRot[k][j] - kRot[j][k])*fRoot;
    *apkQuat[j] = (kRot[j][i] + kRot[i][j])*fRoot;
    *apkQuat[k] = (kRot[k][i] + kRot[i][k])*fRoot;
  }
}