diff options
| -rw-r--r-- | core/math/quaternion.h | 7 | ||||
| -rw-r--r-- | modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs | 28 |
2 files changed, 12 insertions, 23 deletions
diff --git a/core/math/quaternion.h b/core/math/quaternion.h index e20ea74eb4..d8d0c06672 100644 --- a/core/math/quaternion.h +++ b/core/math/quaternion.h @@ -86,13 +86,6 @@ public: void operator*=(const Quaternion &p_q); Quaternion operator*(const Quaternion &p_q) const; - Quaternion operator*(const Vector3 &v) const { - return Quaternion(w * v.x + y * v.z - z * v.y, - w * v.y + z * v.x - x * v.z, - w * v.z + x * v.y - y * v.x, - -x * v.x - y * v.y - z * v.z); - } - _FORCE_INLINE_ Vector3 xform(const Vector3 &v) const { #ifdef MATH_CHECKS ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized."); diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs index 1694ac0320..c18f818ed2 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs @@ -472,26 +472,22 @@ namespace Godot return new Quaternion(-left.x, -left.y, -left.z, -left.w); } - public static Quaternion operator *(Quaternion left, Vector3 right) + public static Vector3 operator *(Quaternion quat, Vector3 vec) { - return new Quaternion - ( - (left.w * right.x) + (left.y * right.z) - (left.z * right.y), - (left.w * right.y) + (left.z * right.x) - (left.x * right.z), - (left.w * right.z) + (left.x * right.y) - (left.y * right.x), - -(left.x * right.x) - (left.y * right.y) - (left.z * right.z) - ); +#if DEBUG + if (!quat.IsNormalized()) + { + throw new InvalidOperationException("Quaternion is not normalized."); + } +#endif + var u = new Vector3(quat.x, quat.y, quat.z); + Vector3 uv = u.Cross(vec); + return vec + (((uv * quat.w) + u.Cross(uv)) * 2); } - public static Quaternion operator *(Vector3 left, Quaternion right) + public static Vector3 operator *(Vector3 vec, Quaternion quat) { - return new Quaternion - ( - (right.w * left.x) + (right.y * left.z) - (right.z * left.y), - (right.w * left.y) + (right.z * left.x) - (right.x * left.z), - (right.w * left.z) + (right.x * left.y) - (right.y * left.x), - -(right.x * left.x) - (right.y * left.y) - (right.z * left.z) - ); + return quat.Inverse() * vec; } public static Quaternion operator *(Quaternion left, real_t right) |