summaryrefslogtreecommitdiff
path: root/servers/physics_2d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d')
-rw-r--r--servers/physics_2d/body_2d_sw.cpp191
-rw-r--r--servers/physics_2d/body_2d_sw.h75
-rw-r--r--servers/physics_2d/body_direct_state_2d_sw.cpp4
-rw-r--r--servers/physics_2d/body_direct_state_2d_sw.h1
-rw-r--r--servers/physics_2d/joints_2d_sw.cpp15
-rw-r--r--servers/physics_2d/physics_server_2d_sw.cpp11
-rw-r--r--servers/physics_2d/physics_server_2d_sw.h6
-rw-r--r--servers/physics_2d/physics_server_2d_wrap_mt.h6
-rw-r--r--servers/physics_2d/space_2d_sw.cpp18
-rw-r--r--servers/physics_2d/space_2d_sw.h20
10 files changed, 197 insertions, 150 deletions
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 64f71fd25c..edd769aa9a 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -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++) {
+ if (is_shape_disabled(i)) {
+ continue;
+ }
total_area += get_shape_aabb(i).get_area();
}
- inertia = 0;
+ if (calculate_center_of_mass) {
+ // We have to recompute the center of mass.
+ center_of_mass = Vector2();
- for (int i = 0; i < get_shape_count(); i++) {
- if (is_shape_disabled(i)) {
- continue;
+ 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);
+ const Shape2DSW *shape = get_shape(i);
- real_t area = get_shape_aabb(i).get_area();
+ real_t area = get_shape_aabb(i).get_area();
+ if (area == 0.0) {
+ continue;
+ }
- real_t mass = area * this->mass / total_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();
+ 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 ? (1.0 / inertia) : 0;
+ _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();
}
@@ -269,11 +307,13 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va
} break;
case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: {
linear_velocity = p_variant;
+ constant_linear_velocity = linear_velocity;
wakeup();
} break;
case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: {
angular_velocity = p_variant;
+ constant_angular_velocity = angular_velocity;
wakeup();
} break;
@@ -296,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);
}
@@ -330,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);
@@ -344,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) {
@@ -429,10 +467,10 @@ void Body2DSW::integrate_forces(real_t p_step) {
if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
//compute motion, angular and etc. velocities from prev transform
motion = new_transform.get_origin() - get_transform().get_origin();
- linear_velocity = motion / p_step;
+ linear_velocity = constant_linear_velocity + motion / p_step;
real_t rot = new_transform.get_rotation() - get_transform().get_rotation();
- angular_velocity = remainder(rot, 2.0 * Math_PI) / p_step;
+ angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step;
do_motion = true;
@@ -444,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;
@@ -478,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();
@@ -515,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() {
@@ -536,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;
}
@@ -617,35 +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;
- angular_velocity = 0;
- biased_angular_velocity = 0;
- 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() {
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 9cd53ceca1..95e89786cd 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -41,48 +41,55 @@ 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;
+ real_t biased_angular_velocity = 0.0;
Vector2 linear_velocity;
- real_t angular_velocity;
+ real_t angular_velocity = 0.0;
- real_t linear_damp;
- real_t angular_damp;
- real_t gravity_scale;
+ Vector2 constant_linear_velocity;
+ real_t constant_angular_velocity = 0.0;
- real_t mass;
- real_t inertia;
- real_t bounce;
- real_t friction;
+ real_t linear_damp = -1.0;
+ real_t angular_damp = -1.0;
+ real_t gravity_scale = 1.0;
- real_t _inv_mass;
- real_t _inv_inertia;
- bool user_inertia;
+ real_t bounce = 0.0;
+ real_t friction = 1.0;
+
+ 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;
@@ -115,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;
@@ -129,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);
@@ -207,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) {
@@ -216,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);
@@ -229,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;
@@ -250,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) {
@@ -262,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; }
diff --git a/servers/physics_2d/body_direct_state_2d_sw.cpp b/servers/physics_2d/body_direct_state_2d_sw.cpp
index 1f68cca843..58250c3077 100644
--- a/servers/physics_2d/body_direct_state_2d_sw.cpp
+++ b/servers/physics_2d/body_direct_state_2d_sw.cpp
@@ -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();
}
diff --git a/servers/physics_2d/body_direct_state_2d_sw.h b/servers/physics_2d/body_direct_state_2d_sw.h
index aef9186086..34faa174d8 100644
--- a/servers/physics_2d/body_direct_state_2d_sw.h
+++ b/servers/physics_2d/body_direct_state_2d_sw.h
@@ -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;
diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index 5a0a628fbc..fa8499a81d 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -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;
diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp
index 85b5e40752..d0a42ca95b 100644
--- a/servers/physics_2d/physics_server_2d_sw.cpp
+++ b/servers/physics_2d/physics_server_2d_sw.cpp
@@ -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);
diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h
index ef1306cf59..6a2d9e37e0 100644
--- a/servers/physics_2d/physics_server_2d_sw.h
+++ b/servers/physics_2d/physics_server_2d_sw.h
@@ -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;
diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h
index 6761e37daa..e65c4f5f3a 100644
--- a/servers/physics_2d/physics_server_2d_wrap_mt.h
+++ b/servers/physics_2d/physics_server_2d_wrap_mt.h
@@ -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);
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 6c23f49928..7dbd1243cc 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -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());
}
}
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index 5de071a475..ad82a14af5 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -49,13 +49,13 @@ class PhysicsDirectSpaceState2DSW : public PhysicsDirectSpaceState2D {
public:
Space2DSW *space;
- virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override;
- virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override;
- virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
- virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
- virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
- virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
- virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
+ virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override;
+ virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override;
+ virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
+ virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
+ virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
+ virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
+ virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
PhysicsDirectSpaceState2DSW();
};
@@ -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;
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-22 08:06:39 +0000