diff options
Diffstat (limited to 'modules/bullet')
| -rw-r--r-- | modules/bullet/bullet_types_converter.cpp | 59 | ||||
| -rw-r--r-- | modules/bullet/rigid_body_bullet.cpp | 4 | ||||
| -rw-r--r-- | modules/bullet/shape_bullet.cpp | 2 |
3 files changed, 57 insertions, 8 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]); } diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp index 32c3240a35..f517eecf64 100644 --- a/modules/bullet/rigid_body_bullet.cpp +++ b/modules/bullet/rigid_body_bullet.cpp @@ -849,8 +849,8 @@ void RigidBodyBullet::on_enter_area(AreaBullet *p_area) { } else { if (areasWhereIam[i]->get_spOv_priority() > p_area->get_spOv_priority()) { // The position was found, just shift all elements - for (int j = i; j < areaWhereIamCount; ++j) { - areasWhereIam[j + 1] = areasWhereIam[j]; + for (int j = areaWhereIamCount; j > i; j--) { + areasWhereIam[j] = areasWhereIam[j - 1]; } areasWhereIam[i] = p_area; break; diff --git a/modules/bullet/shape_bullet.cpp b/modules/bullet/shape_bullet.cpp index 274493ed17..74d6e073b3 100644 --- a/modules/bullet/shape_bullet.cpp +++ b/modules/bullet/shape_bullet.cpp @@ -308,7 +308,7 @@ void CapsuleShapeBullet::setup(real_t p_height, real_t p_radius) { } btCollisionShape *CapsuleShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { - return prepare(ShapeBullet::create_shape_capsule(radius * p_implicit_scale[0] + p_extra_edge, height * p_implicit_scale[1] + p_extra_edge)); + return prepare(ShapeBullet::create_shape_capsule(radius * p_implicit_scale[0] + p_extra_edge, height * p_implicit_scale[1])); } /* Cylinder */ |