RomanNum3ral
/
open_toontown_panda3d
mirror of https://github.com/open-toontown/panda3d.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix issue with Eigen when matrix determinant is nearly 0 

also fix error message for singular matrix
This commit is contained in:
rdb 2014-12-10 16:32:04 +01:00
parent 40a40b20cf
commit 5aaaf72851
3 changed files with 51 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
dtool/src/dtoolbase/nearly_zero.h
View File

@ -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.

31
panda/src/linmath/lmatrix3_src.I
View File

@ -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();

59
panda/src/linmath/lmatrix4_src.I
View File

@ -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;