Proper support for custom mass properties in 2D/3D physics bodies
Changes: -Added support for custom inertia and center of mass in 3D -Added support for custom center of mass in 2D -Calculated center of mass from shapes in 2D (same as in 3D) -Fixed mass properties calculation with disabled shapes in 2D/3D -Removed first_integration which is not used in 2D and doesn't seem to make a lot of sense (prevents omit_force_integration to work during the first frame) -Support for custom inertia on different axes for RigidBody3D
This commit is contained in:
PouleyKetchoupp
parent
e1ae2708ee
commit
82ea2a7045
31 changed files with 665 additions and 319 deletions
|
|
@ -34,47 +34,72 @@
|
|||
#include "body_direct_state_2d_sw.h"
|
||||
#include "space_2d_sw.h"
|
||||
|
||||
void Body2DSW::_update_inertia() {
|
||||
if (!user_inertia && get_space() && !inertia_update_list.in_list()) {
|
||||
get_space()->body_add_to_inertia_update_list(&inertia_update_list);
|
||||
void Body2DSW::_mass_properties_changed() {
|
||||
if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) {
|
||||
get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list);
|
||||
}
|
||||
}
|
||||
|
||||
void Body2DSW::update_inertias() {
|
||||
void Body2DSW::update_mass_properties() {
|
||||
//update shapes and motions
|
||||
|
||||
switch (mode) {
|
||||
case PhysicsServer2D::BODY_MODE_DYNAMIC: {
|
||||
if (user_inertia) {
|
||||
_inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
|
||||
break;
|
||||
}
|
||||
//update tensor for allshapes, 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_aabb(i).get_area();
|
||||
}
|
||||
|
||||
inertia = 0;
|
||||
|
||||
for (int i = 0; i < get_shape_count(); i++) {
|
||||
if (is_shape_disabled(i)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const Shape2DSW *shape = get_shape(i);
|
||||
|
||||
real_t area = get_shape_aabb(i).get_area();
|
||||
|
||||
real_t mass = area * this->mass / total_area;
|
||||
|
||||
Transform2D mtx = get_shape_transform(i);
|
||||
Vector2 scale = mtx.get_scale();
|
||||
inertia += shape->get_moment_of_inertia(mass, scale) + mass * mtx.get_origin().length_squared();
|
||||
total_area += get_shape_aabb(i).get_area();
|
||||
}
|
||||
|
||||
_inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
|
||||
if (calculate_center_of_mass) {
|
||||
// We have to recompute the center of mass.
|
||||
center_of_mass = Vector2();
|
||||
|
||||
if (total_area != 0.0) {
|
||||
for (int i = 0; i < get_shape_count(); i++) {
|
||||
if (is_shape_disabled(i)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
real_t area = get_shape_aabb(i).get_area();
|
||||
|
||||
real_t mass = area * this->mass / total_area;
|
||||
|
||||
// NOTE: we assume that the shape origin is also its center of mass.
|
||||
center_of_mass += mass * get_shape_transform(i).get_origin();
|
||||
}
|
||||
|
||||
center_of_mass /= mass;
|
||||
}
|
||||
}
|
||||
|
||||
if (calculate_inertia) {
|
||||
inertia = 0;
|
||||
|
||||
for (int i = 0; i < get_shape_count(); i++) {
|
||||
if (is_shape_disabled(i)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const Shape2DSW *shape = get_shape(i);
|
||||
|
||||
real_t area = get_shape_aabb(i).get_area();
|
||||
if (area == 0.0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
real_t mass = area * this->mass / total_area;
|
||||
|
||||
Transform2D mtx = get_shape_transform(i);
|
||||
Vector2 scale = mtx.get_scale();
|
||||
Vector2 shape_origin = mtx.get_origin() - center_of_mass;
|
||||
inertia += shape->get_moment_of_inertia(mass, scale) + mass * shape_origin.length_squared();
|
||||
}
|
||||
}
|
||||
|
||||
_inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0;
|
||||
|
||||
if (mass) {
|
||||
_inv_mass = 1.0 / mass;
|
||||
|
|
@ -94,9 +119,12 @@ void Body2DSW::update_inertias() {
|
|||
|
||||
} break;
|
||||
}
|
||||
//_update_inertia_tensor();
|
||||
}
|
||||
|
||||
//_update_shapes();
|
||||
void Body2DSW::reset_mass_properties() {
|
||||
calculate_inertia = true;
|
||||
calculate_center_of_mass = true;
|
||||
_mass_properties_changed();
|
||||
}
|
||||
|
||||
void Body2DSW::set_active(bool p_active) {
|
||||
|
|
@ -118,7 +146,7 @@ void Body2DSW::set_active(bool p_active) {
|
|||
}
|
||||
}
|
||||
|
||||
void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) {
|
||||
void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) {
|
||||
switch (p_param) {
|
||||
case PhysicsServer2D::BODY_PARAM_BOUNCE: {
|
||||
bounce = p_value;
|
||||
|
|
@ -127,21 +155,32 @@ void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value)
|
|||
friction = p_value;
|
||||
} break;
|
||||
case PhysicsServer2D::BODY_PARAM_MASS: {
|
||||
ERR_FAIL_COND(p_value <= 0);
|
||||
mass = p_value;
|
||||
_update_inertia();
|
||||
|
||||
real_t mass_value = p_value;
|
||||
ERR_FAIL_COND(mass_value <= 0);
|
||||
mass = mass_value;
|
||||
if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC) {
|
||||
_mass_properties_changed();
|
||||
}
|
||||
} break;
|
||||
case PhysicsServer2D::BODY_PARAM_INERTIA: {
|
||||
if (p_value <= 0) {
|
||||
user_inertia = false;
|
||||
_update_inertia();
|
||||
real_t inertia_value = p_value;
|
||||
if (inertia_value <= 0.0) {
|
||||
calculate_inertia = true;
|
||||
if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) {
|
||||
_mass_properties_changed();
|
||||
}
|
||||
} else {
|
||||
user_inertia = true;
|
||||
inertia = p_value;
|
||||
_inv_inertia = 1.0 / p_value;
|
||||
calculate_inertia = false;
|
||||
inertia = inertia_value;
|
||||
if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) {
|
||||
_inv_inertia = 1.0 / inertia;
|
||||
}
|
||||
}
|
||||
} break;
|
||||
case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
|
||||
calculate_center_of_mass = false;
|
||||
center_of_mass = p_value;
|
||||
} break;
|
||||
case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
|
||||
gravity_scale = p_value;
|
||||
} break;
|
||||
|
|
@ -156,7 +195,7 @@ void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value)
|
|||
}
|
||||
}
|
||||
|
||||
real_t Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const {
|
||||
Variant Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const {
|
||||
switch (p_param) {
|
||||
case PhysicsServer2D::BODY_PARAM_BOUNCE: {
|
||||
return bounce;
|
||||
|
|
@ -170,6 +209,9 @@ real_t Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const {
|
|||
case PhysicsServer2D::BODY_PARAM_INERTIA: {
|
||||
return inertia;
|
||||
}
|
||||
case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
|
||||
return center_of_mass;
|
||||
}
|
||||
case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
|
||||
return gravity_scale;
|
||||
}
|
||||
|
|
@ -207,7 +249,10 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) {
|
|||
} break;
|
||||
case PhysicsServer2D::BODY_MODE_DYNAMIC: {
|
||||
_inv_mass = mass > 0 ? (1.0 / mass) : 0;
|
||||
_inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
|
||||
if (!calculate_inertia) {
|
||||
_inv_inertia = 1.0 / inertia;
|
||||
}
|
||||
_mass_properties_changed();
|
||||
_set_static(false);
|
||||
set_active(true);
|
||||
|
||||
|
|
@ -215,18 +260,11 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) {
|
|||
case PhysicsServer2D::BODY_MODE_DYNAMIC_LOCKED: {
|
||||
_inv_mass = mass > 0 ? (1.0 / mass) : 0;
|
||||
_inv_inertia = 0;
|
||||
angular_velocity = 0;
|
||||
_set_static(false);
|
||||
set_active(true);
|
||||
angular_velocity = 0;
|
||||
} break;
|
||||
}
|
||||
}
|
||||
if (p_mode == PhysicsServer2D::BODY_MODE_DYNAMIC && _inv_inertia == 0) {
|
||||
_update_inertia();
|
||||
}
|
||||
/*
|
||||
if (get_space())
|
||||
_update_queries();
|
||||
*/
|
||||
}
|
||||
|
||||
PhysicsServer2D::BodyMode Body2DSW::get_mode() const {
|
||||
|
|
@ -234,7 +272,7 @@ PhysicsServer2D::BodyMode Body2DSW::get_mode() const {
|
|||
}
|
||||
|
||||
void Body2DSW::_shapes_changed() {
|
||||
_update_inertia();
|
||||
_mass_properties_changed();
|
||||
wakeup_neighbours();
|
||||
}
|
||||
|
||||
|
|
@ -298,7 +336,7 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va
|
|||
} break;
|
||||
case PhysicsServer2D::BODY_STATE_CAN_SLEEP: {
|
||||
can_sleep = p_variant;
|
||||
if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) {
|
||||
if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) {
|
||||
set_active(true);
|
||||
}
|
||||
|
||||
|
|
@ -332,8 +370,8 @@ void Body2DSW::set_space(Space2DSW *p_space) {
|
|||
if (get_space()) {
|
||||
wakeup_neighbours();
|
||||
|
||||
if (inertia_update_list.in_list()) {
|
||||
get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
|
||||
if (mass_properties_update_list.in_list()) {
|
||||
get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list);
|
||||
}
|
||||
if (active_list.in_list()) {
|
||||
get_space()->body_remove_from_active_list(&active_list);
|
||||
|
|
@ -346,13 +384,11 @@ void Body2DSW::set_space(Space2DSW *p_space) {
|
|||
_set_space(p_space);
|
||||
|
||||
if (get_space()) {
|
||||
_update_inertia();
|
||||
_mass_properties_changed();
|
||||
if (active) {
|
||||
get_space()->body_add_to_active_list(&active_list);
|
||||
}
|
||||
}
|
||||
|
||||
first_integration = false;
|
||||
}
|
||||
|
||||
void Body2DSW::_compute_area_gravity_and_damping(const Area2DSW *p_area) {
|
||||
|
|
@ -446,7 +482,7 @@ void Body2DSW::integrate_forces(real_t p_step) {
|
|||
*/
|
||||
|
||||
} else {
|
||||
if (!omit_force_integration && !first_integration) {
|
||||
if (!omit_force_integration) {
|
||||
//overridden by direct state query
|
||||
|
||||
Vector2 force = gravity * mass;
|
||||
|
|
@ -480,7 +516,6 @@ void Body2DSW::integrate_forces(real_t p_step) {
|
|||
|
||||
//motion=linear_velocity*p_step;
|
||||
|
||||
first_integration = false;
|
||||
biased_angular_velocity = 0;
|
||||
biased_linear_velocity = Vector2();
|
||||
|
||||
|
|
@ -517,14 +552,22 @@ void Body2DSW::integrate_velocities(real_t p_step) {
|
|||
real_t angle = get_transform().get_rotation() + total_angular_velocity * p_step;
|
||||
Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
|
||||
|
||||
real_t center_of_mass_distance = center_of_mass.length();
|
||||
if (center_of_mass_distance > CMP_EPSILON) {
|
||||
// Calculate displacement due to center of mass offset.
|
||||
real_t prev_angle = get_transform().get_rotation();
|
||||
real_t angle_base = Math::atan2(center_of_mass.y, center_of_mass.x);
|
||||
Vector2 point1(Math::cos(angle_base + prev_angle), Math::sin(angle_base + prev_angle));
|
||||
Vector2 point2(Math::cos(angle_base + angle), Math::sin(angle_base + angle));
|
||||
pos += center_of_mass_distance * (point1 - point2);
|
||||
}
|
||||
|
||||
_set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED);
|
||||
_set_inv_transform(get_transform().inverse());
|
||||
|
||||
if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) {
|
||||
new_transform = get_transform();
|
||||
}
|
||||
|
||||
//_update_inertia_tensor();
|
||||
}
|
||||
|
||||
void Body2DSW::wakeup_neighbours() {
|
||||
|
|
@ -538,7 +581,7 @@ void Body2DSW::wakeup_neighbours() {
|
|||
continue;
|
||||
}
|
||||
Body2DSW *b = n[i];
|
||||
if (b->mode != PhysicsServer2D::BODY_MODE_DYNAMIC) {
|
||||
if (b->mode < PhysicsServer2D::BODY_MODE_DYNAMIC) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -619,33 +662,9 @@ PhysicsDirectBodyState2DSW *Body2DSW::get_direct_state() {
|
|||
Body2DSW::Body2DSW() :
|
||||
CollisionObject2DSW(TYPE_BODY),
|
||||
active_list(this),
|
||||
inertia_update_list(this),
|
||||
mass_properties_update_list(this),
|
||||
direct_state_query_list(this) {
|
||||
mode = PhysicsServer2D::BODY_MODE_DYNAMIC;
|
||||
active = true;
|
||||
mass = 1;
|
||||
inertia = 0;
|
||||
user_inertia = false;
|
||||
_inv_inertia = 0;
|
||||
_inv_mass = 1;
|
||||
bounce = 0;
|
||||
friction = 1;
|
||||
omit_force_integration = false;
|
||||
applied_torque = 0;
|
||||
island_step = 0;
|
||||
_set_static(false);
|
||||
first_time_kinematic = false;
|
||||
linear_damp = -1;
|
||||
angular_damp = -1;
|
||||
area_angular_damp = 0;
|
||||
area_linear_damp = 0;
|
||||
contact_count = 0;
|
||||
gravity_scale = 1.0;
|
||||
first_integration = false;
|
||||
|
||||
still_time = 0;
|
||||
continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED;
|
||||
can_sleep = true;
|
||||
}
|
||||
|
||||
Body2DSW::~Body2DSW() {
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ class Constraint2DSW;
|
|||
class PhysicsDirectBodyState2DSW;
|
||||
|
||||
class Body2DSW : public CollisionObject2DSW {
|
||||
PhysicsServer2D::BodyMode mode;
|
||||
PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_DYNAMIC;
|
||||
|
||||
Vector2 biased_linear_velocity;
|
||||
real_t biased_angular_velocity = 0.0;
|
||||
|
|
@ -52,40 +52,44 @@ class Body2DSW : public CollisionObject2DSW {
|
|||
Vector2 constant_linear_velocity;
|
||||
real_t constant_angular_velocity = 0.0;
|
||||
|
||||
real_t linear_damp;
|
||||
real_t angular_damp;
|
||||
real_t gravity_scale;
|
||||
real_t linear_damp = -1.0;
|
||||
real_t angular_damp = -1.0;
|
||||
real_t gravity_scale = 1.0;
|
||||
|
||||
real_t mass;
|
||||
real_t inertia;
|
||||
real_t bounce;
|
||||
real_t friction;
|
||||
real_t bounce = 0.0;
|
||||
real_t friction = 1.0;
|
||||
|
||||
real_t _inv_mass;
|
||||
real_t _inv_inertia;
|
||||
bool user_inertia;
|
||||
real_t mass = 1.0;
|
||||
real_t _inv_mass = 1.0;
|
||||
|
||||
real_t inertia = 0.0;
|
||||
real_t _inv_inertia = 0.0;
|
||||
|
||||
Vector2 center_of_mass;
|
||||
|
||||
bool calculate_inertia = true;
|
||||
bool calculate_center_of_mass = true;
|
||||
|
||||
Vector2 gravity;
|
||||
real_t area_linear_damp;
|
||||
real_t area_angular_damp;
|
||||
real_t area_linear_damp = 0.0;
|
||||
real_t area_angular_damp = 0.0;
|
||||
|
||||
real_t still_time;
|
||||
real_t still_time = 0.0;
|
||||
|
||||
Vector2 applied_force;
|
||||
real_t applied_torque;
|
||||
real_t applied_torque = 0.0;
|
||||
|
||||
SelfList<Body2DSW> active_list;
|
||||
SelfList<Body2DSW> inertia_update_list;
|
||||
SelfList<Body2DSW> mass_properties_update_list;
|
||||
SelfList<Body2DSW> direct_state_query_list;
|
||||
|
||||
VSet<RID> exceptions;
|
||||
PhysicsServer2D::CCDMode continuous_cd_mode;
|
||||
bool omit_force_integration;
|
||||
bool active;
|
||||
bool can_sleep;
|
||||
bool first_time_kinematic;
|
||||
bool first_integration;
|
||||
void _update_inertia();
|
||||
PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED;
|
||||
bool omit_force_integration = false;
|
||||
bool active = true;
|
||||
bool can_sleep = true;
|
||||
bool first_time_kinematic = false;
|
||||
void _mass_properties_changed();
|
||||
virtual void _shapes_changed();
|
||||
Transform2D new_transform;
|
||||
|
||||
|
|
@ -118,7 +122,7 @@ class Body2DSW : public CollisionObject2DSW {
|
|||
};
|
||||
|
||||
Vector<Contact> contacts; //no contacts by default
|
||||
int contact_count;
|
||||
int contact_count = 0;
|
||||
|
||||
void *body_state_callback_instance = nullptr;
|
||||
PhysicsServer2D::BodyStateCallback body_state_callback = nullptr;
|
||||
|
|
@ -132,7 +136,7 @@ class Body2DSW : public CollisionObject2DSW {
|
|||
|
||||
PhysicsDirectBodyState2DSW *direct_state = nullptr;
|
||||
|
||||
uint64_t island_step;
|
||||
uint64_t island_step = 0;
|
||||
|
||||
_FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area2DSW *p_area);
|
||||
|
||||
|
|
@ -210,7 +214,7 @@ public:
|
|||
|
||||
_FORCE_INLINE_ void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) {
|
||||
linear_velocity += p_impulse * _inv_mass;
|
||||
angular_velocity += _inv_inertia * p_position.cross(p_impulse);
|
||||
angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse);
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) {
|
||||
|
|
@ -219,7 +223,7 @@ public:
|
|||
|
||||
_FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) {
|
||||
biased_linear_velocity += p_impulse * _inv_mass;
|
||||
biased_angular_velocity += _inv_inertia * p_position.cross(p_impulse);
|
||||
biased_angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse);
|
||||
}
|
||||
|
||||
void set_active(bool p_active);
|
||||
|
|
@ -232,8 +236,8 @@ public:
|
|||
set_active(true);
|
||||
}
|
||||
|
||||
void set_param(PhysicsServer2D::BodyParameter p_param, real_t);
|
||||
real_t get_param(PhysicsServer2D::BodyParameter p_param) const;
|
||||
void set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value);
|
||||
Variant get_param(PhysicsServer2D::BodyParameter p_param) const;
|
||||
|
||||
void set_mode(PhysicsServer2D::BodyMode p_mode);
|
||||
PhysicsServer2D::BodyMode get_mode() const;
|
||||
|
|
@ -253,7 +257,7 @@ public:
|
|||
|
||||
_FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) {
|
||||
applied_force += p_force;
|
||||
applied_torque += p_position.cross(p_force);
|
||||
applied_torque += (p_position - center_of_mass).cross(p_force);
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ void add_torque(real_t p_torque) {
|
||||
|
|
@ -265,8 +269,10 @@ public:
|
|||
|
||||
void set_space(Space2DSW *p_space);
|
||||
|
||||
void update_inertias();
|
||||
void update_mass_properties();
|
||||
void reset_mass_properties();
|
||||
|
||||
_FORCE_INLINE_ Vector2 get_center_of_mass() const { return center_of_mass; }
|
||||
_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
|
||||
_FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }
|
||||
_FORCE_INLINE_ real_t get_friction() const { return friction; }
|
||||
|
|
|
|||
|
|
@ -46,6 +46,10 @@ real_t PhysicsDirectBodyState2DSW::get_total_linear_damp() const {
|
|||
return body->area_linear_damp;
|
||||
}
|
||||
|
||||
Vector2 PhysicsDirectBodyState2DSW::get_center_of_mass() const {
|
||||
return body->get_center_of_mass();
|
||||
}
|
||||
|
||||
real_t PhysicsDirectBodyState2DSW::get_inverse_mass() const {
|
||||
return body->get_inv_mass();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -45,6 +45,7 @@ public:
|
|||
virtual real_t get_total_angular_damp() const override;
|
||||
virtual real_t get_total_linear_damp() const override;
|
||||
|
||||
virtual Vector2 get_center_of_mass() const override;
|
||||
virtual real_t get_inverse_mass() const override;
|
||||
virtual real_t get_inverse_inertia() const override;
|
||||
|
||||
|
|
|
|||
|
|
@ -68,13 +68,13 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const
|
|||
|
||||
{
|
||||
value += a->get_inv_mass();
|
||||
real_t rcn = rA.cross(n);
|
||||
real_t rcn = (rA - a->get_center_of_mass()).cross(n);
|
||||
value += a->get_inv_inertia() * rcn * rcn;
|
||||
}
|
||||
|
||||
if (b) {
|
||||
value += b->get_inv_mass();
|
||||
real_t rcn = rB.cross(n);
|
||||
real_t rcn = (rB - b->get_center_of_mass()).cross(n);
|
||||
value += b->get_inv_inertia() * rcn * rcn;
|
||||
}
|
||||
|
||||
|
|
@ -83,9 +83,9 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const
|
|||
|
||||
static inline Vector2
|
||||
relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) {
|
||||
Vector2 sum = a->get_linear_velocity() - rA.orthogonal() * a->get_angular_velocity();
|
||||
Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity();
|
||||
if (b) {
|
||||
return (b->get_linear_velocity() - rB.orthogonal() * b->get_angular_velocity()) - sum;
|
||||
return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum;
|
||||
} else {
|
||||
return -sum;
|
||||
}
|
||||
|
|
@ -172,11 +172,11 @@ bool PinJoint2DSW::pre_solve(real_t p_step) {
|
|||
|
||||
void PinJoint2DSW::solve(real_t p_step) {
|
||||
// compute relative velocity
|
||||
Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity());
|
||||
Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity());
|
||||
|
||||
Vector2 rel_vel;
|
||||
if (B) {
|
||||
rel_vel = B->get_linear_velocity() - custom_cross(rB, B->get_angular_velocity()) - vA;
|
||||
rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA;
|
||||
} else {
|
||||
rel_vel = -vA;
|
||||
}
|
||||
|
|
@ -238,6 +238,9 @@ k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2
|
|||
k21 = 0.0f;
|
||||
k22 = m_sum;
|
||||
|
||||
r1 -= a->get_center_of_mass();
|
||||
r2 -= b->get_center_of_mass();
|
||||
|
||||
// add the influence from r1
|
||||
real_t a_i_inv = a->get_inv_inertia();
|
||||
real_t r1xsq = r1.x * r1.x * a_i_inv;
|
||||
|
|
|
|||
|
|
@ -743,20 +743,27 @@ uint32_t PhysicsServer2DSW::body_get_collision_mask(RID p_body) const {
|
|||
return body->get_collision_mask();
|
||||
};
|
||||
|
||||
void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) {
|
||||
void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
|
||||
Body2DSW *body = body_owner.getornull(p_body);
|
||||
ERR_FAIL_COND(!body);
|
||||
|
||||
body->set_param(p_param, p_value);
|
||||
};
|
||||
|
||||
real_t PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const {
|
||||
Variant PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const {
|
||||
Body2DSW *body = body_owner.getornull(p_body);
|
||||
ERR_FAIL_COND_V(!body, 0);
|
||||
|
||||
return body->get_param(p_param);
|
||||
};
|
||||
|
||||
void PhysicsServer2DSW::body_reset_mass_properties(RID p_body) {
|
||||
Body2DSW *body = body_owner.getornull(p_body);
|
||||
ERR_FAIL_COND(!body);
|
||||
|
||||
return body->reset_mass_properties();
|
||||
}
|
||||
|
||||
void PhysicsServer2DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
|
||||
Body2DSW *body = body_owner.getornull(p_body);
|
||||
ERR_FAIL_COND(!body);
|
||||
|
|
|
|||
|
|
@ -205,8 +205,10 @@ public:
|
|||
virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override;
|
||||
virtual uint32_t body_get_collision_mask(RID p_body) const override;
|
||||
|
||||
virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) override;
|
||||
virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override;
|
||||
virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override;
|
||||
virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override;
|
||||
|
||||
virtual void body_reset_mass_properties(RID p_body) override;
|
||||
|
||||
virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
|
||||
virtual Variant body_get_state(RID p_body, BodyState p_state) const override;
|
||||
|
|
|
|||
|
|
@ -212,8 +212,10 @@ public:
|
|||
FUNC2(body_set_collision_mask, RID, uint32_t);
|
||||
FUNC1RC(uint32_t, body_get_collision_mask, RID);
|
||||
|
||||
FUNC3(body_set_param, RID, BodyParameter, real_t);
|
||||
FUNC2RC(real_t, body_get_param, RID, BodyParameter);
|
||||
FUNC3(body_set_param, RID, BodyParameter, const Variant &);
|
||||
FUNC2RC(Variant, body_get_param, RID, BodyParameter);
|
||||
|
||||
FUNC1(body_reset_mass_properties, RID);
|
||||
|
||||
FUNC3(body_set_state, RID, BodyState, const Variant &);
|
||||
FUNC2RC(Variant, body_get_state, RID, BodyState);
|
||||
|
|
|
|||
|
|
@ -482,7 +482,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
|
|||
r_info->metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
|
||||
if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) {
|
||||
const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
|
||||
Vector2 rel_vec = r_info->point - body->get_transform().get_origin();
|
||||
Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass());
|
||||
r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
|
||||
|
||||
} else {
|
||||
|
|
@ -961,7 +961,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
|
|||
r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
|
||||
|
||||
const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
|
||||
Vector2 rel_vec = r_result->collision_point - body->get_transform().get_origin();
|
||||
Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass());
|
||||
r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
|
||||
|
||||
r_result->travel = safe * p_motion;
|
||||
|
|
@ -1041,12 +1041,12 @@ void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW> *p_body) {
|
|||
active_list.remove(p_body);
|
||||
}
|
||||
|
||||
void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW> *p_body) {
|
||||
inertia_update_list.add(p_body);
|
||||
void Space2DSW::body_add_to_mass_properties_update_list(SelfList<Body2DSW> *p_body) {
|
||||
mass_properties_update_list.add(p_body);
|
||||
}
|
||||
|
||||
void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body) {
|
||||
inertia_update_list.remove(p_body);
|
||||
void Space2DSW::body_remove_from_mass_properties_update_list(SelfList<Body2DSW> *p_body) {
|
||||
mass_properties_update_list.remove(p_body);
|
||||
}
|
||||
|
||||
BroadPhase2DSW *Space2DSW::get_broadphase() {
|
||||
|
|
@ -1112,9 +1112,9 @@ void Space2DSW::call_queries() {
|
|||
void Space2DSW::setup() {
|
||||
contact_debug_count = 0;
|
||||
|
||||
while (inertia_update_list.first()) {
|
||||
inertia_update_list.first()->self()->update_inertias();
|
||||
inertia_update_list.remove(inertia_update_list.first());
|
||||
while (mass_properties_update_list.first()) {
|
||||
mass_properties_update_list.first()->self()->update_mass_properties();
|
||||
mass_properties_update_list.remove(mass_properties_update_list.first());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -86,7 +86,7 @@ private:
|
|||
|
||||
BroadPhase2DSW *broadphase;
|
||||
SelfList<Body2DSW>::List active_list;
|
||||
SelfList<Body2DSW>::List inertia_update_list;
|
||||
SelfList<Body2DSW>::List mass_properties_update_list;
|
||||
SelfList<Body2DSW>::List state_query_list;
|
||||
SelfList<Area2DSW>::List monitor_query_list;
|
||||
SelfList<Area2DSW>::List area_moved_list;
|
||||
|
|
@ -140,8 +140,8 @@ public:
|
|||
const SelfList<Body2DSW>::List &get_active_body_list() const;
|
||||
void body_add_to_active_list(SelfList<Body2DSW> *p_body);
|
||||
void body_remove_from_active_list(SelfList<Body2DSW> *p_body);
|
||||
void body_add_to_inertia_update_list(SelfList<Body2DSW> *p_body);
|
||||
void body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body);
|
||||
void body_add_to_mass_properties_update_list(SelfList<Body2DSW> *p_body);
|
||||
void body_remove_from_mass_properties_update_list(SelfList<Body2DSW> *p_body);
|
||||
void area_add_to_moved_list(SelfList<Area2DSW> *p_area);
|
||||
void area_remove_from_moved_list(SelfList<Area2DSW> *p_area);
|
||||
const SelfList<Area2DSW>::List &get_moved_area_list() const;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue