summaryrefslogtreecommitdiff
path: root/servers/physics_2d/godot_space_2d.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d/godot_space_2d.cpp')
-rw-r--r--servers/physics_2d/godot_space_2d.cpp241
1 files changed, 141 insertions, 100 deletions
diff --git a/servers/physics_2d/godot_space_2d.cpp b/servers/physics_2d/godot_space_2d.cpp
index d72014a8ed..4166191be8 100644
--- a/servers/physics_2d/godot_space_2d.cpp
+++ b/servers/physics_2d/godot_space_2d.cpp
@@ -5,8 +5,8 @@
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
-/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2022 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 */
@@ -36,6 +36,7 @@
#include "core/os/os.h"
#include "core/templates/pair.h"
+#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001
#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
@@ -54,13 +55,13 @@ _FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, u
return true;
}
-int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas, ObjectID p_canvas_instance_id) {
+int GodotPhysicsDirectSpaceState2D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
if (p_result_max <= 0) {
return 0;
}
Rect2 aabb;
- aabb.position = p_point - Vector2(0.00001, 0.00001);
+ aabb.position = p_parameters.position - Vector2(0.00001, 0.00001);
aabb.size = Vector2(0.00002, 0.00002);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
@@ -68,21 +69,21 @@ int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point
int cc = 0;
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)) {
+ if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
continue;
}
- if (p_exclude.has(space->intersection_query_results[i]->get_self())) {
+ if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
continue;
}
const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
- if (p_pick_point && !col_obj->is_pickable()) {
+ if (p_parameters.pick_point && !col_obj->is_pickable()) {
continue;
}
- if (p_filter_by_canvas && col_obj->get_canvas_instance_id() != p_canvas_instance_id) {
+ if (p_parameters.canvas_instance_id.is_valid() && col_obj->get_canvas_instance_id() != p_parameters.canvas_instance_id) {
continue;
}
@@ -90,7 +91,7 @@ int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point
GodotShape2D *shape = col_obj->get_shape(shape_idx);
- Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point);
+ Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_parameters.position);
if (!shape->contains_point(local_point)) {
continue;
@@ -113,21 +114,13 @@ int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point
return cc;
}
-int GodotPhysicsDirectSpaceState2D::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) {
- return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point);
-}
-
-int GodotPhysicsDirectSpaceState2D::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, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) {
- return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point, true, p_canvas_instance_id);
-}
-
-bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+bool GodotPhysicsDirectSpaceState2D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) {
ERR_FAIL_COND_V(space->locked, false);
Vector2 begin, end;
Vector2 normal;
- begin = p_from;
- end = p_to;
+ begin = p_parameters.from;
+ end = p_parameters.to;
normal = (end - begin).normalized();
int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
@@ -141,11 +134,11 @@ bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const
real_t min_d = 1e10;
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)) {
+ if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
continue;
}
- if (p_exclude.has(space->intersection_query_results[i]->get_self())) {
+ if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
continue;
}
@@ -157,16 +150,26 @@ bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const
Vector2 local_from = inv_xform.xform(begin);
Vector2 local_to = inv_xform.xform(end);
- /*local_from = col_obj->get_inv_transform().xform(begin);
- local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from);
-
- local_to = col_obj->get_inv_transform().xform(end);
- local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/
-
const GodotShape2D *shape = col_obj->get_shape(shape_idx);
Vector2 shape_point, shape_normal;
+ if (shape->contains_point(local_from)) {
+ if (p_parameters.hit_from_inside) {
+ // Hit shape at starting point.
+ min_d = 0;
+ res_point = begin;
+ res_normal = Vector2();
+ res_shape = shape_idx;
+ res_obj = col_obj;
+ collided = true;
+ break;
+ } else {
+ // Ignore shape when starting inside.
+ continue;
+ }
+ }
+
if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) {
Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
shape_point = xform.xform(shape_point);
@@ -200,16 +203,17 @@ bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const
return true;
}
-int GodotPhysicsDirectSpaceState2D::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, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+int GodotPhysicsDirectSpaceState2D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
if (p_result_max <= 0) {
return 0;
}
- GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape);
+ GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
ERR_FAIL_COND_V(!shape, 0);
- Rect2 aabb = p_xform.xform(shape->get_aabb());
- aabb = aabb.grow(p_margin);
+ Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
+ aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
+ aabb = aabb.grow(p_parameters.margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
@@ -220,18 +224,18 @@ int GodotPhysicsDirectSpaceState2D::intersect_shape(const RID &p_shape, const Tr
break;
}
- if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
+ if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
continue;
}
- if (p_exclude.has(space->intersection_query_results[i]->get_self())) {
+ if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
continue;
}
const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i];
- if (!GodotCollisionSolver2D::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) {
+ if (!GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) {
continue;
}
@@ -248,13 +252,13 @@ int GodotPhysicsDirectSpaceState2D::intersect_shape(const RID &p_shape, const Tr
return cc;
}
-bool GodotPhysicsDirectSpaceState2D::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) {
- GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape);
+bool GodotPhysicsDirectSpaceState2D::cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe) {
+ GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
ERR_FAIL_COND_V(!shape, false);
- Rect2 aabb = p_xform.xform(shape->get_aabb());
- aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
- aabb = aabb.grow(p_margin);
+ Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
+ aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
+ aabb = aabb.grow(p_parameters.margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
@@ -262,11 +266,11 @@ bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Trans
real_t best_unsafe = 1;
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)) {
+ if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
continue;
}
- if (p_exclude.has(space->intersection_query_results[i]->get_self())) {
+ if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
continue; //ignore excluded
}
@@ -275,16 +279,16 @@ bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Trans
Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
- if (!GodotCollisionSolver2D::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) {
+ if (!GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) {
continue;
}
//test initial overlap, ignore objects it's inside of.
- if (GodotCollisionSolver2D::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) {
+ if (GodotCollisionSolver2D::solve(shape, p_parameters.transform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) {
continue;
}
- Vector2 mnormal = p_motion.normalized();
+ Vector2 mnormal = p_parameters.motion.normalized();
//just do kinematic solving
real_t low = 0.0;
@@ -294,7 +298,7 @@ bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Trans
real_t fraction = low + (hi - low) * fraction_coeff;
Vector2 sep = mnormal; //important optimization for this to work fast enough
- bool collided = GodotCollisionSolver2D::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin);
+ bool collided = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_parameters.margin);
if (collided) {
hi = fraction;
@@ -331,17 +335,17 @@ bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Trans
return true;
}
-bool GodotPhysicsDirectSpaceState2D::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, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+bool GodotPhysicsDirectSpaceState2D::collide_shape(const ShapeParameters &p_parameters, Vector2 *r_results, int p_result_max, int &r_result_count) {
if (p_result_max <= 0) {
return false;
}
- GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape);
+ GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
ERR_FAIL_COND_V(!shape, 0);
- 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);
+ Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
+ aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
+ aabb = aabb.grow(p_parameters.margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
@@ -358,13 +362,13 @@ bool GodotPhysicsDirectSpaceState2D::collide_shape(RID p_shape, const Transform2
GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk;
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)) {
+ if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
continue;
}
const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
- if (p_exclude.has(col_obj->get_self())) {
+ if (p_parameters.exclude.has(col_obj->get_self())) {
continue;
}
@@ -373,7 +377,7 @@ bool GodotPhysicsDirectSpaceState2D::collide_shape(RID p_shape, const Transform2
cbk.valid_dir = Vector2();
cbk.valid_depth = 0;
- if (GodotCollisionSolver2D::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_margin)) {
+ if (GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) {
collided = cbk.amount > 0;
}
}
@@ -399,7 +403,7 @@ struct _RestCallbackData2D {
};
static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
- _RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata;
+ _RestCallbackData2D *rd = static_cast<_RestCallbackData2D *>(p_userdata);
Vector2 contact_rel = p_point_B - p_point_A;
real_t len = contact_rel.length();
@@ -432,32 +436,33 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
rd->best_local_shape = rd->local_shape;
}
-bool GodotPhysicsDirectSpaceState2D::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) {
- GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape);
+bool GodotPhysicsDirectSpaceState2D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) {
+ GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
ERR_FAIL_COND_V(!shape, 0);
- real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+ real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE);
- 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);
+ Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
+ aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
+ aabb = aabb.grow(margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
_RestCallbackData2D rcd;
- rcd.best_len = 0;
- rcd.best_object = nullptr;
- rcd.best_shape = 0;
- rcd.min_allowed_depth = min_contact_depth;
+
+ // Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
+ real_t motion_length = p_parameters.motion.length();
+ real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+ rcd.min_allowed_depth = MIN(motion_length, 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)) {
+ if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
continue;
}
const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
- if (p_exclude.has(col_obj->get_self())) {
+ if (p_parameters.exclude.has(col_obj->get_self())) {
continue;
}
@@ -467,7 +472,7 @@ bool GodotPhysicsDirectSpaceState2D::rest_info(RID p_shape, const Transform2D &p
rcd.object = col_obj;
rcd.shape = shape_idx;
rcd.local_shape = 0;
- bool sc = GodotCollisionSolver2D::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, p_margin);
+ bool sc = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, margin);
if (!sc) {
continue;
}
@@ -538,6 +543,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
r_result->collider_id = ObjectID();
r_result->collider_shape = 0;
}
+
Rect2 body_aabb;
bool shapes_found = false;
@@ -563,15 +569,17 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
return false;
}
+ real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE);
+
// Undo the currently transform the physics server is aware of and apply the provided one
body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb));
- body_aabb = body_aabb.grow(p_parameters.margin);
+ body_aabb = body_aabb.grow(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 min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+ real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
real_t motion_length = p_parameters.motion.length();
Vector2 motion_normal = p_parameters.motion / motion_length;
@@ -586,6 +594,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
const int max_results = 32;
int recover_attempts = 4;
Vector2 sr[max_results * 2];
+ real_t priorities[max_results];
do {
GodotPhysicsServer2D::CollCbkData cbk;
@@ -598,6 +607,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk;
GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk;
+ int priority_amount = 0;
bool collided = false;
@@ -625,10 +635,10 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
- cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
+ cbk.valid_dir = col_obj_shape_xform.columns[1].normalized();
real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
- cbk.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work
+ cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
cbk.invalid_by_dir = 0;
if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
@@ -653,9 +663,13 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
bool did_collide = false;
GodotShape2D *against_shape = col_obj->get_shape(shape_idx);
- if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) {
+ if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, margin)) {
did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
}
+ while (cbk.amount > priority_amount) {
+ priorities[priority_amount] = col_obj->get_collision_priority();
+ priority_amount++;
+ }
if (!did_collide && cbk.invalid_by_dir > 0) {
//this shape must be excluded
@@ -678,6 +692,12 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
break;
}
+ real_t inv_total_weight = 0.0;
+ for (int i = 0; i < cbk.amount; i++) {
+ inv_total_weight += priorities[i];
+ }
+ inv_total_weight = Math::is_zero_approx(inv_total_weight) ? 1.0 : (real_t)cbk.amount / inv_total_weight;
+
recovered = true;
Vector2 recover_motion;
@@ -693,7 +713,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
real_t depth = n.dot(a + recover_motion) - d;
if (depth > min_contact_depth + CMP_EPSILON) {
// Only recover if there is penetration.
- recover_motion -= n * (depth - min_contact_depth) * 0.4;
+ recover_motion -= n * (depth - min_contact_depth) * 0.4 * priorities[i] * inv_total_weight;
}
}
@@ -702,7 +722,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
break;
}
- body_transform.elements[2] += recover_motion;
+ body_transform.columns[2] += recover_motion;
body_aabb.position += recover_motion;
recover_attempts--;
@@ -780,7 +800,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
//test initial overlap
if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
- Vector2 direction = col_obj_shape_xform.get_axis(1).normalized();
+ Vector2 direction = col_obj_shape_xform.columns[1].normalized();
if (motion_normal.dot(direction) < 0) {
continue;
}
@@ -830,7 +850,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
cbk.amount = 0;
cbk.passed = 0;
cbk.ptr = cd;
- cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
+ cbk.valid_dir = col_obj_shape_xform.columns[1].normalized();
cbk.valid_depth = 10e20;
@@ -866,23 +886,23 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
bool collided = false;
- if (recovered || (safe < 1)) {
+ if ((p_parameters.recovery_as_collision && recovered) || (safe < 1)) {
if (safe >= 1) {
best_shape = -1; //no best shape with cast, reset to -1
}
//it collided, let's get the rest info in unsafe advance
Transform2D ugt = body_transform;
- ugt.elements[2] += p_parameters.motion * unsafe;
+ ugt.columns[2] += p_parameters.motion * unsafe;
_RestCallbackData2D rcd;
- rcd.best_len = 0;
- rcd.best_object = nullptr;
- 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, min_contact_depth);
+ body_aabb.position += p_parameters.motion * unsafe;
+ int amount = _cull_aabb_for_body(p_body, body_aabb);
+
int from_shape = best_shape != -1 ? best_shape : 0;
int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
@@ -894,10 +914,6 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j);
GodotShape2D *body_shape = p_body->get_shape(j);
- 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 GodotCollisionObject2D *col_obj = intersection_query_results[i];
if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
@@ -925,10 +941,10 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
- rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
+ rcd.valid_dir = col_obj_shape_xform.columns[1].normalized();
real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
- rcd.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work
+ rcd.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
@@ -950,7 +966,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
rcd.object = col_obj;
rcd.shape = shape_idx;
rcd.local_shape = j;
- bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin);
+ bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, margin);
if (!sc) {
continue;
}
@@ -991,11 +1007,8 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
return collided;
}
+// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree.
void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self) {
- if (!A->interacts_with(B)) {
- return nullptr;
- }
-
GodotCollisionObject2D::Type type_A = A->get_type();
GodotCollisionObject2D::Type type_B = B->get_type();
if (type_A > type_B) {
@@ -1004,7 +1017,7 @@ void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A
SWAP(type_A, type_B);
}
- GodotSpace2D *self = (GodotSpace2D *)p_self;
+ GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self);
self->collision_pairs++;
if (type_A == GodotCollisionObject2D::TYPE_AREA) {
@@ -1020,7 +1033,7 @@ void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A
}
} else {
- GodotBodyPair2D *b = memnew(GodotBodyPair2D((GodotBody2D *)A, p_subindex_A, (GodotBody2D *)B, p_subindex_B));
+ GodotBodyPair2D *b = memnew(GodotBodyPair2D(static_cast<GodotBody2D *>(A), p_subindex_A, static_cast<GodotBody2D *>(B), p_subindex_B));
return b;
}
@@ -1032,9 +1045,9 @@ void GodotSpace2D::_broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_
return;
}
- GodotSpace2D *self = (GodotSpace2D *)p_self;
+ GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self);
self->collision_pairs--;
- GodotConstraint2D *c = (GodotConstraint2D *)p_data;
+ GodotConstraint2D *c = static_cast<GodotConstraint2D *>(p_data);
memdelete(c);
}
@@ -1072,7 +1085,7 @@ void GodotSpace2D::remove_object(GodotCollisionObject2D *p_object) {
objects.erase(p_object);
}
-const Set<GodotCollisionObject2D *> &GodotSpace2D::get_objects() const {
+const HashSet<GodotCollisionObject2D *> &GodotSpace2D::get_objects() const {
return objects;
}
@@ -1139,9 +1152,12 @@ void GodotSpace2D::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_v
case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION:
contact_max_separation = p_value;
break;
- case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION:
+ case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION:
contact_max_allowed_penetration = p_value;
break;
+ case PhysicsServer2D::SPACE_PARAM_CONTACT_DEFAULT_BIAS:
+ contact_bias = p_value;
+ break;
case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
body_linear_velocity_sleep_threshold = p_value;
break;
@@ -1154,6 +1170,9 @@ void GodotSpace2D::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_v
case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
constraint_bias = p_value;
break;
+ case PhysicsServer2D::SPACE_PARAM_SOLVER_ITERATIONS:
+ solver_iterations = p_value;
+ break;
}
}
@@ -1163,8 +1182,10 @@ real_t GodotSpace2D::get_param(PhysicsServer2D::SpaceParameter p_param) const {
return contact_recycle_radius;
case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION:
return contact_max_separation;
- case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION:
+ case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION:
return contact_max_allowed_penetration;
+ case PhysicsServer2D::SPACE_PARAM_CONTACT_DEFAULT_BIAS:
+ return contact_bias;
case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
return body_linear_velocity_sleep_threshold;
case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
@@ -1173,6 +1194,8 @@ real_t GodotSpace2D::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_SOLVER_ITERATIONS:
+ return solver_iterations;
}
return 0;
}
@@ -1199,6 +1222,24 @@ GodotSpace2D::GodotSpace2D() {
body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5);
ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/2d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater"));
+ solver_iterations = GLOBAL_DEF("physics/2d/solver/solver_iterations", 16);
+ ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/solver_iterations", PropertyInfo(Variant::INT, "physics/2d/solver/solver_iterations", PROPERTY_HINT_RANGE, "1,32,1,or_greater"));
+
+ contact_recycle_radius = GLOBAL_DEF("physics/2d/solver/contact_recycle_radius", 1.0);
+ ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/contact_recycle_radius", PropertyInfo(Variant::FLOAT, "physics/2d/solver/contact_max_separation", PROPERTY_HINT_RANGE, "0,10,0.01,or_greater"));
+
+ contact_max_separation = GLOBAL_DEF("physics/2d/solver/contact_max_separation", 1.5);
+ ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/contact_max_separation", PropertyInfo(Variant::FLOAT, "physics/2d/solver/contact_max_separation", PROPERTY_HINT_RANGE, "0,10,0.01,or_greater"));
+
+ contact_max_allowed_penetration = GLOBAL_DEF("physics/2d/solver/contact_max_allowed_penetration", 0.3);
+ ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/contact_max_allowed_penetration", PropertyInfo(Variant::FLOAT, "physics/2d/solver/contact_max_allowed_penetration", PROPERTY_HINT_RANGE, "0,10,0.01,or_greater"));
+
+ contact_bias = GLOBAL_DEF("physics/2d/solver/default_contact_bias", 0.8);
+ ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/default_contact_bias", PropertyInfo(Variant::FLOAT, "physics/2d/solver/default_contact_bias", PROPERTY_HINT_RANGE, "0,1,0.01"));
+
+ constraint_bias = GLOBAL_DEF("physics/2d/solver/default_constraint_bias", 0.2);
+ ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/default_constraint_bias", PropertyInfo(Variant::FLOAT, "physics/2d/solver/default_constraint_bias", PROPERTY_HINT_RANGE, "0,1,0.01"));
+
broadphase = GodotBroadPhase2D::create_func();
broadphase->set_pair_callback(_broadphase_pair, this);
broadphase->set_unpair_callback(_broadphase_unpair, this);
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-24 21:58:29 +0000