godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix the calculation of the angular velocity when the rotation speed is not high.

This commit is contained in:
fabriceci 2022-07-19 09:50:54 +02:00
parent 00c0fbb8cf
commit db7c91e0ad
1 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
core/math/basis.cpp
View File

@ -817,14 +817,13 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
#endif
*/
real_t angle, x, y, z; // variables for result
real_t epsilon = 0.01; // margin to allow for rounding errors
real_t epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
real_t angle_epsilon = 0.1; // margin to distinguish between 0 and 180 degrees
if ((Math::abs(rows[1][0] - rows[0][1]) < epsilon) && (Math::abs(rows[2][0] - rows[0][2]) < epsilon) && (Math::abs(rows[2][1] - rows[1][2]) < epsilon)) {
if ((Math::abs(rows[1][0] - rows[0][1]) < CMP_EPSILON) && (Math::abs(rows[2][0] - rows[0][2]) < CMP_EPSILON) && (Math::abs(rows[2][1] - rows[1][2]) < CMP_EPSILON)) {
// singularity found
// first check for identity matrix which must have +1 for all terms
// in leading diagonal and zero in other terms
if ((Math::abs(rows[1][0] + rows[0][1]) < epsilon2) && (Math::abs(rows[2][0] + rows[0][2]) < epsilon2) && (Math::abs(rows[2][1] + rows[1][2]) < epsilon2) && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < epsilon2)) {
if ((Math::abs(rows[1][0] + rows[0][1]) < angle_epsilon) && (Math::abs(rows[2][0] + rows[0][2]) < angle_epsilon) && (Math::abs(rows[2][1] + rows[1][2]) < angle_epsilon) && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < angle_epsilon)) {
// this singularity is identity matrix so angle = 0
r_axis = Vector3(0, 1, 0);
r_angle = 0;
@ -839,7 +838,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
real_t xz = (rows[2][0] + rows[0][2]) / 4;
real_t yz = (rows[2][1] + rows[1][2]) / 4;
if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term
if (xx < epsilon) {
if (xx < CMP_EPSILON) {
x = 0;
y = Math_SQRT12;
z = Math_SQRT12;
@ -849,7 +848,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
z = xz / x;
}
} else if (yy > zz) { // rows[1][1] is the largest diagonal term
if (yy < epsilon) {
if (yy < CMP_EPSILON) {
x = Math_SQRT12;
y = 0;
z = Math_SQRT12;
@ -859,7 +858,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
z = yz / y;
}
} else { // rows[2][2] is the largest diagonal term so base result on this
if (zz < epsilon) {
if (zz < CMP_EPSILON) {
x = Math_SQRT12;
y = Math_SQRT12;
z = 0;