summaryrefslogtreecommitdiff
path: root/servers/physics_2d/space_2d_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d/space_2d_sw.cpp')
-rw-r--r--servers/physics_2d/space_2d_sw.cpp139
1 files changed, 62 insertions, 77 deletions
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 7dbd1243cc..dd0780b5ff 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -34,6 +34,9 @@
#include "core/os/os.h"
#include "core/templates/pair.h"
#include "physics_server_2d_sw.h"
+
+#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
+
_FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (!(p_object->get_collision_layer() & p_collision_mask)) {
return false;
@@ -102,7 +105,6 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S
}
r_results[cc].rid = col_obj->get_self();
r_results[cc].shape = shape_idx;
- r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx);
cc++;
}
@@ -190,7 +192,6 @@ bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vec
r_result.collider = ObjectDB::get_instance(r_result.collider_id);
}
r_result.normal = res_normal;
- r_result.metadata = res_obj->get_shape_metadata(res_shape);
r_result.position = res_point;
r_result.rid = res_obj->get_self();
r_result.shape = res_shape;
@@ -203,7 +204,7 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans
return 0;
}
- Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape);
+ Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0);
Rect2 aabb = p_xform.xform(shape->get_aabb());
@@ -239,7 +240,6 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans
}
r_results[cc].rid = col_obj->get_self();
r_results[cc].shape = shape_idx;
- r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx);
cc++;
}
@@ -248,7 +248,7 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans
}
bool PhysicsDirectSpaceState2DSW::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, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
- Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape);
+ Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, false);
Rect2 aabb = p_xform.xform(shape->get_aabb());
@@ -335,7 +335,7 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &
return false;
}
- Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape);
+ Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0);
Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
@@ -383,18 +383,18 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &
}
struct _RestCallbackData2D {
- const CollisionObject2DSW *object;
- const CollisionObject2DSW *best_object;
- int local_shape;
- int best_local_shape;
- int shape;
- int best_shape;
+ const CollisionObject2DSW *object = nullptr;
+ const CollisionObject2DSW *best_object = nullptr;
+ int local_shape = 0;
+ int best_local_shape = 0;
+ int shape = 0;
+ int best_shape = 0;
Vector2 best_contact;
Vector2 best_normal;
- real_t best_len;
+ real_t best_len = 0.0;
Vector2 valid_dir;
- real_t valid_depth;
- real_t min_allowed_depth;
+ real_t valid_depth = 0.0;
+ real_t min_allowed_depth = 0.0;
};
static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
@@ -432,9 +432,11 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
}
bool PhysicsDirectSpaceState2DSW::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, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
- Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape);
+ Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0);
+ real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+
Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin);
@@ -445,7 +447,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
rcd.best_len = 0;
rcd.best_object = nullptr;
rcd.best_shape = 0;
- rcd.min_allowed_depth = space->test_motion_min_contact_depth;
+ rcd.min_allowed_depth = min_contact_depth;
for (int i = 0; i < amount; i++) {
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
@@ -479,7 +481,6 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
r_info->normal = rcd.best_normal;
r_info->point = rcd.best_contact;
r_info->rid = rcd.best_object->get_self();
- 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() + body->get_center_of_mass());
@@ -492,10 +493,6 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
return true;
}
-PhysicsDirectSpaceState2DSW::PhysicsDirectSpaceState2DSW() {
- space = nullptr;
-}
-
////////////////////////////////////////////////////////////////////////////////////////////////////////////
int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) {
@@ -528,7 +525,7 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) {
return amount;
}
-bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, PhysicsServer2D::MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) {
+bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result) {
//give me back regular physics engine logic
//this is madness
//and most people using this function will think
@@ -560,23 +557,25 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
if (!shapes_found) {
if (r_result) {
*r_result = PhysicsServer2D::MotionResult();
- r_result->travel = p_motion;
+ r_result->travel = p_parameters.motion;
}
return false;
}
// Undo the currently transform the physics server is aware of and apply the provided one
- body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
- body_aabb = body_aabb.grow(p_margin);
+ body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb));
+ body_aabb = body_aabb.grow(p_parameters.margin);
static const int max_excluded_shape_pairs = 32;
ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs];
int excluded_shape_pair_count = 0;
- real_t motion_length = p_motion.length();
- Vector2 motion_normal = p_motion / motion_length;
+ real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+
+ real_t motion_length = p_parameters.motion.length();
+ Vector2 motion_normal = p_parameters.motion / motion_length;
- Transform2D body_transform = p_from;
+ Transform2D body_transform = p_parameters.from;
bool recovered = false;
@@ -613,7 +612,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
- if (p_exclude.has(col_obj->get_self())) {
+ if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
+ continue;
+ }
+ if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
continue;
}
@@ -625,7 +627,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
- cbk.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work
+ cbk.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work
cbk.invalid_by_dir = 0;
if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
@@ -650,7 +652,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
bool did_collide = false;
Shape2DSW *against_shape = col_obj->get_shape(shape_idx);
- if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_margin)) {
+ if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) {
did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
}
@@ -675,6 +677,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
break;
}
+ recovered = true;
+
Vector2 recover_motion;
for (int i = 0; i < cbk.amount; i++) {
Vector2 a = sr[i * 2 + 0];
@@ -686,9 +690,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
// Compute depth on recovered motion.
real_t depth = n.dot(a + recover_motion) - d;
- if (depth > 0.0) {
+ if (depth > min_contact_depth + CMP_EPSILON) {
// Only recover if there is penetration.
- recover_motion -= n * depth * 0.4;
+ recover_motion -= n * (depth - min_contact_depth) * 0.4;
}
}
@@ -697,8 +701,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
break;
}
- recovered = true;
-
body_transform.elements[2] += recover_motion;
body_aabb.position += recover_motion;
@@ -715,7 +717,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
// STEP 2 ATTEMPT MOTION
Rect2 motion_aabb = body_aabb;
- motion_aabb.position += p_motion;
+ motion_aabb.position += p_parameters.motion;
motion_aabb = motion_aabb.merge(body_aabb);
int amount = _cull_aabb_for_body(p_body, motion_aabb);
@@ -729,7 +731,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
// Colliding separation rays allows to properly snap to the ground,
// otherwise it's not needed in regular motion.
- if (!p_collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) {
+ if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) {
// When slide on slope is on, separation ray shape acts like a regular shape.
if (!static_cast<SeparationRayShape2DSW *>(body_shape)->get_slide_on_slope()) {
continue;
@@ -745,9 +747,13 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
- if (p_exclude.has(col_obj->get_self())) {
+ if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
continue;
}
+ if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
+ continue;
+ }
+
int col_shape_idx = intersection_query_subindex_results[i];
Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx);
@@ -766,7 +772,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx);
//test initial overlap, does it collide if going all the way?
- if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
+ if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
continue;
}
@@ -791,7 +797,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
real_t fraction = low + (hi - low) * fraction_coeff;
Vector2 sep = motion_normal; //important optimization for this to work fast enough
- bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
+ bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
if (collided) {
hi = fraction;
@@ -828,7 +834,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
cbk.valid_depth = 10e20;
Vector2 sep = motion_normal; //important optimization for this to work fast enough
- bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), PhysicsServer2DSW::_shape_col_cbk, &cbk, &sep, 0);
+ bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), PhysicsServer2DSW::_shape_col_cbk, &cbk, &sep, 0);
if (!collided || cbk.amount == 0) {
continue;
}
@@ -866,7 +872,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
//it collided, let's get the rest info in unsafe advance
Transform2D ugt = body_transform;
- ugt.elements[2] += p_motion * unsafe;
+ ugt.elements[2] += p_parameters.motion * unsafe;
_RestCallbackData2D rcd;
rcd.best_len = 0;
@@ -874,7 +880,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
rcd.best_shape = 0;
// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
- rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth);
+ rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
int from_shape = best_shape != -1 ? best_shape : 0;
int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
@@ -887,13 +893,16 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j);
Shape2DSW *body_shape = p_body->get_shape(j);
- body_aabb.position += p_motion * unsafe;
+ body_aabb.position += p_parameters.motion * unsafe;
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
- if (p_exclude.has(col_obj->get_self())) {
+ if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
+ continue;
+ }
+ if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
continue;
}
@@ -918,7 +927,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
- rcd.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work
+ rcd.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work
if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *b = static_cast<const Body2DSW *>(col_obj);
@@ -940,7 +949,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
rcd.object = col_obj;
rcd.shape = shape_idx;
rcd.local_shape = j;
- bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_margin);
+ bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin);
if (!sc) {
continue;
}
@@ -958,15 +967,14 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
r_result->collision_depth = rcd.best_len;
r_result->collision_safe_fraction = safe;
r_result->collision_unsafe_fraction = unsafe;
- 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() + 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;
- r_result->remainder = p_motion - safe * p_motion;
- r_result->travel += (body_transform.get_origin() - p_from.get_origin());
+ r_result->travel = safe * p_parameters.motion;
+ r_result->remainder = p_parameters.motion - safe * p_parameters.motion;
+ r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin());
}
collided = true;
@@ -974,9 +982,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
}
if (!collided && r_result) {
- r_result->travel = p_motion;
+ r_result->travel = p_parameters.motion;
r_result->remainder = Vector2();
- r_result->travel += (body_transform.get_origin() - p_from.get_origin());
+ r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin());
}
return collided;
@@ -1145,9 +1153,6 @@ void Space2DSW::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_valu
case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
constraint_bias = p_value;
break;
- case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
- test_motion_min_contact_depth = p_value;
- break;
}
}
@@ -1167,8 +1172,6 @@ real_t Space2DSW::get_param(PhysicsServer2D::SpaceParameter p_param) const {
return body_time_to_sleep;
case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
return constraint_bias;
- case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
- return test_motion_min_contact_depth;
}
return 0;
}
@@ -1190,19 +1193,6 @@ PhysicsDirectSpaceState2DSW *Space2DSW::get_direct_state() {
}
Space2DSW::Space2DSW() {
- collision_pairs = 0;
- active_objects = 0;
- island_count = 0;
-
- contact_debug_count = 0;
-
- locked = false;
- contact_recycle_radius = 1.0;
- contact_max_separation = 1.5;
- contact_max_allowed_penetration = 0.3;
- test_motion_min_contact_depth = 0.005;
-
- constraint_bias = 0.2;
body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0);
body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg2rad(8.0));
body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5);
@@ -1211,14 +1201,9 @@ Space2DSW::Space2DSW() {
broadphase = BroadPhase2DSW::create_func();
broadphase->set_pair_callback(_broadphase_pair, this);
broadphase->set_unpair_callback(_broadphase_unpair, this);
- area = nullptr;
direct_access = memnew(PhysicsDirectSpaceState2DSW);
direct_access->space = this;
-
- for (int i = 0; i < ELAPSED_TIME_MAX; i++) {
- elapsed_time[i] = 0;
- }
}
Space2DSW::~Space2DSW() {
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-13 11:34:08 +0000