diff --git a/dtool/src/dtoolbase/nearly_zero.h b/dtool/src/dtoolbase/nearly_zero.h index 0b36d4652d..a0ffa0b333 100644 --- a/dtool/src/dtoolbase/nearly_zero.h +++ b/dtool/src/dtoolbase/nearly_zero.h @@ -25,17 +25,17 @@ // const identifier, and then returning the value of that identifier, // seems to lead to compilation errors (at least in VC7) in which // sometimes IS_THRESHOLD_COMPEQ(a, a, get_nearly_zero_value(a)) != 0. -INLINE double +CONSTEXPR double get_nearly_zero_value(double) { return 1.0e-12; } -INLINE float +CONSTEXPR float get_nearly_zero_value(float) { return 1.0e-6f; } -INLINE int +CONSTEXPR int get_nearly_zero_value(int) { // This is a bit silly, but we should nevertheless define it in // case it is called for an integer type. diff --git a/panda/src/linmath/lmatrix3_src.I b/panda/src/linmath/lmatrix3_src.I index b6ede0627c..6b12ee6642 100644 --- a/panda/src/linmath/lmatrix3_src.I +++ b/panda/src/linmath/lmatrix3_src.I @@ -998,11 +998,9 @@ INLINE_LINMATH void FLOATNAME(LMatrix3):: transpose_in_place() { TAU_PROFILE("void LMatrix3::transpose_in_place()", " ", TAU_USER); - #define SWAP__(x,y) { FLOATTYPE temp = (x); (x) = (y); (y) = temp;} - SWAP__(_m(0, 1),_m(1, 0)); - SWAP__(_m(0, 2),_m(2, 0)); - SWAP__(_m(1, 2),_m(2, 1)); - #undef SWAP__ + std::swap(_m(0, 1), _m(1, 0)); + std::swap(_m(0, 2), _m(2, 0)); + std::swap(_m(1, 2), _m(2, 1)); } // Matrix inversion code from Numerical Recipes in C. @@ -1050,12 +1048,16 @@ INLINE_LINMATH bool FLOATNAME(LMatrix3):: invert_from(const FLOATNAME(LMatrix3) &other) { TAU_PROFILE("bool LMatrix3::invert_from(const LMatrix3 &)", " ", TAU_USER); + // We throw the value out only if it's smaller than our "small" + // threshold squared. This helps reduce overly-sensitive + // rejections. #ifdef HAVE_EIGEN bool invertible; - other._m.computeInverseWithCheck(_m, invertible); + other._m.computeInverseWithCheck(_m, invertible, + NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE)); if (!invertible) { -#ifdef NDEBUG +#ifdef NOTIFY_DEBUG linmath_cat.warning() << "Tried to invert singular LMatrix3.\n"; #endif (*this) = ident_mat(); @@ -1068,11 +1070,7 @@ invert_from(const FLOATNAME(LMatrix3) &other) { FLOATTYPE other_det = MATRIX3_DETERMINANT(other._m); - // We throw the value out only if it's smaller than our "small" - // threshold squared. This helps reduce overly-sensitive - // rejections. if (IS_THRESHOLD_ZERO(other_det, (NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE)))) { - // if (IS_NEARLY_ZERO(other_det)) { #ifdef NOTIFY_DEBUG linmath_cat.warning() << "Tried to invert singular LMatrix3.\n"; #endif @@ -1125,10 +1123,11 @@ invert_transpose_from(const FLOATNAME(LMatrix3) &other) { #ifdef HAVE_EIGEN bool invertible; EMatrix3 temp; - other._m.computeInverseWithCheck(temp, invertible); + other._m.computeInverseWithCheck(temp, invertible, + NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE)); if (!invertible) { -#ifdef NDEBUG +#ifdef NOTIFY_DEBUG linmath_cat.warning() << "Tried to invert singular LMatrix3.\n"; #endif (*this) = ident_mat(); @@ -1139,6 +1138,7 @@ invert_transpose_from(const FLOATNAME(LMatrix3) &other) { return true; #else // HAVE_EIGEN + FLOATTYPE other_det = MATRIX3_DETERMINANT(other._m); if (IS_THRESHOLD_ZERO(other_det, (NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE)))) { @@ -1180,10 +1180,11 @@ invert_transpose_from(const FLOATNAME(LMatrix4) &other) { #ifdef HAVE_EIGEN bool invertible; EMatrix3 temp; - other._m.block<3, 3>(0, 0).computeInverseWithCheck(temp, invertible); + other._m.block<3, 3>(0, 0).computeInverseWithCheck(temp, invertible, + NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE)); if (!invertible) { -#ifdef NDEBUG +#ifdef NOTIFY_DEBUG linmath_cat.warning() << "Tried to invert singular LMatrix3.\n"; #endif (*this) = ident_mat(); diff --git a/panda/src/linmath/lmatrix4_src.I b/panda/src/linmath/lmatrix4_src.I index 389b472aea..38fe84b0ad 100644 --- a/panda/src/linmath/lmatrix4_src.I +++ b/panda/src/linmath/lmatrix4_src.I @@ -831,7 +831,7 @@ xform(const FLOATNAME(LVecBase4) &v) const { #ifdef HAVE_EIGEN v_res._v.noalias() = v._v * _m; -#else +#else VECTOR4_MATRIX4_PRODUCT(v_res, v,(*this)); #endif // HAVE_EIGEN return v_res; @@ -853,12 +853,12 @@ xform_point(const FLOATNAME(LVecBase3) &v) const { #ifdef HAVE_EIGEN v_res._v.noalias() = v._v * _m.block<3, 3>(0, 0) + _m.block<1, 3>(3, 0); -#else +#else v_res._v(0) = v._v(0)*_m(0, 0) + v._v(1)*_m(1, 0) + v._v(2)*_m(2, 0) + _m(3, 0); v_res._v(1) = v._v(0)*_m(0, 1) + v._v(1)*_m(1, 1) + v._v(2)*_m(2, 1) + _m(3, 1); v_res._v(2) = v._v(0)*_m(0, 2) + v._v(1)*_m(1, 2) + v._v(2)*_m(2, 2) + _m(3, 2); #endif // HAVE_EIGEN - + return v_res; } @@ -888,17 +888,17 @@ INLINE_LINMATH FLOATNAME(LVecBase3) FLOATNAME(LMatrix4):: xform_vec(const FLOATNAME(LVecBase3) &v) const { TAU_PROFILE("LVecBase3 LMatrix4::xform_vec(const LVecBase3 &)", " ", TAU_USER); FLOATNAME(LVecBase3) v_res; - + // v._v(3) == 0.0f for this case - + #ifdef HAVE_EIGEN v_res._v.noalias() = v._v * _m.block<3, 3>(0, 0); -#else +#else v_res._v(0) = v._v(0)*_m(0, 0) + v._v(1)*_m(1, 0) + v._v(2)*_m(2, 0); v_res._v(1) = v._v(0)*_m(0, 1) + v._v(1)*_m(1, 1) + v._v(2)*_m(2, 1); v_res._v(2) = v._v(0)*_m(0, 2) + v._v(1)*_m(1, 2) + v._v(2)*_m(2, 2); #endif // HAVE_EIGEN - + return v_res; } @@ -934,7 +934,7 @@ xform_in_place(FLOATNAME(LVecBase4) &v) const { #ifdef HAVE_EIGEN v._v = v._v * _m; -#else +#else v = xform(v); #endif // HAVE_EIGEN } @@ -953,7 +953,7 @@ xform_point_in_place(FLOATNAME(LVecBase3) &v) const { #ifdef HAVE_EIGEN v._v = v._v * _m.block<3, 3>(0, 0) + _m.block<1, 3>(3, 0); -#else +#else v = xform_point(v); #endif // HAVE_EIGEN } @@ -981,10 +981,10 @@ INLINE_LINMATH void FLOATNAME(LMatrix4):: xform_vec_in_place(FLOATNAME(LVecBase3) &v) const { TAU_PROFILE("void LMatrix4::xform_vec_in_place(LVecBase3 &)", " ", TAU_USER); // v._v(3) == 0.0f for this case - + #ifdef HAVE_EIGEN v._v = v._v * _m.block<3, 3>(0, 0); -#else +#else v = xform_vec(v); #endif // HAVE_EIGEN } @@ -1325,14 +1325,12 @@ transpose_in_place() { _m.transposeInPlace(); #else - #define SWAP__(x,y) { FLOATTYPE temp = (x); (x) = (y); (y) = temp;} - SWAP__(_m(0, 1),_m(1, 0)); - SWAP__(_m(0, 2),_m(2, 0)); - SWAP__(_m(0, 3),_m(3, 0)); - SWAP__(_m(1, 2),_m(2, 1)); - SWAP__(_m(1, 3),_m(3, 1)); - SWAP__(_m(2, 3),_m(3, 2)); - #undef SWAP__ + std::swap(_m(0, 1), _m(1, 0)); + std::swap(_m(0, 2), _m(2, 0)); + std::swap(_m(0, 3), _m(3, 0)); + std::swap(_m(1, 2), _m(2, 1)); + std::swap(_m(1, 3), _m(3, 1)); + std::swap(_m(2, 3), _m(3, 2)); #endif // HAVE_EIGEN } @@ -1356,13 +1354,19 @@ INLINE_LINMATH bool FLOATNAME(LMatrix4):: invert_from(const FLOATNAME(LMatrix4) &other) { TAU_PROFILE("bool LMatrix4::invert_from(const LMatrix4 &)", " ", TAU_USER); #ifdef HAVE_EIGEN + // We use the squared nearly_zero value as determinant threshold + // for checking whether a matrix is singular, since that's the + // same constant we use in the non-Eigen case (see lmatrix3_src.I) + // and also because we otherwise run into issues very quickly. bool invertible; - other._m.computeInverseWithCheck(_m, invertible); + other._m.computeInverseWithCheck(_m, invertible, + NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE)); if (!invertible) { -#ifdef NDEBUG +#ifdef NOTIFY_DEBUG linmath_cat.warning() << "Tried to invert singular LMatrix4.\n"; #endif + (*this) = ident_mat(); nassertr(!no_singular_invert, false); } @@ -1382,9 +1386,10 @@ invert_from(const FLOATNAME(LMatrix4) &other) { int index[4]; if (!decompose_mat(index)) { -#ifdef NDEBUG +#ifdef NOTIFY_DEBUG linmath_cat.warning() << "Tried to invert singular LMatrix4.\n"; #endif + (*this) = ident_mat(); nassertr(!no_singular_invert, false); return false; @@ -1441,11 +1446,11 @@ invert_affine_from(const FLOATNAME(LMatrix4) &other) { _m(3, 0) = -(other._m(3, 0) * _m(0, 0) + other._m(3, 1) * _m(1, 0) + other._m(3, 2) * _m(2, 0)); - + _m(3, 1) = -(other._m(3, 0) * _m(0, 1) + other._m(3, 1) * _m(1, 1) + other._m(3, 2) * _m(2, 1)); - + _m(3, 2) = -(other._m(3, 0) * _m(0, 2) + other._m(3, 1) * _m(1, 2) + other._m(3, 2) * _m(2, 2)); @@ -1481,17 +1486,17 @@ accumulate(const FLOATNAME(LMatrix4) &other, FLOATTYPE weight) { _m(0, 1) += other._m(0, 1) * weight; _m(0, 2) += other._m(0, 2) * weight; _m(0, 3) += other._m(0, 3) * weight; - + _m(1, 0) += other._m(1, 0) * weight; _m(1, 1) += other._m(1, 1) * weight; _m(1, 2) += other._m(1, 2) * weight; _m(1, 3) += other._m(1, 3) * weight; - + _m(2, 0) += other._m(2, 0) * weight; _m(2, 1) += other._m(2, 1) * weight; _m(2, 2) += other._m(2, 2) * weight; _m(2, 3) += other._m(2, 3) * weight; - + _m(3, 0) += other._m(3, 0) * weight; _m(3, 1) += other._m(3, 1) * weight; _m(3, 2) += other._m(3, 2) * weight;