Skip to content

Commit

Permalink
Fixes division by zero when 3d body does not have valid shape
Browse files Browse the repository at this point in the history
Fixes #46738 by setting the default inertia to a valid value when there are no valid shapes for a 3d body.
Changed the comment style for the update_inertias method as well.
  • Loading branch information
likeich committed Mar 9, 2021
1 parent 85cb3c0 commit 5549909
Showing 1 changed file with 14 additions and 6 deletions.
20 changes: 14 additions & 6 deletions servers/physics_3d/body_3d_sw.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,40 +51,43 @@ void Body3DSW::_update_transform_dependant() {
}

void Body3DSW::update_inertias() {
//update shapes and motions
// Update shapes and motions.

switch (mode) {
case PhysicsServer3D::BODY_MODE_RIGID: {
//update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet)
// Update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet)
real_t total_area = 0;

for (int i = 0; i < get_shape_count(); i++) {
total_area += get_shape_area(i);
}

// We have to recompute the center of mass
// 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);

real_t mass = area * this->mass / total_area;

// NOTE: we assume that the shape origin is also its center of mass
// 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;

// Recompute the inertia tensor
// Recompute the inertia tensor.
Basis inertia_tensor;
inertia_tensor.set_zero();
bool inertia_set = false;

for (int i = 0; i < get_shape_count(); i++) {
if (is_shape_disabled(i)) {
continue;
}

inertia_set = true;

const Shape3DSW *shape = get_shape(i);

real_t area = get_shape_area(i);
Expand All @@ -102,7 +105,12 @@ void Body3DSW::update_inertias() {
inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
}

// Compute the principal axes of inertia
// Set the inertia to a valid value when there are no valid shapes.
if (!inertia_set) {
inertia_tensor.set_diagonal(Vector3(1.0, 1.0, 1.0));
}

// Compute the principal axes of inertia.
principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
_inv_inertia = inertia_tensor.get_main_diagonal().inverse();

Expand Down

0 comments on commit 5549909

Please sign in to comment.