diff options
| -rw-r--r-- | modules/bullet/bullet_types_converter.cpp | 59 |
1 files changed, 54 insertions, 5 deletions
diff --git a/modules/bullet/bullet_types_converter.cpp b/modules/bullet/bullet_types_converter.cpp index c9493d8892..7ecad9b78a 100644 --- a/modules/bullet/bullet_types_converter.cpp +++ b/modules/bullet/bullet_types_converter.cpp @@ -95,12 +95,61 @@ void G_TO_B(Transform const &inVal, btTransform &outVal) { } void UNSCALE_BT_BASIS(btTransform &scaledBasis) { - btMatrix3x3 &m(scaledBasis.getBasis()); - btVector3 column0(m[0][0], m[1][0], m[2][0]); - btVector3 column1(m[0][1], m[1][1], m[2][1]); - btVector3 column2(m[0][2], m[1][2], m[2][2]); + btMatrix3x3 &basis(scaledBasis.getBasis()); + btVector3 column0 = basis.getColumn(0); + btVector3 column1 = basis.getColumn(1); + btVector3 column2 = basis.getColumn(2); + + // Check for zero scaling. + if (btFuzzyZero(column0[0])) { + if (btFuzzyZero(column1[1])) { + if (btFuzzyZero(column2[2])) { + // All dimensions are fuzzy zero. Create a default basis. + column0 = btVector3(1, 0, 0); + column1 = btVector3(0, 1, 0); + column2 = btVector3(0, 0, 1); + } else { // Column 2 scale not fuzzy zero. + // Create two vectors orthogonal to row 2. + // Ensure that a default basis is created if row 2 = <0, 0, 1> + column1 = btVector3(0, column2[2], -column2[1]); + column0 = column1.cross(column2); + } + } else { // Column 1 scale not fuzzy zero. + if (btFuzzyZero(column2[2])) { + // Create two vectors othogonal to column 1. + // Ensure that a default basis is created if column 1 = <0, 1, 0> + column0 = btVector3(column1[1], -column1[0], 0); + column2 = column0.cross(column1); + } else { // Column 1 and column 2 scales not fuzzy zero. + // Create column 0 orthogonal to column 1 and column 2. + column0 = column1.cross(column2); + } + } + } else { // Column 0 scale not fuzzy zero. + if (btFuzzyZero(column1[1])) { + if (btFuzzyZero(column2[2])) { + // Create two vectors orthogonal to column 0. + // Ensure that a default basis is created if column 0 = <1, 0, 0> + column2 = btVector3(-column0[2], 0, column0[0]); + column1 = column2.cross(column0); + } else { // Column 0 and column 2 scales not fuzzy zero. + // Create column 1 orthogonal to column 0 and column 2. + column1 = column2.cross(column0); + } + } else { // Column 0 and column 1 scales not fuzzy zero. + if (btFuzzyZero(column2[2])) { + // Create column 2 orthogonal to column 0 and column 1. + column2 = column0.cross(column1); + } + } + } + + // Normalize column0.normalize(); column1.normalize(); column2.normalize(); - m.setValue(column0[0], column1[0], column2[0], column0[1], column1[1], column2[1], column0[2], column1[2], column2[2]); + + basis.setValue(column0[0], column1[0], column2[0], + column0[1], column1[1], column2[1], + column0[2], column1[2], column2[2]); } |