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.cpp275
-rw-r--r--servers/physics_2d/body_2d_sw.h173
-rw-r--r--servers/physics_2d/body_direct_state_2d_sw.cpp186
-rw-r--r--servers/physics_2d/body_direct_state_2d_sw.h92
-rw-r--r--servers/physics_2d/joints_2d_sw.cpp15
-rw-r--r--servers/physics_2d/physics_server_2d_sw.cpp31
-rw-r--r--servers/physics_2d/physics_server_2d_sw.h11
-rw-r--r--servers/physics_2d/physics_server_2d_wrap_mt.h7
-rw-r--r--servers/physics_2d/space_2d_sw.cpp22
-rw-r--r--servers/physics_2d/space_2d_sw.h25
-rw-r--r--servers/physics_2d/step_2d_sw.cpp2
11 files changed, 542 insertions, 297 deletions
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index ad47912b82..edd769aa9a 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -29,51 +29,77 @@
/*************************************************************************/
#include "body_2d_sw.h"
+
#include "area_2d_sw.h"
-#include "physics_server_2d_sw.h"
+#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;
- const Shape2DSW *shape = get_shape(i);
+ for (int i = 0; i < get_shape_count(); i++) {
+ if (is_shape_disabled(i)) {
+ continue;
+ }
- real_t area = get_shape_aabb(i).get_area();
+ const Shape2DSW *shape = get_shape(i);
- real_t mass = area * this->mass / total_area;
+ real_t area = get_shape_aabb(i).get_area();
+ if (area == 0.0) {
+ continue;
+ }
- 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();
+ 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 ? (1.0 / inertia) : 0;
+ _inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0;
if (mass) {
_inv_mass = 1.0 / mass;
@@ -93,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) {
@@ -117,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;
@@ -126,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;
@@ -155,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;
@@ -169,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;
}
@@ -206,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);
@@ -214,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 {
@@ -233,7 +272,7 @@ PhysicsServer2D::BodyMode Body2DSW::get_mode() const {
}
void Body2DSW::_shapes_changed() {
- _update_inertia();
+ _mass_properties_changed();
wakeup_neighbours();
}
@@ -268,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;
@@ -295,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);
}
@@ -329,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);
@@ -343,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) {
@@ -428,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;
@@ -443,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;
@@ -477,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();
@@ -495,7 +533,7 @@ void Body2DSW::integrate_velocities(real_t p_step) {
return;
}
- if (fi_callback) {
+ if (fi_callback_data || body_state_callback) {
get_space()->body_add_to_state_query_list(&direct_state_query_list);
}
@@ -514,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() {
@@ -535,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;
}
@@ -547,27 +593,27 @@ void Body2DSW::wakeup_neighbours() {
}
void Body2DSW::call_queries() {
- if (fi_callback) {
- PhysicsDirectBodyState2DSW *dbs = PhysicsDirectBodyState2DSW::singleton;
- dbs->body = this;
-
- Variant v = dbs;
- const Variant *vp[2] = { &v, &fi_callback->callback_udata };
-
- Object *obj = fi_callback->callable.get_object();
- if (!obj) {
+ if (fi_callback_data) {
+ if (!fi_callback_data->callable.get_object()) {
set_force_integration_callback(Callable());
} else {
+ Variant direct_state_variant = get_direct_state();
+ const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata };
+
Callable::CallError ce;
Variant rv;
- if (fi_callback->callback_udata.get_type() != Variant::NIL) {
- fi_callback->callable.call(vp, 2, rv, ce);
+ if (fi_callback_data->udata.get_type() != Variant::NIL) {
+ fi_callback_data->callable.call(vp, 2, rv, ce);
} else {
- fi_callback->callable.call(vp, 1, rv, ce);
+ fi_callback_data->callable.call(vp, 1, rv, ce);
}
}
}
+
+ if (body_state_callback) {
+ (body_state_callback)(body_state_callback_instance, get_direct_state());
+ }
}
bool Body2DSW::sleep_test(real_t p_step) {
@@ -587,78 +633,45 @@ bool Body2DSW::sleep_test(real_t p_step) {
}
}
+void Body2DSW::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) {
+ body_state_callback_instance = p_instance;
+ body_state_callback = p_callback;
+}
+
void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
- if (fi_callback) {
- memdelete(fi_callback);
- fi_callback = nullptr;
+ if (p_callable.get_object()) {
+ if (!fi_callback_data) {
+ fi_callback_data = memnew(ForceIntegrationCallbackData);
+ }
+ fi_callback_data->callable = p_callable;
+ fi_callback_data->udata = p_udata;
+ } else if (fi_callback_data) {
+ memdelete(fi_callback_data);
+ fi_callback_data = nullptr;
}
+}
- if (p_callable.get_object()) {
- fi_callback = memnew(ForceIntegrationCallback);
- fi_callback->callable = p_callable;
- fi_callback->callback_udata = p_udata;
+PhysicsDirectBodyState2DSW *Body2DSW::get_direct_state() {
+ if (!direct_state) {
+ direct_state = memnew(PhysicsDirectBodyState2DSW);
+ direct_state->body = this;
}
+ return 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;
- fi_callback = nullptr;
}
Body2DSW::~Body2DSW() {
- if (fi_callback) {
- memdelete(fi_callback);
+ if (fi_callback_data) {
+ memdelete(fi_callback_data);
}
-}
-
-PhysicsDirectBodyState2DSW *PhysicsDirectBodyState2DSW::singleton = nullptr;
-
-PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() {
- return body->get_space()->get_direct_state();
-}
-
-Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const {
- ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant());
-
- if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) {
- return Variant();
+ if (direct_state) {
+ memdelete(direct_state);
}
- Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider);
-
- int sidx = body->contacts[p_contact_idx].collider_shape;
- if (sidx < 0 || sidx >= other->get_shape_count()) {
- return Variant();
- }
-
- return other->get_shape_metadata(sidx);
}
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 30e14eb9c7..95e89786cd 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -38,50 +38,58 @@
#include "core/templates/vset.h"
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;
@@ -114,24 +122,32 @@ 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;
- struct ForceIntegrationCallback {
+ struct ForceIntegrationCallbackData {
Callable callable;
- Variant callback_udata;
+ Variant udata;
};
- ForceIntegrationCallback *fi_callback;
+ ForceIntegrationCallbackData *fi_callback_data = nullptr;
- uint64_t island_step;
+ PhysicsDirectBodyState2DSW *direct_state = nullptr;
+
+ uint64_t island_step = 0;
_FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area2DSW *p_area);
friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose
public:
+ void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback);
void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
+ PhysicsDirectBodyState2DSW *get_direct_state();
+
_FORCE_INLINE_ void add_area(Area2DSW *p_area) {
int index = areas.find(AreaCMP(p_area));
if (index > -1) {
@@ -198,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) {
@@ -207,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);
@@ -220,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;
@@ -241,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) {
@@ -253,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; }
@@ -332,87 +350,4 @@ void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_no
c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
}
-class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D {
- GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D);
-
-public:
- static PhysicsDirectBodyState2DSW *singleton;
- Body2DSW *body;
- real_t step;
-
- virtual Vector2 get_total_gravity() const override { return body->gravity; } // get gravity vector working on this body space/area
- virtual real_t get_total_angular_damp() const override { return body->area_angular_damp; } // get density of this body space/area
- virtual real_t get_total_linear_damp() const override { return body->area_linear_damp; } // get density of this body space/area
-
- virtual real_t get_inverse_mass() const override { return body->get_inv_mass(); } // get the mass
- virtual real_t get_inverse_inertia() const override { return body->get_inv_inertia(); } // get density of this body space
-
- virtual void set_linear_velocity(const Vector2 &p_velocity) override { body->set_linear_velocity(p_velocity); }
- virtual Vector2 get_linear_velocity() const override { return body->get_linear_velocity(); }
-
- virtual void set_angular_velocity(real_t p_velocity) override { body->set_angular_velocity(p_velocity); }
- virtual real_t get_angular_velocity() const override { return body->get_angular_velocity(); }
-
- virtual void set_transform(const Transform2D &p_transform) override { body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); }
- virtual Transform2D get_transform() const override { return body->get_transform(); }
-
- virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override { return body->get_velocity_in_local_point(p_position); }
-
- virtual void add_central_force(const Vector2 &p_force) override { body->add_central_force(p_force); }
- virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override { body->add_force(p_force, p_position); }
- virtual void add_torque(real_t p_torque) override { body->add_torque(p_torque); }
- virtual void apply_central_impulse(const Vector2 &p_impulse) override { body->apply_central_impulse(p_impulse); }
- virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override { body->apply_impulse(p_impulse, p_position); }
- virtual void apply_torque_impulse(real_t p_torque) override { body->apply_torque_impulse(p_torque); }
-
- virtual void set_sleep_state(bool p_enable) override { body->set_active(!p_enable); }
- virtual bool is_sleeping() const override { return !body->is_active(); }
-
- virtual int get_contact_count() const override { return body->contact_count; }
-
- virtual Vector2 get_contact_local_position(int p_contact_idx) const override {
- ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
- return body->contacts[p_contact_idx].local_pos;
- }
- virtual Vector2 get_contact_local_normal(int p_contact_idx) const override {
- ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
- return body->contacts[p_contact_idx].local_normal;
- }
- virtual int get_contact_local_shape(int p_contact_idx) const override {
- ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
- return body->contacts[p_contact_idx].local_shape;
- }
-
- virtual RID get_contact_collider(int p_contact_idx) const override {
- ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
- return body->contacts[p_contact_idx].collider;
- }
- virtual Vector2 get_contact_collider_position(int p_contact_idx) const override {
- ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
- return body->contacts[p_contact_idx].collider_pos;
- }
- virtual ObjectID get_contact_collider_id(int p_contact_idx) const override {
- ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
- return body->contacts[p_contact_idx].collider_instance_id;
- }
- virtual int get_contact_collider_shape(int p_contact_idx) const override {
- ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
- return body->contacts[p_contact_idx].collider_shape;
- }
- virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const override;
-
- virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override {
- ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
- return body->contacts[p_contact_idx].collider_velocity_at_pos;
- }
-
- virtual PhysicsDirectSpaceState2D *get_space_state() override;
-
- virtual real_t get_step() const override { return step; }
- PhysicsDirectBodyState2DSW() {
- singleton = this;
- body = nullptr;
- }
-};
-
#endif // BODY_2D_SW_H
diff --git a/servers/physics_2d/body_direct_state_2d_sw.cpp b/servers/physics_2d/body_direct_state_2d_sw.cpp
new file mode 100644
index 0000000000..58250c3077
--- /dev/null
+++ b/servers/physics_2d/body_direct_state_2d_sw.cpp
@@ -0,0 +1,186 @@
+/*************************************************************************/
+/* body_direct_state_2d_sw.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#include "body_direct_state_2d_sw.h"
+
+#include "body_2d_sw.h"
+#include "physics_server_2d_sw.h"
+#include "space_2d_sw.h"
+
+Vector2 PhysicsDirectBodyState2DSW::get_total_gravity() const {
+ return body->gravity;
+}
+
+real_t PhysicsDirectBodyState2DSW::get_total_angular_damp() const {
+ return body->area_angular_damp;
+}
+
+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();
+}
+
+real_t PhysicsDirectBodyState2DSW::get_inverse_inertia() const {
+ return body->get_inv_inertia();
+}
+
+void PhysicsDirectBodyState2DSW::set_linear_velocity(const Vector2 &p_velocity) {
+ body->set_linear_velocity(p_velocity);
+}
+
+Vector2 PhysicsDirectBodyState2DSW::get_linear_velocity() const {
+ return body->get_linear_velocity();
+}
+
+void PhysicsDirectBodyState2DSW::set_angular_velocity(real_t p_velocity) {
+ body->set_angular_velocity(p_velocity);
+}
+
+real_t PhysicsDirectBodyState2DSW::get_angular_velocity() const {
+ return body->get_angular_velocity();
+}
+
+void PhysicsDirectBodyState2DSW::set_transform(const Transform2D &p_transform) {
+ body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform);
+}
+
+Transform2D PhysicsDirectBodyState2DSW::get_transform() const {
+ return body->get_transform();
+}
+
+Vector2 PhysicsDirectBodyState2DSW::get_velocity_at_local_position(const Vector2 &p_position) const {
+ return body->get_velocity_in_local_point(p_position);
+}
+
+void PhysicsDirectBodyState2DSW::add_central_force(const Vector2 &p_force) {
+ body->add_central_force(p_force);
+}
+
+void PhysicsDirectBodyState2DSW::add_force(const Vector2 &p_force, const Vector2 &p_position) {
+ body->add_force(p_force, p_position);
+}
+
+void PhysicsDirectBodyState2DSW::add_torque(real_t p_torque) {
+ body->add_torque(p_torque);
+}
+
+void PhysicsDirectBodyState2DSW::apply_central_impulse(const Vector2 &p_impulse) {
+ body->apply_central_impulse(p_impulse);
+}
+
+void PhysicsDirectBodyState2DSW::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) {
+ body->apply_impulse(p_impulse, p_position);
+}
+
+void PhysicsDirectBodyState2DSW::apply_torque_impulse(real_t p_torque) {
+ body->apply_torque_impulse(p_torque);
+}
+
+void PhysicsDirectBodyState2DSW::set_sleep_state(bool p_enable) {
+ body->set_active(!p_enable);
+}
+
+bool PhysicsDirectBodyState2DSW::is_sleeping() const {
+ return !body->is_active();
+}
+
+int PhysicsDirectBodyState2DSW::get_contact_count() const {
+ return body->contact_count;
+}
+
+Vector2 PhysicsDirectBodyState2DSW::get_contact_local_position(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+ return body->contacts[p_contact_idx].local_pos;
+}
+
+Vector2 PhysicsDirectBodyState2DSW::get_contact_local_normal(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+ return body->contacts[p_contact_idx].local_normal;
+}
+
+int PhysicsDirectBodyState2DSW::get_contact_local_shape(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
+ return body->contacts[p_contact_idx].local_shape;
+}
+
+RID PhysicsDirectBodyState2DSW::get_contact_collider(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
+ return body->contacts[p_contact_idx].collider;
+}
+Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_position(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+ return body->contacts[p_contact_idx].collider_pos;
+}
+
+ObjectID PhysicsDirectBodyState2DSW::get_contact_collider_id(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
+ return body->contacts[p_contact_idx].collider_instance_id;
+}
+
+int PhysicsDirectBodyState2DSW::get_contact_collider_shape(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
+ return body->contacts[p_contact_idx].collider_shape;
+}
+
+Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+ return body->contacts[p_contact_idx].collider_velocity_at_pos;
+}
+
+Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant());
+
+ if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) {
+ return Variant();
+ }
+ Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider);
+
+ int sidx = body->contacts[p_contact_idx].collider_shape;
+ if (sidx < 0 || sidx >= other->get_shape_count()) {
+ return Variant();
+ }
+
+ return other->get_shape_metadata(sidx);
+}
+
+PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() {
+ return body->get_space()->get_direct_state();
+}
+
+real_t PhysicsDirectBodyState2DSW::get_step() const {
+ return body->get_space()->get_last_step();
+}
diff --git a/servers/physics_2d/body_direct_state_2d_sw.h b/servers/physics_2d/body_direct_state_2d_sw.h
new file mode 100644
index 0000000000..34faa174d8
--- /dev/null
+++ b/servers/physics_2d/body_direct_state_2d_sw.h
@@ -0,0 +1,92 @@
+/*************************************************************************/
+/* body_direct_state_2d_sw.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef BODY_DIRECT_STATE_2D_SW_H
+#define BODY_DIRECT_STATE_2D_SW_H
+
+#include "servers/physics_server_2d.h"
+
+class Body2DSW;
+
+class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D {
+ GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D);
+
+public:
+ Body2DSW *body = nullptr;
+
+ virtual Vector2 get_total_gravity() const override;
+ 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;
+
+ virtual void set_linear_velocity(const Vector2 &p_velocity) override;
+ virtual Vector2 get_linear_velocity() const override;
+
+ virtual void set_angular_velocity(real_t p_velocity) override;
+ virtual real_t get_angular_velocity() const override;
+
+ virtual void set_transform(const Transform2D &p_transform) override;
+ virtual Transform2D get_transform() const override;
+
+ virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override;
+
+ virtual void add_central_force(const Vector2 &p_force) override;
+ virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
+ virtual void add_torque(real_t p_torque) override;
+ virtual void apply_central_impulse(const Vector2 &p_impulse) override;
+ virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override;
+ virtual void apply_torque_impulse(real_t p_torque) override;
+
+ virtual void set_sleep_state(bool p_enable) override;
+ virtual bool is_sleeping() const override;
+
+ virtual int get_contact_count() const override;
+
+ virtual Vector2 get_contact_local_position(int p_contact_idx) const override;
+ virtual Vector2 get_contact_local_normal(int p_contact_idx) const override;
+ virtual int get_contact_local_shape(int p_contact_idx) const override;
+
+ virtual RID get_contact_collider(int p_contact_idx) const override;
+ virtual Vector2 get_contact_collider_position(int p_contact_idx) const override;
+ virtual ObjectID get_contact_collider_id(int p_contact_idx) const override;
+ virtual int get_contact_collider_shape(int p_contact_idx) const override;
+ virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const override;
+
+ virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
+
+ virtual PhysicsDirectSpaceState2D *get_space_state() override;
+
+ virtual real_t get_step() const override;
+};
+
+#endif // BODY_2D_SW_H
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 813e5ddc17..d0a42ca95b 100644
--- a/servers/physics_2d/physics_server_2d_sw.cpp
+++ b/servers/physics_2d/physics_server_2d_sw.cpp
@@ -30,6 +30,7 @@
#include "physics_server_2d_sw.h"
+#include "body_direct_state_2d_sw.h"
#include "broad_phase_2d_bvh.h"
#include "collision_solver_2d_sw.h"
#include "core/config/project_settings.h"
@@ -742,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);
@@ -926,6 +934,12 @@ int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const {
return body->get_max_contacts_reported();
}
+void PhysicsServer2DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) {
+ Body2DSW *body = body_owner.getornull(p_body);
+ ERR_FAIL_COND(!body);
+ body->set_state_sync_callback(p_instance, p_callback);
+}
+
void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) {
Body2DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
@@ -960,17 +974,13 @@ bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from,
PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) {
ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
- if (!body_owner.owns(p_body)) {
- return nullptr;
- }
-
Body2DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, nullptr);
+
ERR_FAIL_COND_V(!body->get_space(), nullptr);
ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
- direct_state->body = body;
- return direct_state;
+ return body->get_direct_state();
}
/* JOINT API */
@@ -1234,10 +1244,8 @@ void PhysicsServer2DSW::set_collision_iterations(int p_iterations) {
void PhysicsServer2DSW::init() {
doing_sync = false;
- last_step = 0.001;
iterations = 8; // 8?
stepper = memnew(Step2DSW);
- direct_state = memnew(PhysicsDirectBodyState2DSW);
};
void PhysicsServer2DSW::step(real_t p_step) {
@@ -1247,8 +1255,6 @@ void PhysicsServer2DSW::step(real_t p_step) {
_update_shapes();
- last_step = p_step;
- PhysicsDirectBodyState2DSW::singleton->step = p_step;
island_count = 0;
active_objects = 0;
collision_pairs = 0;
@@ -1320,7 +1326,6 @@ void PhysicsServer2DSW::end_sync() {
void PhysicsServer2DSW::finish() {
memdelete(stepper);
- memdelete(direct_state);
};
void PhysicsServer2DSW::_update_shapes() {
diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h
index c5b6d550d7..6a2d9e37e0 100644
--- a/servers/physics_2d/physics_server_2d_sw.h
+++ b/servers/physics_2d/physics_server_2d_sw.h
@@ -46,7 +46,6 @@ class PhysicsServer2DSW : public PhysicsServer2D {
bool active;
int iterations;
bool doing_sync;
- real_t last_step;
int island_count;
int active_objects;
@@ -59,8 +58,6 @@ class PhysicsServer2DSW : public PhysicsServer2D {
Step2DSW *stepper;
Set<const Space2DSW *> active_spaces;
- PhysicsDirectBodyState2DSW *direct_state;
-
mutable RID_PtrOwner<Shape2DSW, true> shape_owner;
mutable RID_PtrOwner<Space2DSW, true> space_owner;
mutable RID_PtrOwner<Area2DSW, true> area_owner;
@@ -208,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;
@@ -242,7 +241,9 @@ public:
virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override;
virtual int body_get_max_contacts_reported(RID p_body) const override;
+ virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override;
virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override;
+
virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override;
virtual void body_set_pickable(RID p_body, bool p_pickable) override;
diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h
index 05d4ac64b0..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);
@@ -245,6 +247,7 @@ public:
FUNC2(body_set_omit_force_integration, RID, bool);
FUNC1RC(bool, body_is_omitting_force_integration, RID);
+ FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback);
FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &);
bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override {
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index c0493749e0..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 {
@@ -634,7 +634,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
Vector2 lv = b->get_linear_velocity();
//compute displacement from linear velocity
- Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step;
+ Vector2 motion = lv * last_step;
real_t motion_len = motion.length();
motion.normalize();
cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0);
@@ -926,7 +926,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
Vector2 lv = b->get_linear_velocity();
//compute displacement from linear velocity
- Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step;
+ Vector2 motion = lv * last_step;
real_t motion_len = motion.length();
motion.normalize();
rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0);
@@ -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 392df9b025..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;
@@ -117,6 +117,8 @@ private:
bool locked;
+ real_t last_step = 0.001;
+
int island_count;
int active_objects;
int collision_pairs;
@@ -138,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;
@@ -172,6 +174,9 @@ public:
void lock();
void unlock();
+ real_t get_last_step() const { return last_step; }
+ void set_last_step(real_t p_step) { last_step = p_step; }
+
void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value);
real_t get_param(PhysicsServer2D::SpaceParameter p_param) const;
diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp
index 8b30160cc1..0306ec5050 100644
--- a/servers/physics_2d/step_2d_sw.cpp
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -129,6 +129,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
p_space->setup(); //update inertias, etc
+ p_space->set_last_step(p_delta);
+
iterations = p_iterations;
delta = p_delta;
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-18 17:28:49 +0000