Calculate the quaternion formed by two direction vectors

GetQuaternionFromTwoDirVectors.cpp

Eigen::Quaternionf Common::GetQuaternionFromDirVectors(
    const Eigen::Vector3f& upwards, const Eigen::Vector3f& forwards) {
  Eigen::Vector3f right = forwards.cross(upwards);

#define m00 right.x()
#define m01 forwards.x()
#define m02 upwards.x()
#define m10 right.y()
#define m11 forwards.y()
#define m12 upwards.y()
#define m20 right.z()
#define m21 forwards.z()
#define m22 upwards.z()

  Eigen::Quaternionf ret;
  ret.w() = std::sqrt(1.0f + m00 + m11 + m22) * 0.5f;
  float w4_recip = 1.0f / (4.0f * ret.w());
  ret.x() = (m21 - m12) * w4_recip;
  ret.y() = (m02 - m20) * w4_recip;
  ret.z() = (m10 - m01) * w4_recip;

#undef m00
#undef m01
#undef m02
#undef m10
#undef m11
#undef m12
#undef m20
#undef m21
#undef m22

  ret.normalize();
  return ret;
}