Merge pull request #49185 from DrRevert/inertia_nan_fix

GodotPhysics: Avoid NaNs when calculating inertias for bodies without mass/area

1 files changed, 14 insertions, 9 deletions

diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index 4357c474e4..e4f9548a61 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/body_3d_sw.cpp
@@ -65,16 +65,18 @@ void Body3DSW::update_inertias() {
 			// We have to recompute the center of mass.
 			center_of_mass_local.zero();
 
-			for (int i = 0; i < get_shape_count(); i++) {
-				real_t area = get_shape_area(i);
+			if (total_area != 0.0) {
+				for (int i = 0; i < get_shape_count(); i++) {
+					real_t area = get_shape_area(i);
 
-				real_t mass = area * this->mass / total_area;
+					real_t mass = area * this->mass / total_area;
 
-				// NOTE: we assume that the shape origin is also its center of mass.
-				center_of_mass_local += mass * get_shape_transform(i).origin;
-			}
+					// NOTE: we assume that the shape origin is also its center of mass.
+					center_of_mass_local += mass * get_shape_transform(i).origin;
+				}
 
-			center_of_mass_local /= mass;
+				center_of_mass_local /= mass;
+			}
 
 			// Recompute the inertia tensor.
 			Basis inertia_tensor;
@@ -86,12 +88,15 @@ void Body3DSW::update_inertias() {
 					continue;
 				}
 
+				real_t area = get_shape_area(i);
+				if (area == 0.0) {
+					continue;
+				}
+
 				inertia_set = true;
 
 				const Shape3DSW *shape = get_shape(i);
 
-				real_t area = get_shape_area(i);
-
 				real_t mass = area * this->mass / total_area;
 
 				Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix();