diff options
Diffstat (limited to 'src/core/Quat.cpp')
| -rw-r--r-- | src/core/Quat.cpp | 19 |
1 files changed, 19 insertions, 0 deletions
diff --git a/src/core/Quat.cpp b/src/core/Quat.cpp index abfe056..7837e41 100644 --- a/src/core/Quat.cpp +++ b/src/core/Quat.cpp @@ -89,6 +89,10 @@ Quat Quat::normalized() const { return *this / length(); } +bool Quat::is_normalized() const { + return std::abs(length_squared() - 1.0) < 0.00001; +} + Quat Quat::inverse() const { return Quat(-x, -y, -z, w); } @@ -171,6 +175,21 @@ void Quat::get_axis_and_angle(Vector3 &r_axis, real_t &r_angle) const { r_axis.z = z / ::sqrt(1 - w * w); } +void Quat::set_axis_angle(const Vector3 &axis, const float angle) { + ERR_FAIL_COND(!axis.is_normalized()); + + real_t d = axis.length(); + if (d == 0) + set(0, 0, 0, 0); + else { + real_t sin_angle = ::sin(angle * 0.5); + real_t cos_angle = ::cos(angle * 0.5); + real_t s = sin_angle / d; + set(axis.x * s, axis.y * s, axis.z * s, + cos_angle); + } +} + Quat Quat::operator*(const Vector3 &v) const { return Quat(w * v.x + y * v.z - z * v.y, w * v.y + z * v.x - x * v.z, |