Merge pull request #41426 from madmiraal/fix-bullet-zero-scale

Check and correct for zero scaling when unscaling Bullet basis.

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]);
 }