123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657 |
- #include <cmath>
- #include "client/math/Quaternion.h"
- Quaternion::Quaternion(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {
- }
- Quaternion::Quaternion(Vector unitAxis, float angle) {
- float sin;
- sincosf(angle * (M_PI / 360.0f), &sin, &w);
- x = unitAxis.getX() * sin;
- y = unitAxis.getY() * sin;
- z = unitAxis.getZ() * sin;
- }
- Quaternion Quaternion::lerp(float f, const Quaternion& other) const {
- Quaternion q = interpolate(1 - f, f, other);
- q.normalize();
- return q;
- }
- Quaternion Quaternion::slerp(float f, const Quaternion& other) const {
- float arccos = acosf(x * other.x + y * other.y + z * other.z + w * other.w);
- float sin = 1.0f / sinf(arccos);
- return interpolate(sinf((1 - f) * arccos) * sin, sinf(f * arccos) * sin, other);
- }
- Quaternion Quaternion::interpolate(float a, float b, const Quaternion& other) const {
- return Quaternion(x * a + other.x * b, y * a + other.y * b, z * a + other.z * b, w * a + other.w * b);
- }
- void Quaternion::normalize() {
- float f = 1.0f / sqrtf(x * x + y * y + z * z + w * w);
- x *= f;
- y *= f;
- z *= f;
- w *= f;
- }
- Matrix Quaternion::toMatrix() const {
- float x2 = 2 * x * x;
- float y2 = 2 * y * y;
- float z2 = 2 * z * z;
-
- float xy = 2 * x * y;
- float xz = 2 * x * z;
- float xw = 2 * x * w;
- float zw = 2 * z * w;
- float yz = 2 * y * z;
- float yw = 2 * y * w;
- Matrix m;
- m.set(0, 1 - y2 - z2).set(1, xy - zw).set(2, xz + yw);
- m.set(4, xy + zw).set(5, 1 - x2 - z2).set(6, yz - xw);
- m.set(8, xz - yw).set(9, yz + xw).set(10, 1 - x2 - y2);
- return m;
- }
|