vrmaths.cpp

#include "vrmaths.h"
 
const Vector2i Vector2i::ZERO(0, 0);
const Vector2 Vector2::ZERO(0.0f, 0.0f);
const Vector3 Vector3::ZERO(0.0f, 0.0f, 0.0f);
const Vector4 Vector4::ZERO(0.0f, 0.0f, 0.0, 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::ToRotationMatrix(Matrix3& kRot) const
{
  float fTx = x + x;
  float fTy = y + y;
  float fTz = z + z;
  float fTwx = fTx * w;
  float fTwy = fTy * w;
  float fTwz = fTz * w;
  float fTxx = fTx * x;
  float fTxy = fTy * x;
  float fTxz = fTz * x;
  float fTyy = fTy * y;
  float fTyz = fTz * y;
  float fTzz = fTz * z;
 
  kRot[0][0] = 1.0f - (fTyy + fTzz);
  kRot[0][1] = fTxy - fTwz;
  kRot[0][2] = fTxz + fTwy;
  kRot[1][0] = fTxy + fTwz;
  kRot[1][1] = 1.0f - (fTxx + fTzz);
  kRot[1][2] = fTyz - fTwx;
  kRot[2][0] = fTxz - fTwy;
  kRot[2][1] = fTyz + fTwx;
  kRot[2][2] = 1.0f - (fTxx + fTyy);
}
 
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;
  }
}