diff options
Diffstat (limited to 'servers/physics_3d')
27 files changed, 374 insertions, 215 deletions
diff --git a/servers/physics_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h index 01a47f222e..309af76561 100644 --- a/servers/physics_3d/gjk_epa.h +++ b/servers/physics_3d/gjk_epa.h @@ -37,4 +37,4 @@ bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); -#endif +#endif // GJK_EPA_H diff --git a/servers/physics_3d/godot_area_3d.cpp b/servers/physics_3d/godot_area_3d.cpp index e2ad765d62..d4d3b3e6aa 100644 --- a/servers/physics_3d/godot_area_3d.cpp +++ b/servers/physics_3d/godot_area_3d.cpp @@ -276,7 +276,11 @@ void GodotArea3D::call_queries() { Callable::CallError ce; Variant ret; - monitor_callback.call((const Variant **)resptr, 5, ret, ce); + monitor_callback.callp((const Variant **)resptr, 5, ret, ce); + + if (ce.error != Callable::CallError::CALL_OK) { + ERR_PRINT_ONCE("Error calling monitor callback method " + Variant::get_callable_error_text(monitor_callback, (const Variant **)resptr, 5, ce)); + } } } else { monitored_bodies.clear(); @@ -314,7 +318,11 @@ void GodotArea3D::call_queries() { Callable::CallError ce; Variant ret; - area_monitor_callback.call((const Variant **)resptr, 5, ret, ce); + area_monitor_callback.callp((const Variant **)resptr, 5, ret, ce); + + if (ce.error != Callable::CallError::CALL_OK) { + ERR_PRINT_ONCE("Error calling area monitor callback method " + Variant::get_callable_error_text(area_monitor_callback, (const Variant **)resptr, 5, ce)); + } } } else { monitored_areas.clear(); @@ -325,12 +333,12 @@ void GodotArea3D::call_queries() { void GodotArea3D::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const { if (is_gravity_point()) { - const real_t gravity_distance_scale = get_gravity_distance_scale(); + const real_t gr_distance_scale = get_gravity_distance_scale(); Vector3 v = get_transform().xform(get_gravity_vector()) - p_position; - if (gravity_distance_scale > 0) { + if (gr_distance_scale > 0) { const real_t v_length = v.length(); if (v_length > 0) { - const real_t v_scaled = v_length * gravity_distance_scale; + const real_t v_scaled = v_length * gr_distance_scale; r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); } else { r_gravity = Vector3(); diff --git a/servers/physics_3d/godot_area_3d.h b/servers/physics_3d/godot_area_3d.h index a00451f602..51b435eb00 100644 --- a/servers/physics_3d/godot_area_3d.h +++ b/servers/physics_3d/godot_area_3d.h @@ -74,9 +74,9 @@ class GodotArea3D : public GodotCollisionObject3D { static uint32_t hash(const BodyKey &p_key) { uint32_t h = hash_one_uint64(p_key.rid.get_id()); - h = hash_djb2_one_64(p_key.instance_id, h); - h = hash_djb2_one_32(p_key.area_shape, h); - return hash_djb2_one_32(p_key.body_shape, h); + h = hash_murmur3_one_64(p_key.instance_id, h); + h = hash_murmur3_one_32(p_key.area_shape, h); + return hash_fmix32(hash_murmur3_one_32(p_key.body_shape, h)); } _FORCE_INLINE_ bool operator==(const BodyKey &p_key) const { diff --git a/servers/physics_3d/godot_body_3d.cpp b/servers/physics_3d/godot_body_3d.cpp index ad97533f44..53f4ab86f9 100644 --- a/servers/physics_3d/godot_body_3d.cpp +++ b/servers/physics_3d/godot_body_3d.cpp @@ -56,7 +56,7 @@ void GodotBody3D::update_mass_properties() { // Update shapes and motions. switch (mode) { - case PhysicsServer3D::BODY_MODE_DYNAMIC: { + case PhysicsServer3D::BODY_MODE_RIGID: { real_t total_area = 0; for (int i = 0; i < get_shape_count(); i++) { if (is_shape_disabled(i)) { @@ -78,10 +78,10 @@ void GodotBody3D::update_mass_properties() { real_t area = get_shape_area(i); - real_t mass = area * this->mass / total_area; + real_t mass_new = area * mass / total_area; // NOTE: we assume that the shape origin is also its center of mass. - center_of_mass_local += mass * get_shape_transform(i).origin; + center_of_mass_local += mass_new * get_shape_transform(i).origin; } center_of_mass_local /= mass; @@ -108,9 +108,9 @@ void GodotBody3D::update_mass_properties() { const GodotShape3D *shape = get_shape(i); - real_t mass = area * this->mass / total_area; + real_t mass_new = area * mass / total_area; - Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass)); + Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass_new)); Transform3D shape_transform = get_shape_transform(i); Basis shape_basis = shape_transform.basis.orthonormalized(); @@ -118,7 +118,7 @@ void GodotBody3D::update_mass_properties() { shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed(); Vector3 shape_origin = shape_transform.origin - center_of_mass_local; - inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass; + inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass_new; } // Set the inertia to a valid value when there are no valid shapes. @@ -154,7 +154,7 @@ void GodotBody3D::update_mass_properties() { _inv_inertia = Vector3(); _inv_mass = 0; } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: { + case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: { _inv_inertia_tensor.set_zero(); _inv_mass = 1.0 / mass; @@ -201,7 +201,7 @@ void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Varian real_t mass_value = p_value; ERR_FAIL_COND(mass_value <= 0); mass = mass_value; - if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC) { + if (mode >= PhysicsServer3D::BODY_MODE_RIGID) { _mass_properties_changed(); } } break; @@ -209,12 +209,12 @@ void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Varian inertia = p_value; if ((inertia.x <= 0.0) || (inertia.y <= 0.0) || (inertia.z <= 0.0)) { calculate_inertia = true; - if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { + if (mode == PhysicsServer3D::BODY_MODE_RIGID) { _mass_properties_changed(); } } else { calculate_inertia = false; - if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { + if (mode == PhysicsServer3D::BODY_MODE_RIGID) { principal_inertia_axes_local = Basis(); _inv_inertia = inertia.inverse(); _update_transform_dependent(); @@ -263,7 +263,7 @@ Variant GodotBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const { return mass; } break; case PhysicsServer3D::BODY_PARAM_INERTIA: { - if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { + if (mode == PhysicsServer3D::BODY_MODE_RIGID) { return _inv_inertia.inverse(); } else { return Vector3(); @@ -315,7 +315,7 @@ void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) { _update_transform_dependent(); } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC: { + case PhysicsServer3D::BODY_MODE_RIGID: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; if (!calculate_inertia) { principal_inertia_axes_local = Basis(); @@ -327,7 +327,7 @@ void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) { set_active(true); } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: { + case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = Vector3(); angular_velocity = Vector3(); @@ -407,7 +407,7 @@ void GodotBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p } break; case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { can_sleep = p_variant; - if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) { + if (mode >= PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep) { set_active(true); } @@ -637,14 +637,14 @@ void GodotBody3D::integrate_forces(real_t p_step) { damp = 0; } - real_t angular_damp = 1.0 - p_step * total_angular_damp; + real_t angular_damp_new = 1.0 - p_step * total_angular_damp; - if (angular_damp < 0) { // reached zero in the given time - angular_damp = 0; + if (angular_damp_new < 0) { // reached zero in the given time + angular_damp_new = 0; } linear_velocity *= damp; - angular_velocity *= angular_damp; + angular_velocity *= angular_damp_new; linear_velocity += _inv_mass * force * p_step; angular_velocity += _inv_inertia_tensor.xform(torque) * p_step; @@ -674,7 +674,7 @@ void GodotBody3D::integrate_velocities(real_t p_step) { return; } - if (fi_callback_data || body_state_callback) { + if (fi_callback_data || body_state_callback.get_object()) { get_space()->body_add_to_state_query_list(&direct_state_query_list); } @@ -707,27 +707,27 @@ void GodotBody3D::integrate_velocities(real_t p_step) { Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; real_t ang_vel = total_angular_velocity.length(); - Transform3D transform = get_transform(); + Transform3D transform_new = get_transform(); if (!Math::is_zero_approx(ang_vel)) { Vector3 ang_vel_axis = total_angular_velocity / ang_vel; Basis rot(ang_vel_axis, ang_vel * p_step); Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1); - transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local); - transform.basis = rot * transform.basis; - transform.orthonormalize(); + transform_new.origin += ((identity3 - rot) * transform_new.basis).xform(center_of_mass_local); + transform_new.basis = rot * transform_new.basis; + transform_new.orthonormalize(); } Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity; /*for(int i=0;i<3;i++) { if (axis_lock&(1<<i)) { - transform.origin[i]=0.0; + transform_new.origin[i]=0.0; } }*/ - transform.origin += total_linear_velocity * p_step; + transform_new.origin += total_linear_velocity * p_step; - _set_transform(transform); + _set_transform(transform_new); _set_inv_transform(get_transform().inverse()); _update_transform_dependent(); @@ -744,7 +744,7 @@ void GodotBody3D::wakeup_neighbours() { continue; } GodotBody3D *b = n[i]; - if (b->mode < PhysicsServer3D::BODY_MODE_DYNAMIC) { + if (b->mode < PhysicsServer3D::BODY_MODE_RIGID) { continue; } @@ -756,22 +756,26 @@ void GodotBody3D::wakeup_neighbours() { } void GodotBody3D::call_queries() { + Variant direct_state_variant = get_direct_state(); + 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; int argc = (fi_callback_data->udata.get_type() == Variant::NIL) ? 1 : 2; Variant rv; - fi_callback_data->callable.call(vp, argc, rv, ce); + fi_callback_data->callable.callp(vp, argc, rv, ce); } } - if (body_state_callback_instance) { - (body_state_callback)(body_state_callback_instance, get_direct_state()); + if (body_state_callback.get_object()) { + const Variant *vp[1] = { &direct_state_variant }; + Callable::CallError ce; + Variant rv; + body_state_callback.callp(vp, 1, rv, ce); } } @@ -792,9 +796,8 @@ bool GodotBody3D::sleep_test(real_t p_step) { } } -void GodotBody3D::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) { - body_state_callback_instance = p_instance; - body_state_callback = p_callback; +void GodotBody3D::set_state_sync_callback(const Callable &p_callable) { + body_state_callback = p_callable; } void GodotBody3D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h index 93bd5a0071..412cbebc7d 100644 --- a/servers/physics_3d/godot_body_3d.h +++ b/servers/physics_3d/godot_body_3d.h @@ -40,7 +40,7 @@ class GodotConstraint3D; class GodotPhysicsDirectBodyState3D; class GodotBody3D : public GodotCollisionObject3D { - PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_DYNAMIC; + PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_RIGID; Vector3 linear_velocity; Vector3 angular_velocity; @@ -131,8 +131,7 @@ class GodotBody3D : public GodotCollisionObject3D { Vector<Contact> contacts; //no contacts by default int contact_count = 0; - void *body_state_callback_instance = nullptr; - PhysicsServer3D::BodyStateCallback body_state_callback = nullptr; + Callable body_state_callback; struct ForceIntegrationCallbackData { Callable callable; @@ -150,7 +149,7 @@ class GodotBody3D : public GodotCollisionObject3D { friend class GodotPhysicsDirectBodyState3D; // i give up, too many functions to expose public: - void set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback); + void set_state_sync_callback(const Callable &p_callable); void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); GodotPhysicsDirectBodyState3D *get_direct_state(); diff --git a/servers/physics_3d/godot_body_direct_state_3d.cpp b/servers/physics_3d/godot_body_direct_state_3d.cpp index a8c6086e1c..25088d33f3 100644 --- a/servers/physics_3d/godot_body_direct_state_3d.cpp +++ b/servers/physics_3d/godot_body_direct_state_3d.cpp @@ -145,7 +145,7 @@ void GodotPhysicsDirectBodyState3D::add_constant_torque(const Vector3 &p_torque) } void GodotPhysicsDirectBodyState3D::set_constant_force(const Vector3 &p_force) { - if (!p_force.is_equal_approx(Vector3())) { + if (!p_force.is_zero_approx()) { body->wakeup(); } body->set_constant_force(p_force); @@ -156,7 +156,7 @@ Vector3 GodotPhysicsDirectBodyState3D::get_constant_force() const { } void GodotPhysicsDirectBodyState3D::set_constant_torque(const Vector3 &p_torque) { - if (!p_torque.is_equal_approx(Vector3())) { + if (!p_torque.is_zero_approx()) { body->wakeup(); } body->set_constant_torque(p_torque); diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp index eebbe0196d..7e6cc6f834 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -170,7 +170,7 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, Vector3 mnormal = motion / mlen; - real_t min, max; + real_t min = 0.0, max = 0.0; p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max); // Did it move enough in this direction to even attempt raycast? diff --git a/servers/physics_3d/godot_collision_object_3d.h b/servers/physics_3d/godot_collision_object_3d.h index 0f09f21962..2d342f65f3 100644 --- a/servers/physics_3d/godot_collision_object_3d.h +++ b/servers/physics_3d/godot_collision_object_3d.h @@ -59,6 +59,7 @@ private: ObjectID instance_id; uint32_t collision_layer = 1; uint32_t collision_mask = 1; + real_t collision_priority = 1.0; struct Shape { Transform3D xform; @@ -165,6 +166,13 @@ public: } _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } + _FORCE_INLINE_ void set_collision_priority(real_t p_priority) { + ERR_FAIL_COND_MSG(p_priority <= 0, "Priority must be greater than 0."); + collision_priority = p_priority; + _shape_changed(); + } + _FORCE_INLINE_ real_t get_collision_priority() const { return collision_priority; } + _FORCE_INLINE_ bool collides_with(GodotCollisionObject3D *p_other) const { return p_other->collision_layer & collision_mask; } diff --git a/servers/physics_3d/godot_collision_solver_3d.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp index b2d3e4d876..ca76a819ec 100644 --- a/servers/physics_3d/godot_collision_solver_3d.cpp +++ b/servers/physics_3d/godot_collision_solver_3d.cpp @@ -38,7 +38,7 @@ #define collision_solver sat_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A); if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { return false; @@ -48,7 +48,7 @@ bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_s static const int max_supports = 16; Vector3 supports[max_supports]; int support_count; - GodotShape3D::FeatureType support_type; + GodotShape3D::FeatureType support_type = GodotShape3D::FeatureType::FEATURE_POINT; p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); if (support_type == GodotShape3D::FEATURE_CIRCLE) { @@ -70,6 +70,7 @@ bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_s bool found = false; for (int i = 0; i < support_count; i++) { + supports[i] += p_margin * supports[i].normalized(); supports[i] = p_transform_B.xform(supports[i]); if (p.distance_to(supports[i]) >= 0) { continue; @@ -337,7 +338,7 @@ bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const real_t axis_scale = 1.0 / axis.length(); axis *= axis_scale; - real_t smin, smax; + real_t smin = 0.0, smax = 0.0; p_shape_A->project_range(axis, rel_transform, smin, smax); smin -= p_margin_A; smax += p_margin_A; @@ -369,23 +370,27 @@ bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const T if (type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { if (type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + WARN_PRINT_ONCE("Collisions between world boundaries are not supported."); return false; } if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + WARN_PRINT_ONCE("Collisions between world boundaries and rays are not supported."); return false; } if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { + WARN_PRINT_ONCE("Collisions between world boundaries and soft bodies are not supported."); return false; } if (swap) { - return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A); } else { - return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_B); } } else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) { if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + WARN_PRINT_ONCE("Collisions between rays are not supported."); return false; } @@ -397,7 +402,7 @@ bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const T } else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { if (type_A == PhysicsServer3D::SHAPE_SOFT_BODY) { - // Soft Body / Soft Body not supported. + WARN_PRINT_ONCE("Collisions between soft bodies are not supported."); return false; } @@ -409,6 +414,7 @@ bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const T } else if (concave_B) { if (concave_A) { + WARN_PRINT_ONCE("Collisions between two concave shapes are not supported."); return false; } @@ -456,8 +462,17 @@ bool GodotCollisionSolver3D::solve_distance_world_boundary(const GodotShape3D *p Vector3 supports[max_supports]; int support_count; GodotShape3D::FeatureType support_type; + Vector3 support_direction = p_transform_B.basis.xform_inv(-p.normal).normalized(); - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); + p_shape_B->get_supports(support_direction, max_supports, supports, support_count, support_type); + + if (support_count == 0) { // This is a poor man's way to detect shapes that don't implement get_supports, such as GodotMotionShape3D. + Vector3 support_B = p_transform_B.xform(p_shape_B->get_support(support_direction)); + r_point_A = p.project(support_B); + r_point_B = support_B; + bool collided = p.distance_to(support_B) <= 0; + return collided; + } if (support_type == GodotShape3D::FEATURE_CIRCLE) { ERR_FAIL_COND_V(support_count != 3, false); diff --git a/servers/physics_3d/godot_collision_solver_3d.h b/servers/physics_3d/godot_collision_solver_3d.h index a6a0ebfead..e7d67903e9 100644 --- a/servers/physics_3d/godot_collision_solver_3d.h +++ b/servers/physics_3d/godot_collision_solver_3d.h @@ -42,7 +42,7 @@ private: static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); static bool soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex); static bool concave_callback(void *p_userdata, GodotShape3D *p_convex); - static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static bool solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp index 20e9300778..96253cb452 100644 --- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp +++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp @@ -629,12 +629,12 @@ public: _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { Vector3 axis = p_axis; - if (axis.is_equal_approx(Vector3())) { + if (axis.is_zero_approx()) { // strange case, try an upwards separator axis = Vector3(0.0, 1.0, 0.0); } - real_t min_A, max_A, min_B, max_B; + real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0; shape_A->project_range(axis, *transform_A, min_A, max_A); shape_B->project_range(axis, *transform_B, min_B, max_B); @@ -964,8 +964,8 @@ static void _collision_sphere_convex_polygon(const GodotShape3D *p_a, const Tran // edges of B for (int i = 0; i < edge_count; i++) { - Vector3 v1 = p_transform_b.xform(vertices[edges[i].a]); - Vector3 v2 = p_transform_b.xform(vertices[edges[i].b]); + Vector3 v1 = p_transform_b.xform(vertices[edges[i].vertex_a]); + Vector3 v2 = p_transform_b.xform(vertices[edges[i].vertex_b]); Vector3 v3 = p_transform_a.origin; Vector3 n1 = v2 - v1; @@ -1404,7 +1404,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo Vector3 e1 = p_transform_a.basis.get_column(i); for (int j = 0; j < edge_count; j++) { - Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); + Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]); Vector3 axis = e1.cross(e2).normalized(); @@ -1460,8 +1460,8 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo } for (int e = 0; e < edge_count; e++) { - Vector3 p1 = p_transform_b.xform(vertices[edges[e].a]); - Vector3 p2 = p_transform_b.xform(vertices[edges[e].b]); + Vector3 p1 = p_transform_b.xform(vertices[edges[e].vertex_a]); + Vector3 p2 = p_transform_b.xform(vertices[edges[e].vertex_b]); Vector3 n = (p2 - p1); if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) { @@ -1771,7 +1771,7 @@ static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Tra for (int i = 0; i < edge_count; i++) { // cylinder - Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]); + Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].vertex_a]) - p_transform_b.basis.xform(vertices[edges[i].vertex_b]); Vector3 axis = edge_axis.cross(p_transform_a.basis.get_column(1)).normalized(); if (!separator.test_axis(axis)) { @@ -1789,8 +1789,8 @@ static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Tra Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); for (int j = 0; j < edge_count; j++) { - Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]); - Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); + Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].vertex_a]); + Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]); Vector3 axis = n1.cross(n2).cross(n2).normalized(); @@ -2075,6 +2075,16 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D separator.generate_contacts(); } +static _FORCE_INLINE_ bool is_minkowski_face(const Vector3 &A, const Vector3 &B, const Vector3 &B_x_A, const Vector3 &C, const Vector3 &D, const Vector3 &D_x_C) { + // Test if arcs AB and CD intersect on the unit sphere + real_t CBA = C.dot(B_x_A); + real_t DBA = D.dot(B_x_A); + real_t ADC = A.dot(D_x_C); + real_t BDC = B.dot(D_x_C); + + return (CBA * DBA < 0.0f) && (ADC * BDC < 0.0f) && (CBA * BDC > 0.0f); +} + template <bool withMargin> static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a); @@ -2129,16 +2139,27 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co } // A<->B edges + for (int i = 0; i < edge_count_A; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]) - p_transform_a.basis.xform(vertices_A[edges_A[i].b]); + Vector3 p1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_a]); + Vector3 q1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_b]); + Vector3 e1 = q1 - p1; + Vector3 u1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_a].plane.normal).normalized(); + Vector3 v1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_b].plane.normal).normalized(); for (int j = 0; j < edge_count_B; j++) { - Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[j].a]) - p_transform_b.basis.xform(vertices_B[edges_B[j].b]); + Vector3 p2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_a]); + Vector3 q2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_b]); + Vector3 e2 = q2 - p2; + Vector3 u2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_a].plane.normal).normalized(); + Vector3 v2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_b].plane.normal).normalized(); - Vector3 axis = e1.cross(e2).normalized(); + if (is_minkowski_face(u1, v1, -e1, -u2, -v2, -e2)) { + Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis(axis)) { - return; + if (!separator.test_axis(axis)) { + return; + } } } } @@ -2157,8 +2178,8 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co //edge-vertex (shell) for (int i = 0; i < edge_count_A; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]); - Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].b]); + Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_a]); + Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_b]); Vector3 n = (e2 - e1); for (int j = 0; j < vertex_count_B; j++) { @@ -2171,8 +2192,8 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co } for (int i = 0; i < edge_count_B; i++) { - Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].a]); - Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].b]); + Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_a]); + Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_b]); Vector3 n = (e2 - e1); for (int j = 0; j < vertex_count_A; j++) { @@ -2231,7 +2252,7 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf // A<->B edges for (int i = 0; i < edge_count; i++) { - Vector3 e1 = p_transform_a.xform(vertices[edges[i].a]) - p_transform_a.xform(vertices[edges[i].b]); + Vector3 e1 = p_transform_a.xform(vertices[edges[i].vertex_a]) - p_transform_a.xform(vertices[edges[i].vertex_b]); for (int j = 0; j < 3; j++) { Vector3 e2 = vertex[j] - vertex[(j + 1) % 3]; @@ -2266,8 +2287,8 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf //edge-vertex (shell) for (int i = 0; i < edge_count; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].a]); - Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].b]); + Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].vertex_a]); + Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].vertex_b]); Vector3 n = (e2 - e1); for (int j = 0; j < 3; j++) { diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.h b/servers/physics_3d/godot_collision_solver_3d_sat.h index 3eb7aa4c9e..46c5ec3254 100644 --- a/servers/physics_3d/godot_collision_solver_3d_sat.h +++ b/servers/physics_3d/godot_collision_solver_3d_sat.h @@ -28,11 +28,11 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef GODOT_COLLISION_SOLVER_SAT_H -#define GODOT_COLLISION_SOLVER_SAT_H +#ifndef GODOT_COLLISION_SOLVER_3D_SAT_H +#define GODOT_COLLISION_SOLVER_3D_SAT_H #include "godot_collision_solver_3d.h" bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); -#endif // GODOT_COLLISION_SOLVER_SAT_H +#endif // GODOT_COLLISION_SOLVER_3D_SAT_H diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp index b735283ebe..68db5df144 100644 --- a/servers/physics_3d/godot_physics_server_3d.cpp +++ b/servers/physics_3d/godot_physics_server_3d.cpp @@ -387,6 +387,13 @@ void GodotPhysicsServer3D::area_set_collision_layer(RID p_area, uint32_t p_layer area->set_collision_layer(p_layer); } +uint32_t GodotPhysicsServer3D::area_get_collision_layer(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, 0); + + return area->get_collision_layer(); +} + void GodotPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) { GodotArea3D *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); @@ -394,6 +401,13 @@ void GodotPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) area->set_collision_mask(p_mask); } +uint32_t GodotPhysicsServer3D::area_get_collision_mask(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, 0); + + return area->get_collision_mask(); +} + void GodotPhysicsServer3D::area_set_monitorable(RID p_area, bool p_monitorable) { GodotArea3D *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); @@ -593,6 +607,20 @@ uint32_t GodotPhysicsServer3D::body_get_collision_mask(RID p_body) const { return body->get_collision_mask(); } +void GodotPhysicsServer3D::body_set_collision_priority(RID p_body, real_t p_priority) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_collision_priority(p_priority); +} + +real_t GodotPhysicsServer3D::body_get_collision_priority(RID p_body) const { + const GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_priority(); +} + void GodotPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p_id) { GodotBody3D *body = body_owner.get_or_null(p_body); if (body) { @@ -746,7 +774,7 @@ void GodotPhysicsServer3D::body_set_constant_force(RID p_body, const Vector3 &p_ ERR_FAIL_COND(!body); body->set_constant_force(p_force); - if (!p_force.is_equal_approx(Vector3())) { + if (!p_force.is_zero_approx()) { body->wakeup(); } } @@ -762,7 +790,7 @@ void GodotPhysicsServer3D::body_set_constant_torque(RID p_body, const Vector3 &p ERR_FAIL_COND(!body); body->set_constant_torque(p_torque); - if (!p_torque.is_equal_approx(Vector3())) { + if (!p_torque.is_zero_approx()) { body->wakeup(); } } @@ -863,10 +891,10 @@ int GodotPhysicsServer3D::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } -void GodotPhysicsServer3D::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { +void GodotPhysicsServer3D::body_set_state_sync_callback(RID p_body, const Callable &p_callable) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->set_state_sync_callback(p_instance, p_callback); + body->set_state_sync_callback(p_callable); } void GodotPhysicsServer3D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { @@ -1182,6 +1210,7 @@ RID GodotPhysicsServer3D::joint_create() { void GodotPhysicsServer3D::joint_clear(RID p_joint) { GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); if (joint->get_type() != JOINT_TYPE_MAX) { GodotJoint3D *empty_joint = memnew(GodotJoint3D); empty_joint->copy_settings_from(joint); diff --git a/servers/physics_3d/godot_physics_server_3d.h b/servers/physics_3d/godot_physics_server_3d.h index 1d57451925..e3e649da57 100644 --- a/servers/physics_3d/godot_physics_server_3d.h +++ b/servers/physics_3d/godot_physics_server_3d.h @@ -148,8 +148,11 @@ public: virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; - virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; + virtual uint32_t area_get_collision_layer(RID p_area) const override; + + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; + virtual uint32_t area_get_collision_mask(RID p_area) const override; virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; @@ -192,6 +195,9 @@ 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_collision_priority(RID p_body, real_t p_priority) override; + virtual real_t body_get_collision_priority(RID p_body) const override; + virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override; virtual uint32_t body_get_user_flags(RID p_body) const override; @@ -239,7 +245,7 @@ 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_state_sync_callback(RID p_body, const Callable &p_callable) override; virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp index 5e310670a5..1443cd166b 100644 --- a/servers/physics_3d/godot_shape_3d.cpp +++ b/servers/physics_3d/godot_shape_3d.cpp @@ -411,9 +411,9 @@ void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 * } bool GodotBoxShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { - AABB aabb(-half_extents, half_extents * 2.0); + AABB aabb_ext(-half_extents, half_extents * 2.0); - return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); + return aabb_ext.intersects_segment(p_begin, p_end, &r_result, &r_normal); } bool GodotBoxShape3D::intersect_point(const Vector3 &p_point) const { @@ -735,29 +735,6 @@ void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vect r_amount = 1; r_type = FEATURE_POINT; r_supports[0] = get_support(p_normal); - return; - - Vector3 n = p_normal; - real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius); - if (Math::abs(h) > 1.0) { - // Top or bottom surface. - n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5; - } else { - // Lateral surface. - n.y = height * 0.5 * h; - } - - real_t s = Math::sqrt(n.x * n.x + n.z * n.z); - if (Math::is_zero_approx(s)) { - n.x = 0.0; - n.z = 0.0; - } else { - real_t scaled_radius = radius / s; - n.x = n.x * scaled_radius; - n.z = n.z * scaled_radius; - } - - r_supports[0] = n; } } @@ -840,48 +817,78 @@ GodotCylinderShape3D::GodotCylinderShape3D() {} /********** CONVEX POLYGON *************/ void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - int vertex_count = mesh.vertices.size(); + uint32_t vertex_count = mesh.vertices.size(); if (vertex_count == 0) { return; } const Vector3 *vrts = &mesh.vertices[0]; - for (int i = 0; i < vertex_count; i++) { - real_t d = p_normal.dot(p_transform.xform(vrts[i])); + if (vertex_count > 3 * extreme_vertices.size()) { + // For a large mesh, two calls to get_support() is faster than a full + // scan over all vertices. - if (i == 0 || d > r_max) { - r_max = d; - } - if (i == 0 || d < r_min) { - r_min = d; + Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); + r_min = p_normal.dot(p_transform.xform(get_support(-n))); + r_max = p_normal.dot(p_transform.xform(get_support(n))); + } else { + for (uint32_t i = 0; i < vertex_count; i++) { + real_t d = p_normal.dot(p_transform.xform(vrts[i])); + + if (i == 0 || d > r_max) { + r_max = d; + } + if (i == 0 || d < r_min) { + r_min = d; + } } } } Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const { - Vector3 n = p_normal; - - int vert_support_idx = -1; - real_t support_max = 0; - - int vertex_count = mesh.vertices.size(); - if (vertex_count == 0) { + if (mesh.vertices.size() == 0) { return Vector3(); } - const Vector3 *vrts = &mesh.vertices[0]; + // Find an initial guess for the support vertex by checking the ones we + // found in _setup(). - for (int i = 0; i < vertex_count; i++) { - real_t d = n.dot(vrts[i]); - - if (i == 0 || d > support_max) { - support_max = d; - vert_support_idx = i; + int best_vertex = -1; + real_t max_support = 0.0; + for (uint32_t i = 0; i < extreme_vertices.size(); i++) { + real_t s = p_normal.dot(mesh.vertices[extreme_vertices[i]]); + if (best_vertex == -1 || s > max_support) { + best_vertex = extreme_vertices[i]; + max_support = s; } } + if (extreme_vertices.size() == mesh.vertices.size()) { + // We've already checked every vertex, so we can return now. + return mesh.vertices[best_vertex]; + } + + // Move along the surface until we reach the true support vertex. - return vrts[vert_support_idx]; + int last_vertex = -1; + while (true) { + int next_vertex = -1; + for (uint32_t i = 0; i < vertex_neighbors[best_vertex].size(); i++) { + int vert = vertex_neighbors[best_vertex][i]; + if (vert != last_vertex) { + real_t s = p_normal.dot(mesh.vertices[vert]); + if (s > max_support) { + next_vertex = vert; + max_support = s; + break; + } + } + } + if (next_vertex == -1) { + return mesh.vertices[best_vertex]; + } + last_vertex = best_vertex; + best_vertex = next_vertex; + } } void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { @@ -938,13 +945,13 @@ void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, } for (int i = 0; i < ec; i++) { - real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal); + real_t dot = (vertices[edges[i].vertex_a] - vertices[edges[i].vertex_b]).normalized().dot(p_normal); dot = ABS(dot); - if (dot < edge_support_threshold && (edges[i].a == vtx || edges[i].b == vtx)) { + if (dot < edge_support_threshold && (edges[i].vertex_a == vtx || edges[i].vertex_b == vtx)) { r_amount = 2; r_type = FEATURE_EDGE; - r_supports[0] = vertices[edges[i].a]; - r_supports[1] = vertices[edges[i].b]; + r_supports[0] = vertices[edges[i].vertex_a]; + r_supports[1] = vertices[edges[i].vertex_b]; return; } } @@ -1048,8 +1055,8 @@ Vector3 GodotConvexPolygonShape3D::get_closest_point_to(const Vector3 &p_point) int ec = mesh.edges.size(); for (int i = 0; i < ec; i++) { Vector3 s[2] = { - vertices[edges[i].a], - vertices[edges[i].b] + vertices[edges[i].vertex_a], + vertices[edges[i].vertex_b] }; Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s); @@ -1081,7 +1088,7 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) { AABB _aabb; - for (int i = 0; i < mesh.vertices.size(); i++) { + for (uint32_t i = 0; i < mesh.vertices.size(); i++) { if (i == 0) { _aabb.position = mesh.vertices[i]; } else { @@ -1090,6 +1097,43 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) { } configure(_aabb); + + // Pre-compute the extreme vertices in 26 directions. This will be used + // to speed up get_support() by letting us quickly get a good guess for + // the support vertex. + + for (int x = -1; x < 2; x++) { + for (int y = -1; y < 2; y++) { + for (int z = -1; z < 2; z++) { + if (x != 0 || y != 0 || z != 0) { + Vector3 dir(x, y, z); + dir.normalize(); + real_t max_support = 0.0; + int best_vertex = -1; + for (uint32_t i = 0; i < mesh.vertices.size(); i++) { + real_t s = dir.dot(mesh.vertices[i]); + if (best_vertex == -1 || s > max_support) { + best_vertex = i; + max_support = s; + } + } + if (extreme_vertices.find(best_vertex) == -1) + extreme_vertices.push_back(best_vertex); + } + } + } + } + + // Record all the neighbors of each vertex. This is used in get_support(). + + if (extreme_vertices.size() < mesh.vertices.size()) { + vertex_neighbors.resize(mesh.vertices.size()); + for (uint32_t i = 0; i < mesh.edges.size(); i++) { + Geometry3D::MeshData::Edge &edge = mesh.edges[i]; + vertex_neighbors[edge.vertex_a].push_back(edge.vertex_b); + vertex_neighbors[edge.vertex_b].push_back(edge.vertex_a); + } + } } void GodotConvexPolygonShape3D::set_data(const Variant &p_data) { @@ -1097,7 +1141,12 @@ void GodotConvexPolygonShape3D::set_data(const Variant &p_data) { } Variant GodotConvexPolygonShape3D::get_data() const { - return mesh.vertices; + Vector<Vector3> vertices; + vertices.resize(mesh.vertices.size()); + for (uint32_t i = 0; i < mesh.vertices.size(); i++) { + vertices.write[i] = mesh.vertices[i]; + } + return vertices; } GodotConvexPolygonShape3D::GodotConvexPolygonShape3D() { @@ -1282,14 +1331,14 @@ Vector3 GodotConcavePolygonShape3D::get_support(const Vector3 &p_normal) const { } void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_params) const { - const BVH *bvh = &p_params->bvh[p_idx]; + const BVH *params_bvh = &p_params->bvh[p_idx]; - if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) { + if (!params_bvh->aabb.intersects_segment(p_params->from, p_params->to)) { return; } - if (bvh->face_index >= 0) { - const Face *f = &p_params->faces[bvh->face_index]; + if (params_bvh->face_index >= 0) { + const Face *f = &p_params->faces[params_bvh->face_index]; GodotFaceShape3D *face = p_params->face; face->normal = f->normal; face->vertex[0] = p_params->vertices[f->indices[0]]; @@ -1308,11 +1357,11 @@ void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_ } } } else { - if (bvh->left >= 0) { - _cull_segment(bvh->left, p_params); + if (params_bvh->left >= 0) { + _cull_segment(params_bvh->left, p_params); } - if (bvh->right >= 0) { - _cull_segment(bvh->right, p_params); + if (params_bvh->right >= 0) { + _cull_segment(params_bvh->right, p_params); } } } @@ -1362,14 +1411,14 @@ Vector3 GodotConcavePolygonShape3D::get_closest_point_to(const Vector3 &p_point) } bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const { - const BVH *bvh = &p_params->bvh[p_idx]; + const BVH *params_bvh = &p_params->bvh[p_idx]; - if (!p_params->aabb.intersects(bvh->aabb)) { + if (!p_params->aabb.intersects(params_bvh->aabb)) { return false; } - if (bvh->face_index >= 0) { - const Face *f = &p_params->faces[bvh->face_index]; + if (params_bvh->face_index >= 0) { + const Face *f = &p_params->faces[params_bvh->face_index]; GodotFaceShape3D *face = p_params->face; face->normal = f->normal; face->vertex[0] = p_params->vertices[f->indices[0]]; @@ -1379,14 +1428,14 @@ bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const { return true; } } else { - if (bvh->left >= 0) { - if (_cull(bvh->left, p_params)) { + if (params_bvh->left >= 0) { + if (_cull(params_bvh->left, p_params)) { return true; } } - if (bvh->right >= 0) { - if (_cull(bvh->right, p_params)) { + if (params_bvh->right >= 0) { + if (_cull(params_bvh->right, p_params)) { return true; } } @@ -1942,14 +1991,14 @@ Vector3 GodotHeightMapShape3D::get_closest_point_to(const Vector3 &p_point) cons } void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const { - const AABB &aabb = get_aabb(); + const AABB &shape_aabb = get_aabb(); - Vector3 pos_local = aabb.position + local_origin; + Vector3 pos_local = shape_aabb.position + local_origin; Vector3 clamped_point(p_point); - clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x); - clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y); - clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z); + clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + shape_aabb.size.x); + clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + shape_aabb.size.y); + clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + shape_aabb.size.z); r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5); r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5); @@ -2093,19 +2142,19 @@ void GodotHeightMapShape3D::_setup(const Vector<real_t> &p_heights, int p_width, depth = p_depth; // Initialize aabb. - AABB aabb; - aabb.position = Vector3(0.0, p_min_height, 0.0); - aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1); + AABB aabb_new; + aabb_new.position = Vector3(0.0, p_min_height, 0.0); + aabb_new.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1); // Initialize origin as the aabb center. - local_origin = aabb.position + 0.5 * aabb.size; + local_origin = aabb_new.position + 0.5 * aabb_new.size; local_origin.y = 0.0; - aabb.position -= local_origin; + aabb_new.position -= local_origin; _build_accelerator(); - configure(aabb); + configure(aabb_new); } void GodotHeightMapShape3D::set_data(const Variant &p_data) { @@ -2116,11 +2165,11 @@ void GodotHeightMapShape3D::set_data(const Variant &p_data) { ERR_FAIL_COND(!d.has("depth")); ERR_FAIL_COND(!d.has("heights")); - int width = d["width"]; - int depth = d["depth"]; + int width_new = d["width"]; + int depth_new = d["depth"]; - ERR_FAIL_COND(width <= 0.0); - ERR_FAIL_COND(depth <= 0.0); + ERR_FAIL_COND(width_new <= 0.0); + ERR_FAIL_COND(depth_new <= 0.0); Variant heights_variant = d["heights"]; Vector<real_t> heights_buffer; @@ -2174,10 +2223,10 @@ void GodotHeightMapShape3D::set_data(const Variant &p_data) { ERR_FAIL_COND(min_height > max_height); - ERR_FAIL_COND(heights_buffer.size() != (width * depth)); + ERR_FAIL_COND(heights_buffer.size() != (width_new * depth_new)); // If specified, min and max height will be used as precomputed values. - _setup(heights_buffer, width, depth, min_height, max_height); + _setup(heights_buffer, width_new, depth_new, min_height, max_height); } Variant GodotHeightMapShape3D::get_data() const { @@ -2185,9 +2234,9 @@ Variant GodotHeightMapShape3D::get_data() const { d["width"] = width; d["depth"] = depth; - const AABB &aabb = get_aabb(); - d["min_height"] = aabb.position.y; - d["max_height"] = aabb.position.y + aabb.size.y; + const AABB &shape_aabb = get_aabb(); + d["min_height"] = shape_aabb.position.y; + d["max_height"] = shape_aabb.position.y + shape_aabb.size.y; d["heights"] = heights; diff --git a/servers/physics_3d/godot_shape_3d.h b/servers/physics_3d/godot_shape_3d.h index 1fc8f7c711..dc8e34e2bc 100644 --- a/servers/physics_3d/godot_shape_3d.h +++ b/servers/physics_3d/godot_shape_3d.h @@ -277,6 +277,8 @@ public: struct GodotConvexPolygonShape3D : public GodotShape3D { Geometry3D::MeshData mesh; + LocalVector<int> extreme_vertices; + LocalVector<LocalVector<int>> vertex_neighbors; void _setup(const Vector<Vector3> &p_vertices); diff --git a/servers/physics_3d/godot_soft_body_3d.cpp b/servers/physics_3d/godot_soft_body_3d.cpp index 173843072a..77110c5fbc 100644 --- a/servers/physics_3d/godot_soft_body_3d.cpp +++ b/servers/physics_3d/godot_soft_body_3d.cpp @@ -722,7 +722,14 @@ void GodotSoftBody3D::reoptimize_link_order() { const int reop_not_dependent = -1; const int reop_node_complete = -2; - uint32_t i, link_count = links.size(), node_count = nodes.size(); + uint32_t link_count = links.size(); + uint32_t node_count = nodes.size(); + + if (link_count < 1 || node_count < 2) { + return; + } + + uint32_t i; Link *lr; int ar, br; Node *node0 = &(nodes[0]); diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp index 533d7605ce..c98409e2c4 100644 --- a/servers/physics_3d/godot_space_3d.cpp +++ b/servers/physics_3d/godot_space_3d.cpp @@ -120,8 +120,8 @@ bool GodotPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parame bool collided = false; Vector3 res_point, res_normal; - int res_shape; - const GodotCollisionObject3D *res_obj; + int res_shape = -1; + const GodotCollisionObject3D *res_obj = nullptr; real_t min_d = 1e10; for (int i = 0; i < amount; i++) { @@ -185,6 +185,7 @@ bool GodotPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parame if (!collided) { return false; } + ERR_FAIL_NULL_V(res_obj, false); // Shouldn't happen but silences warning. r_result.collider_id = res_obj->get_instance_id(); if (r_result.collider_id.is_valid()) { @@ -701,6 +702,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 +712,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 +740,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 +751,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 +772,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; } } @@ -989,6 +1002,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 +1016,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; @@ -1235,7 +1250,7 @@ GodotPhysicsDirectSpaceState3D *GodotSpace3D::get_direct_state() { GodotSpace3D::GodotSpace3D() { body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); - body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0)); + body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg_to_rad(8.0)); body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); @@ -1243,7 +1258,7 @@ GodotSpace3D::GodotSpace3D() { ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/solver_iterations", PropertyInfo(Variant::INT, "physics/3d/solver/solver_iterations", PROPERTY_HINT_RANGE, "1,32,1,or_greater")); contact_recycle_radius = GLOBAL_DEF("physics/3d/solver/contact_recycle_radius", 0.01); - ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/contact_recycle_radius", PropertyInfo(Variant::FLOAT, "physics/3d/solver/contact_max_separation", PROPERTY_HINT_RANGE, "0,0.1,0.01,or_greater")); + ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/contact_recycle_radius", PropertyInfo(Variant::FLOAT, "physics/3d/solver/contact_recycle_radius", PROPERTY_HINT_RANGE, "0,0.1,0.01,or_greater")); contact_max_separation = GLOBAL_DEF("physics/3d/solver/contact_max_separation", 0.05); ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/contact_max_separation", PropertyInfo(Variant::FLOAT, "physics/3d/solver/contact_max_separation", PROPERTY_HINT_RANGE, "0,0.1,0.01,or_greater")); diff --git a/servers/physics_3d/godot_step_3d.cpp b/servers/physics_3d/godot_step_3d.cpp index 99656d01a0..bfedcd29c0 100644 --- a/servers/physics_3d/godot_step_3d.cpp +++ b/servers/physics_3d/godot_step_3d.cpp @@ -44,7 +44,7 @@ void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D p_body->set_island_step(_step); if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { - // Only dynamic bodies are tested for activation. + // Only rigid bodies are tested for activation. p_body_island.push_back(p_body); } @@ -343,7 +343,8 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) { /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ uint32_t total_contraint_count = all_constraints.size(); - work_pool.do_work(total_contraint_count, this, &GodotStep3D::_setup_contraint, nullptr); + WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep3D::_setup_contraint, nullptr, total_contraint_count, -1, true, SNAME("Physics3DConstraintSetup")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -362,7 +363,8 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) { // Warning: _solve_island modifies the constraint islands for optimization purpose, // their content is not reliable after these calls and shouldn't be used anymore. - work_pool.do_work(island_count, this, &GodotStep3D::_solve_island, nullptr); + group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep3D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics3DConstraintSolveIslands")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -409,10 +411,7 @@ GodotStep3D::GodotStep3D() { body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE); all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); - - work_pool.init(); } GodotStep3D::~GodotStep3D() { - work_pool.finish(); } diff --git a/servers/physics_3d/godot_step_3d.h b/servers/physics_3d/godot_step_3d.h index 6d975b0dd3..189487757f 100644 --- a/servers/physics_3d/godot_step_3d.h +++ b/servers/physics_3d/godot_step_3d.h @@ -33,8 +33,8 @@ #include "godot_space_3d.h" +#include "core/object/worker_thread_pool.h" #include "core/templates/local_vector.h" -#include "core/templates/thread_work_pool.h" class GodotStep3D { uint64_t _step = 1; @@ -42,8 +42,6 @@ class GodotStep3D { int iterations = 0; real_t delta = 0.0; - ThreadWorkPool work_pool; - LocalVector<LocalVector<GodotBody3D *>> body_islands; LocalVector<LocalVector<GodotConstraint3D *>> constraint_islands; LocalVector<GodotConstraint3D *> all_constraints; diff --git a/servers/physics_3d/joints/godot_cone_twist_joint_3d.h b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h index fdcc2ceea3..1651d34916 100644 --- a/servers/physics_3d/joints/godot_cone_twist_joint_3d.h +++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h @@ -28,6 +28,9 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_CONE_TWIST_JOINT_3D_H +#define GODOT_CONE_TWIST_JOINT_3D_H + /* Adapted to Godot from the Bullet library. */ @@ -49,9 +52,6 @@ subject to the following restrictions: Written by: Marcus Hennix */ -#ifndef GODOT_CONE_TWIST_JOINT_3D_H -#define GODOT_CONE_TWIST_JOINT_3D_H - #include "servers/physics_3d/godot_joint_3d.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" diff --git a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp index e0fa940104..dfe2bfa4d3 100644 --- a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp @@ -232,7 +232,7 @@ GodotGeneric6DOFJoint3D::GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D * void GodotGeneric6DOFJoint3D::calculateAngleInfo() { Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis; - m_calculatedAxisAngleDiff = relative_frame.get_euler(Basis::EULER_ORDER_XYZ); + m_calculatedAxisAngleDiff = relative_frame.get_euler(EulerOrder::XYZ); // in euler angle mode we do not actually constrain the angular velocity // along the axes axis[0] and axis[2] (although we do use axis[1]) : diff --git a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h index bcf2d18647..6a0ab81461 100644 --- a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h +++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h @@ -28,13 +28,13 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_GENERIC_6DOF_JOINT_3D_H +#define GODOT_GENERIC_6DOF_JOINT_3D_H + /* Adapted to Godot from the Bullet library. */ -#ifndef GODOT_GENERIC_6DOF_JOINT_3D_H -#define GODOT_GENERIC_6DOF_JOINT_3D_H - #include "servers/physics_3d/godot_joint_3d.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" diff --git a/servers/physics_3d/joints/godot_hinge_joint_3d.h b/servers/physics_3d/joints/godot_hinge_joint_3d.h index b934540e8d..f1187771af 100644 --- a/servers/physics_3d/joints/godot_hinge_joint_3d.h +++ b/servers/physics_3d/joints/godot_hinge_joint_3d.h @@ -28,13 +28,13 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_HINGE_JOINT_3D_H +#define GODOT_HINGE_JOINT_3D_H + /* Adapted to Godot from the Bullet library. */ -#ifndef GODOT_HINGE_JOINT_3D_H -#define GODOT_HINGE_JOINT_3D_H - #include "servers/physics_3d/godot_joint_3d.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" diff --git a/servers/physics_3d/joints/godot_jacobian_entry_3d.h b/servers/physics_3d/joints/godot_jacobian_entry_3d.h index 0fe15751d5..a46ce830ec 100644 --- a/servers/physics_3d/joints/godot_jacobian_entry_3d.h +++ b/servers/physics_3d/joints/godot_jacobian_entry_3d.h @@ -28,13 +28,13 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_JACOBIAN_ENTRY_3D_H +#define GODOT_JACOBIAN_ENTRY_3D_H + /* Adapted to Godot from the Bullet library. */ -#ifndef GODOT_JACOBIAN_ENTRY_3D_H -#define GODOT_JACOBIAN_ENTRY_3D_H - /* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ diff --git a/servers/physics_3d/joints/godot_pin_joint_3d.h b/servers/physics_3d/joints/godot_pin_joint_3d.h index eeeaa650bd..b3e2389d5a 100644 --- a/servers/physics_3d/joints/godot_pin_joint_3d.h +++ b/servers/physics_3d/joints/godot_pin_joint_3d.h @@ -28,13 +28,13 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_PIN_JOINT_3D_H +#define GODOT_PIN_JOINT_3D_H + /* Adapted to Godot from the Bullet library. */ -#ifndef GODOT_PIN_JOINT_3D_H -#define GODOT_PIN_JOINT_3D_H - #include "servers/physics_3d/godot_joint_3d.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" diff --git a/servers/physics_3d/joints/godot_slider_joint_3d.h b/servers/physics_3d/joints/godot_slider_joint_3d.h index f596c9ff75..29d19be0d7 100644 --- a/servers/physics_3d/joints/godot_slider_joint_3d.h +++ b/servers/physics_3d/joints/godot_slider_joint_3d.h @@ -28,13 +28,13 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_SLIDER_JOINT_3D_H +#define GODOT_SLIDER_JOINT_3D_H + /* Adapted to Godot from the Bullet library. */ -#ifndef GODOT_SLIDER_JOINT_3D_H -#define GODOT_SLIDER_JOINT_3D_H - #include "servers/physics_3d/godot_joint_3d.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" |