diff options
Diffstat (limited to 'servers/physics_3d/godot_space_3d.cpp')
| -rw-r--r-- | servers/physics_3d/godot_space_3d.cpp | 32 |
1 files changed, 23 insertions, 9 deletions
diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp index e8af2d7283..074232dd66 100644 --- a/servers/physics_3d/godot_space_3d.cpp +++ b/servers/physics_3d/godot_space_3d.cpp @@ -445,7 +445,7 @@ struct _RestCallbackData { }; static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - _RestCallbackData *rd = (_RestCallbackData *)p_userdata; + _RestCallbackData *rd = static_cast<_RestCallbackData *>(p_userdata); Vector3 contact_rel = p_point_B - p_point_A; real_t len = contact_rel.length(); @@ -701,6 +701,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: const int max_results = 32; int recover_attempts = 4; Vector3 sr[max_results * 2]; + real_t priorities[max_results]; do { GodotPhysicsServer3D::CollCbkData cbk; @@ -710,6 +711,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk; GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk; + int priority_amount = 0; bool collided = false; @@ -737,6 +739,10 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: if (GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, margin)) { collided = cbk.amount > 0; } + while (cbk.amount > priority_amount) { + priorities[priority_amount] = col_obj->get_collision_priority(); + priority_amount++; + } } } @@ -744,6 +750,12 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: 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; Vector3 recover_motion; @@ -759,7 +771,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: 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; } } @@ -906,7 +918,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: } 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 } @@ -989,6 +1001,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: r_result->collision_unsafe_fraction = unsafe; r_result->collision_count = rcd.result_count; + r_result->collision_depth = rcd.best_result.len; } collided = true; @@ -1002,6 +1015,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: r_result->collision_safe_fraction = 1.0; r_result->collision_unsafe_fraction = 1.0; + r_result->collision_depth = 0.0; } return collided; @@ -1017,7 +1031,7 @@ void *GodotSpace3D::_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A SWAP(type_A, type_B); } - GodotSpace3D *self = (GodotSpace3D *)p_self; + GodotSpace3D *self = static_cast<GodotSpace3D *>(p_self); self->collision_pairs++; @@ -1038,10 +1052,10 @@ void *GodotSpace3D::_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A } } else if (type_A == GodotCollisionObject3D::TYPE_BODY) { if (type_B == GodotCollisionObject3D::TYPE_SOFT_BODY) { - GodotBodySoftBodyPair3D *soft_pair = memnew(GodotBodySoftBodyPair3D((GodotBody3D *)A, p_subindex_A, (GodotSoftBody3D *)B)); + GodotBodySoftBodyPair3D *soft_pair = memnew(GodotBodySoftBodyPair3D(static_cast<GodotBody3D *>(A), p_subindex_A, static_cast<GodotSoftBody3D *>(B))); return soft_pair; } else { - GodotBodyPair3D *b = memnew(GodotBodyPair3D((GodotBody3D *)A, p_subindex_A, (GodotBody3D *)B, p_subindex_B)); + GodotBodyPair3D *b = memnew(GodotBodyPair3D(static_cast<GodotBody3D *>(A), p_subindex_A, static_cast<GodotBody3D *>(B), p_subindex_B)); return b; } } else { @@ -1056,9 +1070,9 @@ void GodotSpace3D::_broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_ return; } - GodotSpace3D *self = (GodotSpace3D *)p_self; + GodotSpace3D *self = static_cast<GodotSpace3D *>(p_self); self->collision_pairs--; - GodotConstraint3D *c = (GodotConstraint3D *)p_data; + GodotConstraint3D *c = static_cast<GodotConstraint3D *>(p_data); memdelete(c); } @@ -1096,7 +1110,7 @@ void GodotSpace3D::remove_object(GodotCollisionObject3D *p_object) { objects.erase(p_object); } -const Set<GodotCollisionObject3D *> &GodotSpace3D::get_objects() const { +const HashSet<GodotCollisionObject3D *> &GodotSpace3D::get_objects() const { return objects; } |