diff options
Diffstat (limited to 'core/math/basis.cpp')
| -rw-r--r-- | core/math/basis.cpp | 66 |
1 files changed, 51 insertions, 15 deletions
diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 0030cb1144..a9b4651664 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -58,8 +58,8 @@ void Basis::invert() { cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1) }; real_t det = elements[0][0] * co[0] + - elements[0][1] * co[1] + - elements[0][2] * co[2]; + elements[0][1] * co[1] + + elements[0][2] * co[2]; #ifdef MATH_CHECKS ERR_FAIL_COND(det == 0); #endif @@ -94,6 +94,18 @@ Basis Basis::orthonormalized() const { return c; } +void Basis::orthogonalize() { + Vector3 scl = get_scale(); + orthonormalize(); + scale_local(scl); +} + +Basis Basis::orthogonalized() const { + Basis c = *this; + c.orthogonalize(); + return c; +} + bool Basis::is_orthogonal() const { Basis identity; Basis m = (*this) * transposed(); @@ -237,6 +249,24 @@ void Basis::scale_local(const Vector3 &p_scale) { *this = scaled_local(p_scale); } +void Basis::scale_orthogonal(const Vector3 &p_scale) { + *this = scaled_orthogonal(p_scale); +} + +Basis Basis::scaled_orthogonal(const Vector3 &p_scale) const { + Basis m = *this; + Vector3 s = Vector3(-1, -1, -1) + p_scale; + Vector3 dots; + Basis b; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + dots[j] += s[i] * abs(m.get_axis(i).normalized().dot(b.get_axis(j))); + } + } + m.scale_local(Vector3(1, 1, 1) + dots); + return m; +} + float Basis::get_uniform_scale() const { return (elements[0].length() + elements[1].length() + elements[2].length()) / 3.0; } @@ -261,7 +291,7 @@ Vector3 Basis::get_scale_abs() const { } Vector3 Basis::get_scale_local() const { - real_t det_sign = SGN(determinant()); + real_t det_sign = SIGN(determinant()); return det_sign * Vector3(elements[0].length(), elements[1].length(), elements[2].length()); } @@ -287,11 +317,8 @@ Vector3 Basis::get_scale() const { // matrix elements. // // The rotation part of this decomposition is returned by get_rotation* functions. - real_t det_sign = SGN(determinant()); - return det_sign * Vector3( - Vector3(elements[0][0], elements[1][0], elements[2][0]).length(), - Vector3(elements[0][1], elements[1][1], elements[2][1]).length(), - Vector3(elements[0][2], elements[1][2], elements[2][2]).length()); + real_t det_sign = SIGN(determinant()); + return det_sign * get_scale_abs(); } // Decomposes a Basis into a rotation-reflection matrix (an element of the group O(3)) and a positive scaling matrix as B = O.S. @@ -682,8 +709,8 @@ bool Basis::operator!=(const Basis &p_matrix) const { Basis::operator String() const { return "[X: " + get_axis(0).operator String() + - ", Y: " + get_axis(1).operator String() + - ", Z: " + get_axis(2).operator String() + "]"; + ", Y: " + get_axis(1).operator String() + + ", Z: " + get_axis(2).operator String() + "]"; } Quaternion Basis::get_quaternion() const { @@ -704,9 +731,9 @@ Quaternion Basis::get_quaternion() const { temp[1] = ((m.elements[0][2] - m.elements[2][0]) * s); temp[2] = ((m.elements[1][0] - m.elements[0][1]) * s); } else { - int i = m.elements[0][0] < m.elements[1][1] ? - (m.elements[1][1] < m.elements[2][2] ? 2 : 1) : - (m.elements[0][0] < m.elements[2][2] ? 2 : 0); + int i = m.elements[0][0] < m.elements[1][1] + ? (m.elements[1][1] < m.elements[2][2] ? 2 : 1) + : (m.elements[0][0] < m.elements[2][2] ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; @@ -934,6 +961,15 @@ void Basis::_set_diagonal(const Vector3 &p_diag) { elements[2][2] = p_diag.z; } +Basis Basis::lerp(const Basis &p_to, const real_t &p_weight) const { + Basis b; + b.elements[0] = elements[0].lerp(p_to.elements[0], p_weight); + b.elements[1] = elements[1].lerp(p_to.elements[1], p_weight); + b.elements[2] = elements[2].lerp(p_to.elements[2], p_weight); + + return b; +} + Basis Basis::slerp(const Basis &p_to, const real_t &p_weight) const { //consider scale Quaternion from(*this); |