Add Products of Inertia to RigidBody3D

core/math/eigen_value_symmetric.h

@@ -0,0 +1,146 @@
+/**************************************************************************/
+/*  eigen_value_symmetric.h                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/basis.h"
+#include "core/math/math_funcs.h"
+#include "core/math/vector3.h"
+
+// This code is simply a re-implementation of the same function from Jolt Physics,
+// but using Godot's Vector3 and Basis structures
+bool eigen_value_symmetric(const Basis &in_matrix, Basis &out_eig_vec, Vector3 &out_eig_val) {
+	const int MAX_SWEEPS = 50;
+	const int N = 3;
+
+	Basis a = in_matrix;
+
+	Vector3 b, z;
+
+	for (int ip = 0; ip < N; ++ip) {
+		// Initialize b and output to diagonal of a
+		b[ip] = a[ip][ip];
+		out_eig_val[ip] = a[ip][ip];
+
+		// reset z
+		z[ip] = 0.0;
+	}
+
+	for (int sweep = 0; sweep < MAX_SWEEPS; ++sweep) {
+		// sum the off-diagonal elements of a
+		real_t sm = 0.0;
+		for (int ip = 0; ip < N; ++ip) {
+			for (int iq = ip + 1; iq < N; ++iq) {
+				sm += Math::abs(a[ip][iq]);
+			}
+		}
+		real_t avg_sm = sm / (N * N);
+
+		// Normal return
+		if (avg_sm < CMP_EPSILON2) {
+			return true;
+		}
+
+		real_t thresh = sweep < 4 ? 0.2 * avg_sm : CMP_EPSILON2;
+
+		for (int ip = 0; ip < N - 1; ++ip) {
+			for (int iq = ip + 1; iq < N; ++iq) {
+				real_t &a_pq = a[ip][iq];
+				real_t &eigval_p = out_eig_val[ip];
+				real_t &eigval_q = out_eig_val[iq];
+
+				real_t abs_a_pq = Math::abs(a_pq);
+				real_t g = 100.0 * abs_a_pq;
+
+				// After four sweeps, skip the rotation if the off-diagonal element is small
+				if (sweep > 4 && Math::abs(eigval_p) + g == Math::abs(eigval_p) && Math::abs(eigval_q) + g == Math::abs(eigval_q)) {
+					a_pq = 0.0;
+				} else if (abs_a_pq > thresh) {
+					real_t h = eigval_q - eigval_p;
+					real_t abs_h = Math::abs(h);
+
+					real_t t;
+					if (abs_h + g == abs_h) {
+						t = a_pq / h;
+					} else {
+						real_t theta = 0.5 * h / a_pq; // Warning: Can become infinite if a(ip, iq) is very small which may trigger an invalid float exception
+						t = 1.0 / (Math::abs(theta) + sqrt(1.0 + theta * theta)); // If theta becomes inf, t will be 0 so the infinite is not a problem for the algorithm
+						if (theta < 0.0f) {
+							t = -t;
+						}
+					}
+					real_t c = 1.0 / sqrt(1.0 + t * t);
+					real_t s = t * c;
+					real_t tau = s / (1.0 + c);
+					h = t * a_pq;
+
+					a_pq = 0.0;
+
+					z[ip] -= h;
+					z[iq] += h;
+
+					eigval_p -= h;
+					eigval_q += h;
+
+#define GODOT_EVS_ROTATE(a, i, j, k, l) \
+	g = a[i][j],                        \
+	h = a[k][l],                        \
+	a[i][j] = g - s * (h + g * tau),    \
+	a[k][l] = h + s * (g - h * tau)
+
+					int j;
+					for (j = 0; j < ip; ++j) {
+						GODOT_EVS_ROTATE(a, j, ip, j, iq);
+					}
+					for (j = ip + 1; j < iq; ++j) {
+						GODOT_EVS_ROTATE(a, ip, j, j, iq);
+					}
+					for (j = iq + 1; j < N; ++j) {
+						GODOT_EVS_ROTATE(a, ip, j, iq, j);
+					}
+					for (j = 0; j < N; ++j) {
+						GODOT_EVS_ROTATE(out_eig_vec, j, ip, j, iq);
+					}
+
+#undef GODOT_EVS_ROTATE
+				}
+			}
+		}
+		// Update eigenvalues with the sum of ta_pq and reinitialize z
+		for (int ip = 0; ip < N; ++ip) {
+			b[ip] += z[ip];
+			out_eig_val[ip] = b[ip];
+			z[ip] = 0.0;
+		}
+	}
+
+	// Too many iterations
+	return false;
+}

doc/classes/PhysicsServer3D.xml

@@ -1782,25 +1782,28 @@
 		<constant name="BODY_PARAM_INERTIA" value="3" enum="BodyParameter">
 			Constant to set/get a body's inertia.
 		</constant>
-		<constant name="BODY_PARAM_CENTER_OF_MASS" value="4" enum="BodyParameter">
+		<constant name="BODY_PARAM_PROD_INERTIA" value="4" enum="BodyParameter">
+			Constant to set/get a body's product of inertia.
+		</constant>
+		<constant name="BODY_PARAM_CENTER_OF_MASS" value="5" enum="BodyParameter">
 			Constant to set/get a body's center of mass position in the body's local coordinate system.
 		</constant>
-		<constant name="BODY_PARAM_GRAVITY_SCALE" value="5" enum="BodyParameter">
+		<constant name="BODY_PARAM_GRAVITY_SCALE" value="6" enum="BodyParameter">
 			Constant to set/get a body's gravity multiplier.
 		</constant>
-		<constant name="BODY_PARAM_LINEAR_DAMP_MODE" value="6" enum="BodyParameter">
+		<constant name="BODY_PARAM_LINEAR_DAMP_MODE" value="7" enum="BodyParameter">
 			Constant to set/get a body's linear damping mode. See [enum BodyDampMode] for possible values.
 		</constant>
-		<constant name="BODY_PARAM_ANGULAR_DAMP_MODE" value="7" enum="BodyParameter">
+		<constant name="BODY_PARAM_ANGULAR_DAMP_MODE" value="8" enum="BodyParameter">
 			Constant to set/get a body's angular damping mode. See [enum BodyDampMode] for possible values.
 		</constant>
-		<constant name="BODY_PARAM_LINEAR_DAMP" value="8" enum="BodyParameter">
+		<constant name="BODY_PARAM_LINEAR_DAMP" value="9" enum="BodyParameter">
 			Constant to set/get a body's linear damping factor.
 		</constant>
-		<constant name="BODY_PARAM_ANGULAR_DAMP" value="9" enum="BodyParameter">
+		<constant name="BODY_PARAM_ANGULAR_DAMP" value="10" enum="BodyParameter">
 			Constant to set/get a body's angular damping factor.
 		</constant>
-		<constant name="BODY_PARAM_MAX" value="10" enum="BodyParameter">
+		<constant name="BODY_PARAM_MAX" value="11" enum="BodyParameter">
 			Represents the size of the [enum BodyParameter] enum.
 		</constant>
 		<constant name="BODY_DAMP_MODE_COMBINE" value="0" enum="BodyDampMode">

doc/classes/RigidBody3D.xml

@@ -233,6 +233,11 @@
 			The physics material override for the body.
 			If a material is assigned to this property, it will be used instead of any other physics material, such as an inherited one.
 		</member>
+		<member name="product_of_inertia" type="Vector3" setter="set_product_of_inertia" getter="get_product_of_inertia" default="Vector3(0, 0, 0)">
+			The body's products of inertia in (Iyz, Ixz, Ixy) order. Like the inertia parameter, this also determines how much torque it takes to rotate an object. However, it describes how asymmetrically the mass is distributed relative to the body's axes. When combined with the inertia vector, the local inertia tensor can be fully defined. However, for most applications, this can safely be left at zero.
+			If inertia is computed automatically, the value set here will be ignored.
+			[b]Note:[/b] The "alternate" convention for products of inertia is used. These values will be negated in the inertia tensor.
+		</member>
 		<member name="sleeping" type="bool" setter="set_sleeping" getter="is_sleeping" default="false">
 			If [code]true[/code], the body will not move and will not calculate forces until woken up by another body through, for example, a collision, or by using the [method apply_impulse] or [method apply_force] methods.
 		</member>

modules/godot_physics_3d/godot_body_3d.cpp

@@ -30,6 +30,7 @@
 
 #include "godot_body_3d.h"
 
+#include "core/math/eigen_value_symmetric.h"
 #include "godot_area_3d.h"
 #include "godot_body_direct_state_3d.h"
 #include "godot_constraint_3d.h"
@@ -41,6 +42,35 @@ void GodotBody3D::_mass_properties_changed() {
 	}
 }
 
+void GodotBody3D::_update_local_inertia() {
+	// Set local inertia tensor to diagonal matrix
+	inertia_tensor_local = Basis();
+	inertia_tensor_local.scale(inertia);
+
+	// Add off-diagonal entries using the "alternate" convention
+	// https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor
+	inertia_tensor_local[1][2] = -product_of_inertia.x;
+	inertia_tensor_local[2][1] = -product_of_inertia.x;
+
+	inertia_tensor_local[0][2] = -product_of_inertia.y;
+	inertia_tensor_local[2][0] = -product_of_inertia.y;
+
+	inertia_tensor_local[0][1] = -product_of_inertia.z;
+	inertia_tensor_local[1][0] = -product_of_inertia.z;
+
+	principal_inertia_axes_local = Basis();
+	Vector3 principal_inertia = Vector3();
+
+	// Compute the principal axes and moments of inertia.
+	if (!eigen_value_symmetric(inertia_tensor_local, principal_inertia_axes_local, principal_inertia)) {
+		// Set to identity and on-axis inertia if it fails.
+		WARN_PRINT("Unable to determine principle axes of inertia. Falling back to on-axis inertia only.");
+		principal_inertia_axes_local = Basis();
+		principal_inertia = inertia;
+	}
+	_inv_inertia = principal_inertia.inverse();
+}
+
 void GodotBody3D::_update_transform_dependent() {
 	center_of_mass = get_transform().basis.xform(center_of_mass_local);
 	principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
@@ -216,10 +246,22 @@ void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Varian
 			} else {
 				calculate_inertia = false;
 				if (mode == PhysicsServer3D::BODY_MODE_RIGID) {
-					principal_inertia_axes_local = Basis();
-					_inv_inertia = inertia.inverse();
+					_update_local_inertia();
+					_update_transform_dependent();
+				}
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_PROD_INERTIA: {
+			if (!calculate_inertia) {
+				product_of_inertia = p_value;
+				if (mode == PhysicsServer3D::BODY_MODE_RIGID) {
+					_update_local_inertia();
 					_update_transform_dependent();
 				}
+			} else {
+				product_of_inertia.x = 0.0;
+				product_of_inertia.y = 0.0;
+				product_of_inertia.z = 0.0;
 			}
 		} break;
 		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
@@ -265,7 +307,16 @@ Variant GodotBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const {
 		} break;
 		case PhysicsServer3D::BODY_PARAM_INERTIA: {
 			if (mode == PhysicsServer3D::BODY_MODE_RIGID) {
-				return _inv_inertia.inverse();
+				return inertia_tensor_local.get_main_diagonal();
+			} else {
+				return Vector3();
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_PROD_INERTIA: {
+			if (mode == PhysicsServer3D::BODY_MODE_RIGID) {
+				return Vector3(-inertia_tensor_local[1][2],
+						-inertia_tensor_local[0][2],
+						-inertia_tensor_local[0][1]);
 			} else {
 				return Vector3();
 			}
@@ -320,7 +371,7 @@ void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
 			if (!calculate_inertia) {
 				principal_inertia_axes_local = Basis();
-				_inv_inertia = inertia.inverse();
+				_update_local_inertia();
 				_update_transform_dependent();
 			}
 			_mass_properties_changed();

modules/godot_physics_3d/godot_body_3d.h

@@ -56,6 +56,7 @@ class GodotBody3D : public GodotCollisionObject3D {
 	real_t bounce = 0.0;
 	real_t friction = 1.0;
 	Vector3 inertia;
+	Vector3 product_of_inertia;
 
 	PhysicsServer3D::BodyDampMode linear_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
 	PhysicsServer3D::BodyDampMode angular_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
@@ -74,6 +75,7 @@ class GodotBody3D : public GodotCollisionObject3D {
 	Vector3 _inv_inertia; // Relative to the principal axes of inertia
 
 	// Relative to the local frame of reference
+	Basis inertia_tensor_local;
 	Basis principal_inertia_axes_local;
 	Vector3 center_of_mass_local;
 
@@ -146,6 +148,7 @@ class GodotBody3D : public GodotCollisionObject3D {
 	uint64_t island_step = 0;
 
 	void _update_transform_dependent();
+	void _update_local_inertia();
 
 	friend class GodotPhysicsDirectBodyState3D; // i give up, too many functions to expose
 
@@ -203,6 +206,7 @@ class GodotBody3D : public GodotCollisionObject3D {
 	_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; }
 	_FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
 
+	_FORCE_INLINE_ Basis get_inertia_tensor_local() const { return inertia_tensor_local; }
 	_FORCE_INLINE_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; }
 	_FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; }
 	_FORCE_INLINE_ Vector3 get_center_of_mass_local() const { return center_of_mass_local; }

modules/jolt_physics/objects/jolt_body_3d.cpp

@@ -292,6 +292,20 @@ JPH::MassProperties JoltBody3D::_calculate_mass_properties(const JPH::Shape &p_s
 		mass_properties.mInertia(1, 2) = 0;
 	}
 
+	// If using custom inertia, set products of inertia
+	// Uses the "alternate" convention
+	// https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor
+	if (!calculate_inertia) {
+		mass_properties.mInertia(1, 2) = -(float)product_of_inertia.x; // Iyz
+		mass_properties.mInertia(2, 1) = -(float)product_of_inertia.x; // Iyz
+
+		mass_properties.mInertia(0, 2) = -(float)product_of_inertia.y; // Ixz
+		mass_properties.mInertia(2, 0) = -(float)product_of_inertia.y; // Ixz
+
+		mass_properties.mInertia(0, 1) = -(float)product_of_inertia.z; // Ixy
+		mass_properties.mInertia(1, 0) = -(float)product_of_inertia.z; // Ixy
+	}
+
 	mass_properties.mInertia(3, 3) = 1.0f;
 
 	return mass_properties;
@@ -666,6 +680,9 @@ void JoltBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Variant
 		case PhysicsServer3D::BODY_PARAM_INERTIA: {
 			set_inertia(p_value);
 		} break;
+		case PhysicsServer3D::BODY_PARAM_PROD_INERTIA: {
+			set_product_of_inertia(p_value);
+		} break;
 		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
 			set_center_of_mass_custom(p_value);
 		} break;
@@ -1219,6 +1236,13 @@ void JoltBody3D::set_inertia(const Vector3 &p_inertia) {
 	}
 }
 
+void JoltBody3D::set_product_of_inertia(const Vector3 &p_product_of_inertia) {
+	if (p_product_of_inertia != product_of_inertia) {
+		product_of_inertia = p_product_of_inertia;
+		_update_mass_properties();
+	}
+}
+
 float JoltBody3D::get_bounce() const {
 	if (!in_space()) {
 		return jolt_settings->mRestitution;

modules/jolt_physics/objects/jolt_body_3d.h

@@ -70,6 +70,7 @@ class JoltBody3D final : public JoltShapedObject3D {
 	Transform3D kinematic_transform;
 
 	Vector3 inertia;
+	Vector3 product_of_inertia;
 	Vector3 center_of_mass_custom;
 	Vector3 constant_force;
 	Vector3 constant_torque;
@@ -270,6 +271,9 @@ class JoltBody3D final : public JoltShapedObject3D {
 	Vector3 get_inertia() const { return inertia; }
 	void set_inertia(const Vector3 &p_inertia);
 
+	Vector3 get_product_of_inertia() const { return product_of_inertia; }
+	void set_product_of_inertia(const Vector3 &p_product_of_inertia);
+
 	float get_bounce() const;
 	void set_bounce(float p_bounce);