Fix calculation of up vector angle.

This commit is contained in:
Filip Sykala - NTB T15p 2023-09-27 14:37:00 +02:00
parent 4ea9a250ba
commit 211146e5de
2 changed files with 5 additions and 9 deletions

View File

@ -1647,22 +1647,17 @@ std::optional<float> Emboss::calc_up(const Transform3d &tr, double up_limit)
Vec3d normal = tr_linear.col(2); Vec3d normal = tr_linear.col(2);
// scaled matrix has base with different size // scaled matrix has base with different size
normal.normalize(); normal.normalize();
Vec3d suggested = suggest_up(normal); Vec3d suggested = suggest_up(normal, up_limit);
assert(is_approx(suggested.squaredNorm(), 1.)); assert(is_approx(suggested.squaredNorm(), 1.));
Vec3d up = tr_linear.col(1); // tr * UnitY() Vec3d up = tr_linear.col(1); // tr * UnitY()
up.normalize(); up.normalize();
double dot = suggested.dot(up);
if (dot >= 1. || dot <= -1.)
return {}; // zero angle
Matrix3d m; Matrix3d m;
m.row(0) = up; m.row(0) = up;
m.row(1) = suggested; m.row(1) = suggested;
m.row(2) = normal; m.row(2) = normal;
double det = m.determinant(); double det = m.determinant();
double dot = suggested.dot(up);
return -atan2(det, dot); return -atan2(det, dot);
} }

View File

@ -317,9 +317,10 @@ template<typename T> T angle_to_0_2PI(T angle)
return angle; return angle;
} }
template<typename T> void to_range_pi_pi(T &angle){ template<typename T> void to_range_pi_pi(T &angle){
if (angle > T(PI) || angle < -T(PI)) { if (angle > T(PI) || angle <= -T(PI)) {
int count = static_cast<int>(std::round(angle / (2 * PI))); int count = static_cast<int>(std::round(angle / (2 * PI)));
angle -= static_cast<T>(count * 2 * PI); angle -= static_cast<T>(count * 2 * PI);
assert(angle <= T(PI) && angle > -T(PI));
} }
} }