Quaternion.cpp 1.0 KB

12345678910111213141516171819202122232425262728293031323334353637383940
  1. #include "math/Quaternion.h"
  2. Quaternion::Quaternion() : w(1.0f) {
  3. }
  4. Quaternion::Quaternion(const Vector3& axis, float angle) : xyz(axis) {
  5. xyz.normalize();
  6. float factor = 0.0f;
  7. sincosf(angle * (M_PI / 360.0f), &factor, &w);
  8. xyz *= factor;
  9. }
  10. Quaternion Quaternion::lerp(float f, const Quaternion& other) const {
  11. Quaternion q;
  12. q.xyz = xyz * (1.0f - f) + other.xyz * f;
  13. q.w = w * (1.0f - f) + other.w * f;
  14. float iLength = 1.0f / sqrtf(q.xyz.squareLength() + q.w * q.w);
  15. q.xyz *= iLength;
  16. q.w *= iLength;
  17. return q;
  18. }
  19. Quaternion& Quaternion::operator*=(const Quaternion& other) {
  20. float dot = xyz.dot(other.xyz);
  21. xyz = other.xyz * w + xyz * other.w + xyz.cross(other.xyz);
  22. w = w * other.w - dot;
  23. return *this;
  24. }
  25. Quaternion Quaternion::operator*(const Quaternion& other) const {
  26. Quaternion q(*this);
  27. q *= other;
  28. return q;
  29. }
  30. Vector3 Quaternion::operator*(const Vector3& v) const {
  31. Vector3 qv = v * w + xyz.cross(v);
  32. Vector3 qvq = xyz * xyz.dot(v) + qv * w - qv.cross(xyz);
  33. return qvq;
  34. }