From 2e38b32e0f261445c2d0b095c1822fbe6df16e25 Mon Sep 17 00:00:00 2001 From: m4nu3lf Date: Sat, 31 Dec 2016 14:39:25 +0000 Subject: [PATCH] Fixed inertia tensor computation and center of mass --- core/math/matrix3.cpp | 74 +++++++++++++++++++ core/math/matrix3.h | 67 ++++++++++++++++- core/math/quat.h | 4 +- core/math/vector3.h | 20 +++++ core/variant_call.cpp | 5 ++ doc/base/classes.xml | 36 ++++++++- scene/3d/vehicle_body.cpp | 2 +- servers/physics/body_pair_sw.cpp | 24 +++--- servers/physics/body_pair_sw.h | 2 +- servers/physics/body_sw.cpp | 64 +++++++++++----- servers/physics/body_sw.h | 43 ++++++++--- servers/physics/collision_object_sw.cpp | 2 + servers/physics/collision_object_sw.h | 2 + .../physics/joints/cone_twist_joint_sw.cpp | 8 +- .../physics/joints/generic_6dof_joint_sw.cpp | 14 ++-- servers/physics/joints/hinge_joint_sw.cpp | 20 ++--- servers/physics/joints/pin_joint_sw.cpp | 8 +- servers/physics/joints/slider_joint_sw.cpp | 12 +-- servers/physics/physics_server_sw.cpp | 9 +++ servers/physics/physics_server_sw.h | 1 + servers/physics/shape_sw.h | 9 +++ servers/physics_server.cpp | 5 ++ servers/physics_server.h | 4 + 23 files changed, 359 insertions(+), 76 deletions(-) diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp index c30401cc242..77b260dd177 100644 --- a/core/math/matrix3.cpp +++ b/core/math/matrix3.cpp @@ -100,6 +100,80 @@ Matrix3 Matrix3::orthonormalized() const { } +bool Matrix3::is_symmetric() const { + + if (Math::abs(elements[0][1] - elements[1][0]) > CMP_EPSILON) + return false; + if (Math::abs(elements[0][2] - elements[2][0]) > CMP_EPSILON) + return false; + if (Math::abs(elements[1][2] - elements[2][1]) > CMP_EPSILON) + return false; + + return true; +} + + +Matrix3 Matrix3::diagonalize() { + + //NOTE: only implemented for symmetric matrices + //with the Jacobi iterative method method + + ERR_FAIL_COND_V(!is_symmetric(), Matrix3()); + + const int ite_max = 1024; + + real_t off_matrix_norm_2 = elements[0][1] * elements[0][1] + elements[0][2] * elements[0][2] + elements[1][2] * elements[1][2]; + + int ite = 0; + Matrix3 acc_rot; + while (off_matrix_norm_2 > CMP_EPSILON2 && ite++ < ite_max ) { + real_t el01_2 = elements[0][1] * elements[0][1]; + real_t el02_2 = elements[0][2] * elements[0][2]; + real_t el12_2 = elements[1][2] * elements[1][2]; + // Find the pivot element + int i, j; + if (el01_2 > el02_2) { + if (el12_2 > el01_2) { + i = 1; + j = 2; + } else { + i = 0; + j = 1; + } + } else { + if (el12_2 > el02_2) { + i = 1; + j = 2; + } else { + i = 0; + j = 2; + } + } + + // Compute the rotation angle + real_t angle; + if (Math::abs(elements[j][j] - elements[i][i]) < CMP_EPSILON) { + angle = Math_PI / 4; + } else { + angle = 0.5 * Math::atan(2 * elements[i][j] / (elements[j][j] - elements[i][i])); + } + + // Compute the rotation matrix + Matrix3 rot; + rot.elements[i][i] = rot.elements[j][j] = Math::cos(angle); + rot.elements[i][j] = - (rot.elements[j][i] = Math::sin(angle)); + + // Update the off matrix norm + off_matrix_norm_2 -= elements[i][j] * elements[i][j]; + + // Apply the rotation + *this = rot * *this * rot.transposed(); + acc_rot = rot * acc_rot; + } + + return acc_rot; +} + Matrix3 Matrix3::inverse() const { Matrix3 inv=*this; diff --git a/core/math/matrix3.h b/core/math/matrix3.h index 2792200b7dc..d9ad3ec2bb2 100644 --- a/core/math/matrix3.h +++ b/core/math/matrix3.h @@ -26,10 +26,12 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + +#include "vector3.h" + #ifndef MATRIX3_H #define MATRIX3_H -#include "vector3.h" #include "quat.h" /** @@ -98,6 +100,12 @@ public: _FORCE_INLINE_ Vector3 xform_inv(const Vector3& p_vector) const; _FORCE_INLINE_ void operator*=(const Matrix3& p_matrix); _FORCE_INLINE_ Matrix3 operator*(const Matrix3& p_matrix) const; + _FORCE_INLINE_ void operator+=(const Matrix3& p_matrix); + _FORCE_INLINE_ Matrix3 operator+(const Matrix3& p_matrix) const; + _FORCE_INLINE_ void operator-=(const Matrix3& p_matrix); + _FORCE_INLINE_ Matrix3 operator-(const Matrix3& p_matrix) const; + _FORCE_INLINE_ void operator*=(real_t p_val); + _FORCE_INLINE_ Matrix3 operator*(real_t p_val) const; int get_orthogonal_index() const; void set_orthogonal_index(int p_index); @@ -130,6 +138,10 @@ public: return Vector3(elements[i][0],elements[i][1],elements[i][2]); } + _FORCE_INLINE_ Vector3 get_main_diagonal() const { + return Vector3(elements[0][0],elements[1][1],elements[2][2]); + } + _FORCE_INLINE_ void set_row(int i, const Vector3& p_row) { elements[i][0]=p_row.x; elements[i][1]=p_row.y; @@ -163,12 +175,22 @@ public: void orthonormalize(); Matrix3 orthonormalized() const; + bool is_symmetric() const; + Matrix3 diagonalize(); + operator Quat() const; Matrix3(const Quat& p_quat); // euler Matrix3(const Vector3& p_euler); // euler Matrix3(const Vector3& p_axis, real_t p_phi); + _FORCE_INLINE_ Matrix3(const Vector3& row0, const Vector3& row1, const Vector3& row2) + { + elements[0]=row0; + elements[1]=row1; + elements[2]=row2; + } + _FORCE_INLINE_ Matrix3() { elements[0][0]=1; @@ -203,6 +225,49 @@ _FORCE_INLINE_ Matrix3 Matrix3::operator*(const Matrix3& p_matrix) const { } + +_FORCE_INLINE_ void Matrix3::operator+=(const Matrix3& p_matrix) { + + elements[0] += p_matrix.elements[0]; + elements[1] += p_matrix.elements[1]; + elements[2] += p_matrix.elements[2]; +} + +_FORCE_INLINE_ Matrix3 Matrix3::operator+(const Matrix3& p_matrix) const { + + Matrix3 ret(*this); + ret += p_matrix; + return ret; +} + +_FORCE_INLINE_ void Matrix3::operator-=(const Matrix3& p_matrix) { + + elements[0] -= p_matrix.elements[0]; + elements[1] -= p_matrix.elements[1]; + elements[2] -= p_matrix.elements[2]; +} + +_FORCE_INLINE_ Matrix3 Matrix3::operator-(const Matrix3& p_matrix) const { + + Matrix3 ret(*this); + ret -= p_matrix; + return ret; +} + +_FORCE_INLINE_ void Matrix3::operator*=(real_t p_val) { + + elements[0]*=p_val; + elements[1]*=p_val; + elements[2]*=p_val; +} + +_FORCE_INLINE_ Matrix3 Matrix3::operator*(real_t p_val) const { + + Matrix3 ret(*this); + ret *= p_val; + return ret; +} + Vector3 Matrix3::xform(const Vector3& p_vector) const { return Vector3( diff --git a/core/math/quat.h b/core/math/quat.h index 9f4145cddbb..6ffdf7a81b7 100644 --- a/core/math/quat.h +++ b/core/math/quat.h @@ -26,13 +26,15 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + +#include "vector3.h" + #ifndef QUAT_H #define QUAT_H #include "math_defs.h" #include "math_funcs.h" #include "ustring.h" -#include "vector3.h" /** @author Juan Linietsky diff --git a/core/math/vector3.h b/core/math/vector3.h index 3f451b0ab70..279891c3b25 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -34,6 +34,7 @@ #include "math_funcs.h" #include "ustring.h" +class Matrix3; struct Vector3 { @@ -92,6 +93,8 @@ struct Vector3 { _FORCE_INLINE_ Vector3 cross(const Vector3& p_b) const; _FORCE_INLINE_ real_t dot(const Vector3& p_b) const; + _FORCE_INLINE_ Matrix3 outer(const Vector3& p_b) const; + _FORCE_INLINE_ Matrix3 to_diagonal_matrix() const; _FORCE_INLINE_ Vector3 abs() const; _FORCE_INLINE_ Vector3 floor() const; @@ -144,6 +147,8 @@ struct Vector3 { #else +#include "matrix3.h" + Vector3 Vector3::cross(const Vector3& p_b) const { Vector3 ret ( @@ -160,6 +165,21 @@ real_t Vector3::dot(const Vector3& p_b) const { return x*p_b.x + y*p_b.y + z*p_b.z; } +Matrix3 Vector3::outer(const Vector3& p_b) const { + + Vector3 row0(x*p_b.x, x*p_b.y, x*p_b.z); + Vector3 row1(y*p_b.x, y*p_b.y, y*p_b.z); + Vector3 row2(z*p_b.x, z*p_b.y, z*p_b.z); + + return Matrix3(row0, row1, row2); +} + +Matrix3 Vector3::to_diagonal_matrix() const { + return Matrix3(x, 0, 0, + 0, y, 0, + 0, 0, z); +} + Vector3 Vector3::abs() const { return Vector3( Math::abs(x), Math::abs(y), Math::abs(z) ); diff --git a/core/variant_call.cpp b/core/variant_call.cpp index cedf21b3770..7ea6a3a8c40 100644 --- a/core/variant_call.cpp +++ b/core/variant_call.cpp @@ -370,6 +370,8 @@ static void _call_##m_type##_##m_method(Variant& r_ret,Variant& p_self,const Var VCALL_LOCALMEM4R(Vector3, cubic_interpolate); VCALL_LOCALMEM1R(Vector3, dot); VCALL_LOCALMEM1R(Vector3, cross); + VCALL_LOCALMEM1R(Vector3, outer); + VCALL_LOCALMEM0R(Vector3, to_diagonal_matrix); VCALL_LOCALMEM0R(Vector3, abs); VCALL_LOCALMEM0R(Vector3, floor); VCALL_LOCALMEM0R(Vector3, ceil); @@ -1481,6 +1483,9 @@ _VariantCall::addfunc(Variant::m_vtype,Variant::m_ret,_SCS(#m_method),VCALL(m_cl ADDFUNC4(VECTOR3,VECTOR3,Vector3,cubic_interpolate,VECTOR3,"b",VECTOR3,"pre_a",VECTOR3,"post_b",REAL,"t",varray()); ADDFUNC1(VECTOR3,REAL,Vector3,dot,VECTOR3,"b",varray()); ADDFUNC1(VECTOR3,VECTOR3,Vector3,cross,VECTOR3,"b",varray()); + ADDFUNC1(VECTOR3,MATRIX3,Vector3,outer,VECTOR3,"b",varray()); + ADDFUNC0(VECTOR3,MATRIX3,Vector3,to_diagonal_matrix,varray()); + ADDFUNC0(VECTOR3,VECTOR3,Vector3,abs,varray()); ADDFUNC0(VECTOR3,VECTOR3,Vector3,abs,varray()); ADDFUNC0(VECTOR3,VECTOR3,Vector3,floor,varray()); ADDFUNC0(VECTOR3,VECTOR3,Vector3,ceil,varray()); diff --git a/doc/base/classes.xml b/doc/base/classes.xml index b49c23f1171..869f7781c5d 100644 --- a/doc/base/classes.xml +++ b/doc/base/classes.xml @@ -28723,12 +28723,24 @@ + + + + + + + + + + + + @@ -28807,7 +28819,7 @@ - + @@ -28825,6 +28837,12 @@ + + + + + + @@ -45318,6 +45336,15 @@ do_property]. Return a copy of the normalized vector to unit length. This is the same as v / v.length(). + + + + + + + Return the outer product with b. + + @@ -45356,6 +45383,13 @@ do_property]. Return a copy of the vector, snapped to the lowest neared multiple. + + + + + Return a diagonal matrix with the vector as main diagonal. + + diff --git a/scene/3d/vehicle_body.cpp b/scene/3d/vehicle_body.cpp index 23feac0150d..8edd28d0597 100644 --- a/scene/3d/vehicle_body.cpp +++ b/scene/3d/vehicle_body.cpp @@ -590,7 +590,7 @@ void VehicleBody::_resolve_single_bilateral(PhysicsDirectBodyState *s, const Vec rel_pos1, rel_pos2, normal, - s->get_inverse_inertia(), + s->get_inverse_inertia_tensor().get_main_diagonal(), 1.0/mass, b2invinertia, b2invmass); diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index 630d8e3d50f..7defa87bb12 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -307,8 +307,8 @@ bool BodyPairSW::setup(float p_step) { } #endif - c.rA = global_A; - c.rB = global_B-offset_B; + c.rA = global_A-A->get_center_of_mass(); + c.rB = global_B-B->get_center_of_mass()-offset_B; // contact query reporting... #if 0 @@ -364,12 +364,12 @@ bool BodyPairSW::setup(float p_step) { c.depth=depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - A->apply_impulse( c.rA, -j_vec ); - B->apply_impulse( c.rB, j_vec ); + A->apply_impulse( c.rA+A->get_center_of_mass(), -j_vec ); + B->apply_impulse( c.rB+B->get_center_of_mass(), j_vec ); c.acc_bias_impulse=0; Vector3 jb_vec = c.normal * c.acc_bias_impulse; - A->apply_bias_impulse( c.rA, -jb_vec ); - B->apply_bias_impulse( c.rB, jb_vec ); + A->apply_bias_impulse( c.rA+A->get_center_of_mass(), -jb_vec ); + B->apply_bias_impulse( c.rB+B->get_center_of_mass(), jb_vec ); c.bounce = MAX(A->get_bounce(),B->get_bounce()); if (c.bounce) { @@ -418,8 +418,8 @@ void BodyPairSW::solve(float p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(c.rA,-jb); - B->apply_bias_impulse(c.rB, jb); + A->apply_bias_impulse(c.rA+A->get_center_of_mass(),-jb); + B->apply_bias_impulse(c.rB+B->get_center_of_mass(), jb); c.active=true; } @@ -442,8 +442,8 @@ void BodyPairSW::solve(float p_step) { Vector3 j =c.normal * (c.acc_normal_impulse - jnOld); - A->apply_impulse(c.rA,-j); - B->apply_impulse(c.rB, j); + A->apply_impulse(c.rA+A->get_center_of_mass(),-j); + B->apply_impulse(c.rB+B->get_center_of_mass(), j); c.active=true; } @@ -489,8 +489,8 @@ void BodyPairSW::solve(float p_step) { jt = c.acc_tangent_impulse - jtOld; - A->apply_impulse( c.rA, -jt ); - B->apply_impulse( c.rB, jt ); + A->apply_impulse( c.rA+A->get_center_of_mass(), -jt ); + B->apply_impulse( c.rB+B->get_center_of_mass(), jt ); c.active=true; diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h index 4962c78dac7..a37f75d13c5 100644 --- a/servers/physics/body_pair_sw.h +++ b/servers/physics/body_pair_sw.h @@ -65,7 +65,7 @@ class BodyPairSW : public ConstraintSW { real_t depth; bool active; - Vector3 rA,rB; + Vector3 rA,rB; // Offset in world orientation with respect to center of mass }; Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index 74bba4f977e..2f859a4ed4c 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;imass / 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; + } + + center_of_mass_local /= mass; + + // Recompute the inertia tensor + Matrix3 inertia_tensor; + inertia_tensor.set_zero(); for (int i=0;imass / 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) { diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h index 9b28468b34c..acdd84d259d 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -57,8 +57,16 @@ class BodySW : public CollisionObjectSW { PhysicsServer::BodyAxisLock axis_lock; real_t _inv_mass; - Vector3 _inv_inertia; + Vector3 _inv_inertia; // Relative to the principal axes of inertia + + // Relative to the local frame of reference + Matrix3 principal_inertia_axes_local; + Vector3 center_of_mass_local; + + // In world orientation with local origin Matrix3 _inv_inertia_tensor; + Matrix3 principal_inertia_axes; + Vector3 center_of_mass; Vector3 gravity; @@ -135,7 +143,7 @@ class BodySW : public CollisionObjectSW { _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const AreaSW *p_area); - _FORCE_INLINE_ void _update_inertia_tensor(); + _FORCE_INLINE_ void _update_transform_dependant(); friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose @@ -190,6 +198,10 @@ public: _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_ Matrix3 get_principal_inertia_axes() const { return principal_inertia_axes; } + _FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; } + _FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3& p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); } + _FORCE_INLINE_ void set_linear_velocity(const Vector3& p_velocity) {linear_velocity=p_velocity; } _FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; } @@ -202,13 +214,7 @@ public: _FORCE_INLINE_ void apply_impulse(const Vector3& p_pos, const Vector3& p_j) { linear_velocity += p_j * _inv_mass; - angular_velocity += _inv_inertia_tensor.xform( p_pos.cross(p_j) ); - } - - _FORCE_INLINE_ void apply_bias_impulse(const Vector3& p_pos, const Vector3& p_j) { - - biased_linear_velocity += p_j * _inv_mass; - biased_angular_velocity += _inv_inertia_tensor.xform( p_pos.cross(p_j) ); + angular_velocity += _inv_inertia_tensor.xform( (p_pos-center_of_mass).cross(p_j) ); } _FORCE_INLINE_ void apply_torque_impulse(const Vector3& p_j) { @@ -216,6 +222,17 @@ public: angular_velocity += _inv_inertia_tensor.xform(p_j); } + _FORCE_INLINE_ void apply_bias_impulse(const Vector3& p_pos, const Vector3& p_j) { + + biased_linear_velocity += p_j * _inv_mass; + biased_angular_velocity += _inv_inertia_tensor.xform( (p_pos-center_of_mass).cross(p_j) ); + } + + _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3& p_j) { + + biased_angular_velocity += _inv_inertia_tensor.xform(p_j); + } + _FORCE_INLINE_ void add_force(const Vector3& p_force, const Vector3& p_pos) { applied_force += p_force; @@ -268,12 +285,12 @@ public: _FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3& rel_pos) const { - return linear_velocity + angular_velocity.cross(rel_pos); + return linear_velocity + angular_velocity.cross(rel_pos-center_of_mass); } _FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3& p_pos, const Vector3& p_normal) const { - Vector3 r0 = p_pos - get_transform().origin; + Vector3 r0 = p_pos - get_transform().origin - center_of_mass; Vector3 c0 = (r0).cross(p_normal); @@ -363,6 +380,9 @@ public: virtual float get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area virtual float get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area + virtual Vector3 get_center_of_mass() const { return body->get_center_of_mass(); } + virtual Matrix3 get_principal_inertia_axes() const { return body->get_principal_inertia_axes(); } + virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass virtual Vector3 get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space virtual Matrix3 get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor(); } // get density of this body space @@ -378,6 +398,7 @@ public: virtual void add_force(const Vector3& p_force, const Vector3& p_pos) { body->add_force(p_force,p_pos); } virtual void apply_impulse(const Vector3& p_pos, const Vector3& p_j) { body->apply_impulse(p_pos,p_j); } + virtual void apply_torque_impulse(const Vector3& p_j) { body->apply_torque_impulse(p_j); } virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); } virtual bool is_sleeping() const { return !body->is_active(); } diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp index c45b76da224..2b2b73738ac 100644 --- a/servers/physics/collision_object_sw.cpp +++ b/servers/physics/collision_object_sw.cpp @@ -144,6 +144,8 @@ void CollisionObjectSW::_update_shapes() { s.aabb_cache=shape_aabb; s.aabb_cache=s.aabb_cache.grow( (s.aabb_cache.size.x + s.aabb_cache.size.y)*0.5*0.05 ); + Vector3 scale = xform.get_basis().get_scale(); + s.area_cache=s.shape->get_area()*scale.x*scale.y*scale.z; space->get_broadphase()->move(s.bpid,s.aabb_cache); } diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h index 5738a629dc9..f3e43c98dbc 100644 --- a/servers/physics/collision_object_sw.h +++ b/servers/physics/collision_object_sw.h @@ -61,6 +61,7 @@ private: Transform xform_inv; BroadPhaseSW::ID bpid; AABB aabb_cache; //for rayqueries + real_t area_cache; ShapeSW *shape; bool trigger; @@ -123,6 +124,7 @@ public: _FORCE_INLINE_ const Transform& get_shape_transform(int p_index) const { return shapes[p_index].xform; } _FORCE_INLINE_ const Transform& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } _FORCE_INLINE_ const AABB& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } + _FORCE_INLINE_ const real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; } _FORCE_INLINE_ Transform get_transform() const { return transform; } _FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; } diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp index c94cc8bd8ca..5036a1d8a37 100644 --- a/servers/physics/joints/cone_twist_joint_sw.cpp +++ b/servers/physics/joints/cone_twist_joint_sw.cpp @@ -128,10 +128,10 @@ bool ConeTwistJointSW::setup(float p_step) { for (int i=0;i<3;i++) { memnew_placement(&m_jac[i], JacobianEntrySW( - A->get_transform().basis.transposed(), - B->get_transform().basis.transposed(), - pivotAInW - A->get_transform().origin, - pivotBInW - B->get_transform().origin, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), normal[i], A->get_inv_inertia(), A->get_inv_mass(), diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp index 4e25bebe55d..3c0119971a8 100644 --- a/servers/physics/joints/generic_6dof_joint_sw.cpp +++ b/servers/physics/joints/generic_6dof_joint_sw.cpp @@ -352,10 +352,10 @@ void Generic6DOFJointSW::buildLinearJacobian( const Vector3 & pivotAInW,const Vector3 & pivotBInW) { memnew_placement(&jacLinear, JacobianEntrySW( - A->get_transform().basis.transposed(), - B->get_transform().basis.transposed(), - pivotAInW - A->get_transform().origin, - pivotBInW - B->get_transform().origin, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), normalWorld, A->get_inv_inertia(), A->get_inv_mass(), @@ -368,8 +368,8 @@ void Generic6DOFJointSW::buildAngularJacobian( JacobianEntrySW & jacAngular,const Vector3 & jointAxisW) { memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW, - A->get_transform().basis.transposed(), - B->get_transform().basis.transposed(), + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), A->get_inv_inertia(), B->get_inv_inertia())); @@ -440,7 +440,7 @@ bool Generic6DOFJointSW::setup(float p_step) { } -void Generic6DOFJointSW::solve(real_t timeStep) +void Generic6DOFJointSW::solve(real_t timeStep) { m_timeStep = timeStep; diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp index f7e1df92b81..23e00a9e7fd 100644 --- a/servers/physics/joints/hinge_joint_sw.cpp +++ b/servers/physics/joints/hinge_joint_sw.cpp @@ -173,10 +173,10 @@ bool HingeJointSW::setup(float p_step) { for (int i=0;i<3;i++) { memnew_placement(&m_jac[i], JacobianEntrySW( - A->get_transform().basis.transposed(), - B->get_transform().basis.transposed(), - pivotAInW - A->get_transform().origin, - pivotBInW - B->get_transform().origin, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), normal[i], A->get_inv_inertia(), A->get_inv_mass(), @@ -200,20 +200,20 @@ bool HingeJointSW::setup(float p_step) { Vector3 hingeAxisWorld = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) ); memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0, - A->get_transform().basis.transposed(), - B->get_transform().basis.transposed(), + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), A->get_inv_inertia(), B->get_inv_inertia())); memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1, - A->get_transform().basis.transposed(), - B->get_transform().basis.transposed(), + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), A->get_inv_inertia(), B->get_inv_inertia())); memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld, - A->get_transform().basis.transposed(), - B->get_transform().basis.transposed(), + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), A->get_inv_inertia(), B->get_inv_inertia())); diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp index 74d62e35fa2..292d30443c7 100644 --- a/servers/physics/joints/pin_joint_sw.cpp +++ b/servers/physics/joints/pin_joint_sw.cpp @@ -44,10 +44,10 @@ bool PinJointSW::setup(float p_step) { { normal[i] = 1; memnew_placement(&m_jac[i],JacobianEntrySW( - A->get_transform().basis.transposed(), - B->get_transform().basis.transposed(), - A->get_transform().xform(m_pivotInA) - A->get_transform().origin, - B->get_transform().xform(m_pivotInB) - B->get_transform().origin, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), + B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(), normal, A->get_inv_inertia(), A->get_inv_mass(), diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp index 7d1933e0df4..bdcae08ca4f 100644 --- a/servers/physics/joints/slider_joint_sw.cpp +++ b/servers/physics/joints/slider_joint_sw.cpp @@ -132,10 +132,10 @@ bool SliderJointSW::setup(float p_step) { normalWorld = m_calculatedTransformA.basis.get_axis(i); memnew_placement(&m_jacLin[i], JacobianEntrySW( - A->get_transform().basis.transposed(), - B->get_transform().basis.transposed(), - m_relPosA, - m_relPosB, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + m_relPosA - A->get_center_of_mass(), + m_relPosB - B->get_center_of_mass(), normalWorld, A->get_inv_inertia(), A->get_inv_mass(), @@ -152,8 +152,8 @@ bool SliderJointSW::setup(float p_step) normalWorld = m_calculatedTransformA.basis.get_axis(i); memnew_placement(&m_jacAng[i], JacobianEntrySW( normalWorld, - A->get_transform().basis.transposed(), - B->get_transform().basis.transposed(), + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), A->get_inv_inertia(), B->get_inv_inertia() )); diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index ab54497eac9..4069ccdccb5 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -805,6 +805,15 @@ void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3& p_pos, const body->wakeup(); }; +void PhysicsServerSW::body_apply_torque_impulse(RID p_body, const Vector3& p_impulse) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->apply_torque_impulse(p_impulse); + body->wakeup(); +}; + void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity) { BodySW *body = body_owner.get(p_body); diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index bd9b5e4e302..a3f98392fc6 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -193,6 +193,7 @@ public: virtual Vector3 body_get_applied_torque(RID p_body) const; virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse); + virtual void body_apply_torque_impulse(RID p_body, const Vector3& p_impulse); virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity); virtual void body_set_axis_lock(RID p_body,BodyAxisLock p_lock); diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h index d031a9fb9b6..e76237017d4 100644 --- a/servers/physics/shape_sw.h +++ b/servers/physics/shape_sw.h @@ -73,6 +73,8 @@ public: MAX_SUPPORTS=8 }; + virtual real_t get_area() const { return aabb.get_area();} + _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } _FORCE_INLINE_ RID get_self() const {return self; } @@ -128,6 +130,7 @@ public: Plane get_plane() const; + virtual real_t get_area() const { return INFINITY; } virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; } virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3& p_normal) const; @@ -152,6 +155,7 @@ public: float get_length() const; + virtual real_t get_area() const { return 0.0; } virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; } virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3& p_normal) const; @@ -176,6 +180,8 @@ public: real_t get_radius() const; + virtual real_t get_area() const { return 4.0/3.0 * Math_PI * radius * radius * radius; } + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; } virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; @@ -198,6 +204,7 @@ class BoxShapeSW : public ShapeSW { public: _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; } + virtual real_t get_area() const { return 8 * half_extents.x * half_extents.y * half_extents.z; } virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; } @@ -226,6 +233,8 @@ public: _FORCE_INLINE_ real_t get_height() const { return height; } _FORCE_INLINE_ real_t get_radius() const { return radius; } + virtual real_t get_area() { return 4.0/3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; } virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp index 32c8da66ffe..bbbbfdf3ab9 100644 --- a/servers/physics_server.cpp +++ b/servers/physics_server.cpp @@ -78,6 +78,9 @@ void PhysicsDirectBodyState::_bind_methods() { ClassDB::bind_method(_MD("get_total_linear_damp"),&PhysicsDirectBodyState::get_total_linear_damp); ClassDB::bind_method(_MD("get_total_angular_damp"),&PhysicsDirectBodyState::get_total_angular_damp); + ClassDB::bind_method(_MD("get_center_of_mass"),&PhysicsDirectBodyState::get_center_of_mass); + ClassDB::bind_method(_MD("get_principal_inetria_axes"),&PhysicsDirectBodyState::get_principal_inertia_axes); + ClassDB::bind_method(_MD("get_inverse_mass"),&PhysicsDirectBodyState::get_inverse_mass); ClassDB::bind_method(_MD("get_inverse_inertia"),&PhysicsDirectBodyState::get_inverse_inertia); @@ -92,6 +95,7 @@ void PhysicsDirectBodyState::_bind_methods() { ClassDB::bind_method(_MD("add_force","force","pos"),&PhysicsDirectBodyState::add_force); ClassDB::bind_method(_MD("apply_impulse","pos","j"),&PhysicsDirectBodyState::apply_impulse); + ClassDB::bind_method(_MD("apply_torqe_impulse","j"),&PhysicsDirectBodyState::apply_torque_impulse); ClassDB::bind_method(_MD("set_sleep_state","enabled"),&PhysicsDirectBodyState::set_sleep_state); ClassDB::bind_method(_MD("is_sleeping"),&PhysicsDirectBodyState::is_sleeping); @@ -517,6 +521,7 @@ void PhysicsServer::_bind_methods() { ClassDB::bind_method(_MD("body_get_state","body","state"),&PhysicsServer::body_get_state); ClassDB::bind_method(_MD("body_apply_impulse","body","pos","impulse"),&PhysicsServer::body_apply_impulse); + ClassDB::bind_method(_MD("body_apply_torque_impulse","body","impulse"),&PhysicsServer::body_apply_torque_impulse); ClassDB::bind_method(_MD("body_set_axis_velocity","body","axis_velocity"),&PhysicsServer::body_set_axis_velocity); ClassDB::bind_method(_MD("body_set_axis_lock","body","axis"),&PhysicsServer::body_set_axis_lock); diff --git a/servers/physics_server.h b/servers/physics_server.h index 26b28a20762..3070f919b0d 100644 --- a/servers/physics_server.h +++ b/servers/physics_server.h @@ -45,6 +45,8 @@ public: virtual float get_total_angular_damp() const=0; virtual float get_total_linear_damp() const=0; + virtual Vector3 get_center_of_mass() const=0; + virtual Matrix3 get_principal_inertia_axes() const=0; virtual float get_inverse_mass() const=0; // get the mass virtual Vector3 get_inverse_inertia() const=0; // get density of this body space virtual Matrix3 get_inverse_inertia_tensor() const=0; // get density of this body space @@ -60,6 +62,7 @@ public: virtual void add_force(const Vector3& p_force, const Vector3& p_pos)=0; virtual void apply_impulse(const Vector3& p_pos, const Vector3& p_j)=0; + virtual void apply_torque_impulse(const Vector3& p_j)=0; virtual void set_sleep_state(bool p_enable)=0; virtual bool is_sleeping() const=0; @@ -441,6 +444,7 @@ public: virtual Vector3 body_get_applied_torque(RID p_body) const=0; virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse)=0; + virtual void body_apply_torque_impulse(RID p_body, const Vector3& p_impulse)=0; virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity)=0; enum BodyAxisLock {