Add heightmap and concave shape to 3d.

Cargo.toml

@@ -10,7 +10,7 @@ publish = false
 crate-type = ["cdylib"]
 
 [features]
-default = ["single-dim2", "serde-serialize", "parallel", "simd-stable"]
+default = ["single-dim3", "serde-serialize", "parallel", "simd-stable"]
 dim2 = []
 dim3 = []
 single = []

src/bodies/rapier_body.rs

@@ -7,7 +7,7 @@ use hashbrown::hash_set::HashSet;
 #[cfg(feature = "dim3")]
 use rapier::dynamics::LockedAxes;
 use rapier::geometry::ColliderHandle;
-use rapier::math::Real;
+use rapier::math::DEFAULT_EPSILON;
 use servers::rapier_physics_server_extra::PhysicsCollisionObjects;
 use servers::rapier_physics_server_extra::PhysicsShapes;
 use servers::rapier_physics_server_extra::PhysicsSpaces;
@@ -111,8 +111,10 @@ pub struct RapierBody {
  #[cfg(feature = "dim3")]
  axis_lock: u8,
  mass: real,
+ inv_mass: real,
  mass_properties_update_pending: bool,
  inertia: Angle,
+ inv_inertia: Angle,
  #[cfg(feature = "dim3")]
  inv_inertia_tensor: Basis,
  contact_skin: real,
@@ -164,8 +166,10 @@ impl RapierBody {
  #[cfg(feature = "dim3")]
  axis_lock: 0,
  mass: 1.0,
+ inv_mass: 1.0,
  mass_properties_update_pending: false,
  inertia: ANGLE_ZERO,
+ inv_inertia: ANGLE_ZERO,
  #[cfg(feature = "dim3")]
  inv_inertia_tensor: Basis::IDENTITY,
  contact_skin: RapierProjectSettings::get_contact_skin(),
@@ -1156,6 +1160,11 @@ impl RapierBody {
  return;
  }
  self.mass = mass_value;
+ if self.mass.is_zero_approx() {
+ self.inv_mass = 0.0;
+ } else {
+ self.inv_mass = 1.0 / self.mass;
+ }
  if self.base.mode.ord() >= BodyMode::RIGID.ord() {
  self.mass_properties_changed(physics_engine, physics_spaces);
  }
@@ -1195,10 +1204,18 @@ impl RapierBody {
  }
  }
  BodyParameter::CENTER_OF_MASS => {
+ #[cfg(feature = "dim2")]
  if p_value.get_type() != VariantType::VECTOR2 {
+ godot_error!("Invalid body data.");
+ return;
+ }
+ #[cfg(feature = "dim3")]
+ if p_value.get_type() != VariantType::VECTOR3 {
+ godot_error!("Invalid body data.");
  return;
  }
  self.center_of_mass = p_value.to();
+ self.calculate_center_of_mass = false;
  if self.base.mode.ord() >= BodyMode::RIGID.ord() {
  self.mass_properties_changed(physics_engine, physics_spaces);
  }
@@ -1548,107 +1565,74 @@ impl RapierBody {
  if self.base.mode.ord() < BodyMode::RIGID.ord() {
  return;
  }
- let mut total_area = 0.0;
- let shape_count = self.base.get_shape_count() as usize;
- for i in 0..shape_count {
- if self.base.is_shape_disabled(i) {
- continue;
+ // compute rigidbody mass properties by changing collider mass. Will get overriden later
+ let rigid_body_mass_properties = physics_engine.body_get_mass_properties(
+ self.base.get_space_handle(),
+ self.base.get_body_handle(),
+ self.mass,
+ );
+ if self.calculate_center_of_mass || self.calculate_inertia {
+ if self.calculate_center_of_mass {
+ self.center_of_mass =
+ vector_to_godot(rigid_body_mass_properties.local_mprops.local_com.coords);
  }
- if let Some(shape) = physics_shapes.get(&self.base.get_shape(i)) {
- total_area += shape.get_base().get_aabb_area();
+ if self.calculate_inertia {
+ let angular_inertia = rigid_body_mass_properties.local_mprops.principal_inertia();
+ self.inertia = angle_to_godot(angular_inertia);
  }
  }
- if self.calculate_center_of_mass {
- self.center_of_mass = Vector::default();
- if total_area != 0.0 {
- for i in 0..shape_count {
- if self.base.is_shape_disabled(i) {
- continue;
- }
- if let Some(shape) = physics_shapes.get(&self.base.get_shape(i)) {
- let shape_area = shape.get_base().get_aabb_area();
- if shape_area == 0.0 || self.mass == 0.0 {
- continue;
- }
- let shape_mass = shape_area * self.mass / total_area;
- // NOTE: we assume that the shape origin is also its center of mass.
- self.center_of_mass += shape_mass * self.base.get_shape_transform(i).origin;
- }
- }
- self.center_of_mass /= self.mass;
- }
+ if self.inertia.is_zero_approx() {
+ self.inv_inertia = ANGLE_ZERO;
  }
- if self.calculate_inertia {
- self.inertia = ANGLE_ZERO;
- if total_area != 0.0 {
- for i in 0..shape_count {
- if self.base.is_shape_disabled(i) {
- continue;
- }
- if let Some(shape) = physics_shapes.get(&self.base.get_shape(i)) {
- let shape_area = shape.get_base().get_aabb_area();
- if shape_area == 0.0 || self.mass == 0.0 {
- continue;
- }
- let shape_mass = shape_area * self.mass / total_area;
- let mtx = self.base.get_shape_transform(i);
- let scale = transform_scale(&mtx);
- self.inertia += self.get_inertia_for_shape(
- shape.get_moment_of_inertia(shape_mass, scale),
- shape_mass,
- mtx,
- i,
- );
- }
- }
+ #[cfg(feature = "dim2")]
+ if !self.inertia.is_zero_approx() {
+ self.inv_inertia = 1.0 / self.inertia;
+ }
+ #[cfg(feature = "dim3")]
+ if !self.inertia.is_zero_approx() {
+ // inv inertia
+ if !self.inv_inertia.x.is_zero_approx() {
+ self.inv_inertia.x = 1.0 / self.inertia.x;
+ } else {
+ self.inv_inertia.x = 0.0;
  }
+ if !self.inv_inertia.y.is_zero_approx() {
+ self.inv_inertia.y = 1.0 / self.inertia.y;
+ } else {
+ self.inv_inertia.y = 0.0;
+ }
+ if !self.inv_inertia.z.is_zero_approx() {
+ self.inv_inertia.z = 1.0 / self.inertia.z;
+ } else {
+ self.inv_inertia.z = 0.0;
+ }
+ // inv inertia tensor
+ let rotation_matrix = rigid_body_mass_properties
+ .local_mprops
+ .principal_inertia_local_frame
+ .to_rotation_matrix();
+ let vector = rotation_matrix.matrix();
+ let column_0 = vector
+ .column(0)
+ .pseudo_inverse(DEFAULT_EPSILON)
+ .unwrap_or_default();
+ let column_1 = vector
+ .column(1)
+ .pseudo_inverse(DEFAULT_EPSILON)
+ .unwrap_or_default();
+ let column_2 = vector
+ .column(2)
+ .pseudo_inverse(DEFAULT_EPSILON)
+ .unwrap_or_default();
+ self.inv_inertia_tensor = Basis::from_cols(
+ Vector3::new(column_0.x, column_0.y, column_0.z),
+ Vector3::new(column_1.x, column_1.y, column_1.z),
+ Vector3::new(column_2.x, column_2.y, column_2.z),
+ );
  }
  self.apply_mass_properties(force_update, physics_engine);
  }
 
- #[cfg(feature = "dim2")]
- fn get_inertia_for_shape(
- &self,
- moment_of_inertia: Angle,
- shape_mass: Real,
- shape_transform: Transform,
- _i: usize,
- ) -> Real {
- let mtx = shape_transform;
- let _scale = transform_scale(&mtx);
- let shape_origin = mtx.origin - self.center_of_mass;
- moment_of_inertia + shape_mass * shape_origin.length_squared()
- }
-
- #[cfg(feature = "dim3")]
- fn get_inertia_for_shape(
- &self,
- moment_of_inertia: Angle,
- shape_mass: Real,
- shape_transform: Transform,
- _i: usize,
- ) -> Vector3 {
- let mut shape_inertia_tensor = Basis::from_scale(moment_of_inertia);
- let shape_transform = shape_transform;
- let 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();
- let shape_origin = shape_transform.origin - self.center_of_mass;
- let shape_outer = shape_origin.outer(shape_origin);
- let mut shape_dot = Basis::IDENTITY * (shape_origin.dot(shape_origin));
- shape_dot.set_col_a(shape_dot.col_a() - shape_outer.col_a());
- shape_dot.set_col_b(shape_dot.col_b() - shape_outer.col_b());
- shape_dot.set_col_c(shape_dot.col_c() - shape_outer.col_c());
- shape_dot *= shape_mass;
- let mut inertia_tensor = shape_inertia_tensor;
- inertia_tensor.set_col_a(inertia_tensor.col_a() + shape_dot.col_a());
- inertia_tensor.set_col_b(inertia_tensor.col_b() + shape_dot.col_b());
- inertia_tensor.set_col_c(inertia_tensor.col_c() + shape_dot.col_c());
- //return inertia_tensor.diagonalize().transposed();
- // TODO
- moment_of_inertia
- }
-
  pub fn reset_mass_properties(
  &mut self,
  physics_engine: &mut PhysicsEngine,
@@ -1668,30 +1652,11 @@ impl RapierBody {
  }
 
  pub fn get_inv_mass(&self) -> real {
- if self.mass != 0.0 {
- return 1.0 / self.mass;
- }
- 0.0
+ self.inv_mass
  }
 
- #[cfg(feature = "dim2")]
  pub fn get_inv_inertia(&self) -> Angle {
- if self.inertia != ANGLE_ZERO {
- return 1.0 / self.inertia;
- }
- ANGLE_ZERO
- }
-
- #[cfg(feature = "dim3")]
- pub fn get_inv_inertia(&self) -> Angle {
- if self.inertia != ANGLE_ZERO {
- return Vector3::new(
- 1.0 / self.inertia.x,
- 1.0 / self.inertia.y,
- 1.0 / self.inertia.z,
- );
- }
- ANGLE_ZERO
+ self.inv_inertia
  }
 
  #[cfg(feature = "dim3")]

src/bodies/rapier_direct_body_state_impl.rs

@@ -144,7 +144,15 @@ impl RapierDirectBodyStateImpl {
 
  #[cfg(feature = "dim3")]
  pub(super) fn get_inverse_inertia_tensor(&self) -> Basis {
- // TODO
+ let Some(ref physics_singleton) = self.physics_singleton else {
+ return Basis::IDENTITY;
+ };
+ let physics_data = &physics_singleton.bind().implementation.physics_data;
+ if let Some(body) = physics_data.collision_objects.get(&self.body) {
+ if let Some(body) = body.get_body() {
+ return body.get_inv_inertia_tensor();
+ }
+ }
  Basis::IDENTITY
  }
 

src/rapier_wrapper/body.rs

@@ -1,5 +1,3 @@
-use godot::builtin::math::FloatExt;
-use godot::log::godot_error;
 use nalgebra::Point;
 use rapier::prelude::*;
 
@@ -50,7 +48,7 @@ impl PhysicsEngine {
  let default_activation = RigidBodyActivation::default();
  activation.angular_threshold = default_activation.angular_threshold;
  activation.normalized_linear_threshold = default_activation.normalized_linear_threshold;
- activation.time_until_sleep = 0.5;
+ //activation.time_until_sleep = 0.5;
  set_rigid_body_properties_internal(&mut rigid_body, pos, rot, true);
  rigid_body.user_data = user_data.get_data();
  physics_world
@@ -371,7 +369,7 @@ impl PhysicsEngine {
  activation.angular_threshold = default_activation.angular_threshold;
  activation.normalized_linear_threshold =
  default_activation.normalized_linear_threshold;
- activation.time_until_sleep = 0.5;
+ //activation.time_until_sleep = 0.5;
  }
  if !can_sleep && body.is_sleeping() {
  body.wake_up(true);
@@ -402,7 +400,7 @@ impl PhysicsEngine {
  body_handle: RigidBodyHandle,
  mass: Real,
  inertia: AngVector<Real>,
- _local_com: Vector<Real>,
+ local_com: Vector<Real>,
  wake_up: bool,
  force_update: bool,
  ) {
@@ -418,33 +416,19 @@ impl PhysicsEngine {
  } else {
  body.lock_rotations(false, wake_up);
  }
- let colliders = body.colliders();
- let colliders_len_inv = 1.0 / (colliders.len() as f32);
- for collider in colliders {
+ for collider in body.colliders() {
  if let Some(collider) = physics_world
  .physics_objects
  .collider_set
  .get_mut(*collider)
  {
- // reuse local center
- let mass_properties = collider.shape().mass_properties(1.0);
- if mass_properties.mass().is_zero_approx() {
- godot_error!("Collider has zero computed mass");
- }
- let local_com = mass_properties.local_com;
- //let shape_inertia = mass_properties.inv_principal_inertia_sqrt;
- let _mass_properties = MassProperties::new(
- local_com,
- colliders_len_inv,
- inertia, // * colliders_len_inv,
- );
- //collider.set_mass_properties(mass_properties);
- collider.set_mass(colliders_len_inv);
- //collider.set_density(colliders_len_inv)
+ collider.set_density(0.0);
  }
  }
- let _props = MassProperties::new(Point { coords: _local_com }, mass, inertia);
- //body.set_additional_mass_properties(props, true);
+ body.set_additional_mass_properties(
+ MassProperties::new(Point { coords: local_com }, mass, inertia),
+ wake_up,
+ );
  if force_update {
  body.recompute_mass_properties_from_colliders(
  &physics_world.physics_objects.collider_set,
@@ -635,4 +619,33 @@ impl PhysicsEngine {
  body.sleep();
  }
  }
+
+ pub fn body_get_mass_properties(
+ &mut self,
+ world_handle: WorldHandle,
+ body_handle: RigidBodyHandle,
+ mass: Real,
+ ) -> RigidBodyMassProps {
+ if let Some(physics_world) = self.get_mut_world(world_handle)
+ && let Some(body) = physics_world
+ .physics_objects
+ .rigid_body_set
+ .get_mut(body_handle)
+ {
+ for collider in body.colliders() {
+ if let Some(collider) = physics_world
+ .physics_objects
+ .collider_set
+ .get_mut(*collider)
+ {
+ collider.set_mass(mass);
+ }
+ }
+ body.recompute_mass_properties_from_colliders(
+ &physics_world.physics_objects.collider_set,
+ );
+ return body.mass_properties().clone();
+ }
+ RigidBodyMassProps::default()
+ }
 }