diff options
Diffstat (limited to 'servers/physics/body_sw.cpp')
| -rw-r--r-- | servers/physics/body_sw.cpp | 64 |
1 files changed, 47 insertions, 17 deletions
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index 74bba4f97..2f859a4ed 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -37,12 +37,16 @@ void BodySW::_update_inertia() { } +void BodySW::_update_transform_dependant() { + + center_of_mass = get_transform().basis.xform(center_of_mass_local); + principal_inertia_axes = get_transform().basis * principal_inertia_axes_local; -void BodySW::_update_inertia_tensor() { - - Matrix3 tb = get_transform().basis; + // update inertia tensor + Matrix3 tb = principal_inertia_axes; + Matrix3 tbt = tb.transposed(); tb.scale(_inv_inertia); - _inv_inertia_tensor = tb * get_transform().basis.transposed(); + _inv_inertia_tensor = tb * tbt; } @@ -54,33 +58,56 @@ void BodySW::update_inertias() { case PhysicsServer::BODY_MODE_RIGID: { - //update tensor for allshapes, 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) float total_area=0; for (int i=0;i<get_shape_count();i++) { - total_area+=get_shape_aabb(i).get_area(); + total_area+=get_shape_area(i); + } + + // We have to recompute the center of mass + center_of_mass_local.zero(); + + for (int i=0; i<get_shape_count(); i++) { + float area=get_shape_area(i); + + float 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; } - Vector3 _inertia; + center_of_mass_local /= mass; + // Recompute the inertia tensor + Matrix3 inertia_tensor; + inertia_tensor.set_zero(); for (int i=0;i<get_shape_count();i++) { const ShapeSW* shape=get_shape(i); - float area=get_shape_aabb(i).get_area(); + float area=get_shape_area(i); float mass = area * this->mass / total_area; - _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin(); + Matrix3 shape_inertia_tensor=shape->get_moment_of_inertia(mass).to_diagonal_matrix(); + Transform shape_transform=get_shape_transform(i); + Matrix3 shape_basis = shape_transform.basis.orthonormalized(); + + // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor! + shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed(); + + Vector3 shape_origin = shape_transform.origin - center_of_mass_local; + inertia_tensor += shape_inertia_tensor + (Matrix3()*shape_origin.dot(shape_origin)-shape_origin.outer(shape_origin))*mass; + } - if (_inertia!=Vector3()) - _inv_inertia=_inertia.inverse(); - else - _inv_inertia=Vector3(); + // Compute the principal axes of inertia + principal_inertia_axes_local = inertia_tensor.diagonalize().transposed(); + _inv_inertia = inertia_tensor.get_main_diagonal().inverse(); if (mass) _inv_mass=1.0/mass; @@ -92,20 +119,21 @@ void BodySW::update_inertias() { case PhysicsServer::BODY_MODE_KINEMATIC: case PhysicsServer::BODY_MODE_STATIC: { - _inv_inertia=Vector3(); + _inv_inertia_tensor.set_zero(); _inv_mass=0; } break; case PhysicsServer::BODY_MODE_CHARACTER: { - _inv_inertia=Vector3(); + _inv_inertia_tensor.set_zero(); _inv_mass=1.0/mass; } break; } - _update_inertia_tensor(); + //_update_shapes(); + _update_transform_dependant(); } @@ -582,6 +610,8 @@ void BodySW::integrate_velocities(real_t p_step) { if (ang_vel!=0.0) { Vector3 ang_vel_axis = total_angular_velocity / ang_vel; Matrix3 rot( ang_vel_axis, -ang_vel*p_step ); + Matrix3 identity3(1, 0, 0, 0, 1, 0, 0, 0, 1); + transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local); transform.basis = rot * transform.basis; transform.orthonormalize(); } @@ -598,7 +628,7 @@ void BodySW::integrate_velocities(real_t p_step) { _set_transform(transform); _set_inv_transform(get_transform().inverse()); - _update_inertia_tensor(); + _update_transform_dependant(); //if (fi_callback) { |