diff options
Diffstat (limited to 'servers/physics_3d')
28 files changed, 715 insertions, 547 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index a9f5c4aec3..c9e8bcb8ca 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -30,8 +30,16 @@ #include "area_3d_sw.h" #include "body_3d_sw.h" +#include "soft_body_3d_sw.h" #include "space_3d_sw.h" +Area3DSW::BodyKey::BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + Area3DSW::BodyKey::BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { rid = p_body->get_self(); instance_id = p_body->get_instance_id(); @@ -155,6 +163,20 @@ void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant & case PhysicsServer3D::AREA_PARAM_PRIORITY: priority = p_value; break; + case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: + ERR_FAIL_COND_MSG(wind_force_magnitude < 0, "Wind force magnitude must be a non-negative real number, but a negative number was specified."); + wind_force_magnitude = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: + wind_source = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: + wind_direction = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: + ERR_FAIL_COND_MSG(wind_attenuation_factor < 0, "Wind attenuation factor must be a non-negative real number, but a negative number was specified."); + wind_attenuation_factor = p_value; + break; } } @@ -176,6 +198,14 @@ Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const { return angular_damp; case PhysicsServer3D::AREA_PARAM_PRIORITY: return priority; + case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: + return wind_force_magnitude; + case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: + return wind_source; + case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: + return wind_direction; + case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: + return wind_attenuation_factor; } return Variant(); @@ -274,6 +304,26 @@ void Area3DSW::call_queries() { } } +void Area3DSW::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const { + if (is_gravity_point()) { + const real_t gravity_distance_scale = get_gravity_distance_scale(); + Vector3 v = get_transform().xform(get_gravity_vector()) - p_position; + if (gravity_distance_scale > 0) { + const real_t v_length = v.length(); + if (v_length > 0) { + const real_t v_scaled = v_length * gravity_distance_scale; + r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); + } else { + r_gravity = Vector3(); + } + } else { + r_gravity = v.normalized() * get_gravity(); + } + } else { + r_gravity = get_gravity_vector() * get_gravity(); + } +} + Area3DSW::Area3DSW() : CollisionObject3DSW(TYPE_AREA), monitor_query_list(this), diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h index 12f7545c08..d5f1e60119 100644 --- a/servers/physics_3d/area_3d_sw.h +++ b/servers/physics_3d/area_3d_sw.h @@ -34,10 +34,10 @@ #include "collision_object_3d_sw.h" #include "core/templates/self_list.h" #include "servers/physics_server_3d.h" -//#include "servers/physics_3d/query_sw.h" class Space3DSW; class Body3DSW; +class SoftBody3DSW; class Constraint3DSW; class Area3DSW : public CollisionObject3DSW { @@ -49,6 +49,10 @@ class Area3DSW : public CollisionObject3DSW { real_t point_attenuation; real_t linear_damp; real_t angular_damp; + real_t wind_force_magnitude = 0.0; + real_t wind_attenuation_factor = 0.0; + Vector3 wind_source; + Vector3 wind_direction; int priority; bool monitorable; @@ -80,6 +84,7 @@ class Area3DSW : public CollisionObject3DSW { } _FORCE_INLINE_ BodyKey() {} + BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); BodyKey(Area3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); }; @@ -91,21 +96,16 @@ class Area3DSW : public CollisionObject3DSW { _FORCE_INLINE_ BodyState() { state = 0; } }; + Map<BodyKey, BodyState> monitored_soft_bodies; Map<BodyKey, BodyState> monitored_bodies; Map<BodyKey, BodyState> monitored_areas; - //virtual void shape_changed_notify(ShapeSW *p_shape); - //virtual void shape_deleted_notify(ShapeSW *p_shape); - Set<Constraint3DSW *> constraints; virtual void _shapes_changed(); void _queue_monitor_update(); public: - //_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; } - //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } - void set_monitor_callback(ObjectID p_id, const StringName &p_method); _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } @@ -115,6 +115,9 @@ public: _FORCE_INLINE_ void add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); _FORCE_INLINE_ void remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); _FORCE_INLINE_ void remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); @@ -148,6 +151,18 @@ public: _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ int get_priority() const { return priority; } + _FORCE_INLINE_ void set_wind_force_magnitude(real_t p_wind_force_magnitude) { wind_force_magnitude = p_wind_force_magnitude; } + _FORCE_INLINE_ real_t get_wind_force_magnitude() const { return wind_force_magnitude; } + + _FORCE_INLINE_ void set_wind_attenuation_factor(real_t p_wind_attenuation_factor) { wind_attenuation_factor = p_wind_attenuation_factor; } + _FORCE_INLINE_ real_t get_wind_attenuation_factor() const { return wind_attenuation_factor; } + + _FORCE_INLINE_ void set_wind_source(const Vector3 &p_wind_source) { wind_source = p_wind_source; } + _FORCE_INLINE_ const Vector3 &get_wind_source() const { return wind_source; } + + _FORCE_INLINE_ void set_wind_direction(const Vector3 &p_wind_direction) { wind_direction = p_wind_direction; } + _FORCE_INLINE_ const Vector3 &get_wind_direction() const { return wind_direction; } + _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); } _FORCE_INLINE_ const Set<Constraint3DSW *> &get_constraints() const { return constraints; } @@ -162,10 +177,28 @@ public: void call_queries(); + void compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const; + Area3DSW(); ~Area3DSW(); }; +void Area3DSW::add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape); + monitored_soft_bodies[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void Area3DSW::remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape); + monitored_soft_bodies[bk].dec(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + void Area3DSW::add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { BodyKey bk(p_body, p_body_shape, p_area_shape); monitored_bodies[bk].inc(); @@ -198,4 +231,16 @@ void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, u } } +struct AreaCMP { + Area3DSW *area; + int refCount; + _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } + _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } + _FORCE_INLINE_ AreaCMP() {} + _FORCE_INLINE_ AreaCMP(Area3DSW *p_area) { + area = p_area; + refCount = 1; + } +}; + #endif // AREA__SW_H diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp index f43c2c965e..bf4f0035b4 100644 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -33,7 +33,7 @@ bool AreaPair3DSW::setup(real_t p_step) { bool result = false; - if (area->interacts_with(body) && CollisionSolver3DSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) { + if (area->collides_with(body) && CollisionSolver3DSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) { result = true; } @@ -109,46 +109,51 @@ AreaPair3DSW::~AreaPair3DSW() { //////////////////////////////////////////////////// bool Area2Pair3DSW::setup(real_t p_step) { - bool result = false; - if (area_a->interacts_with(area_b) && CollisionSolver3DSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) { - result = true; + bool result_a = area_a->collides_with(area_b); + bool result_b = area_b->collides_with(area_a); + if ((result_a || result_b) && !CollisionSolver3DSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) { + result_a = false; + result_b = false; } - process_collision = false; - if (result != colliding) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - process_collision = true; - } else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + bool process_collision = false; + + process_collision_a = false; + if (result_a != colliding_a) { + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + process_collision_a = true; process_collision = true; } + colliding_a = result_a; + } - colliding = result; + process_collision_b = false; + if (result_b != colliding_b) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + process_collision_b = true; + process_collision = true; + } + colliding_b = result_b; } return process_collision; } bool Area2Pair3DSW::pre_solve(real_t p_step) { - if (!process_collision) { - return false; + if (process_collision_a) { + if (colliding_a) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } else { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } } - if (colliding) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + if (process_collision_b) { + if (colliding_b) { area_b->add_area_to_query(area_a, shape_a, shape_b); - } - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->add_area_to_query(area_b, shape_b, shape_a); - } - } else { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + } else { area_b->remove_area_from_query(area_a, shape_a, shape_b); } - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } } return false; // Never do any post solving. @@ -168,16 +173,100 @@ Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area } Area2Pair3DSW::~Area2Pair3DSW() { - if (colliding) { - if (area_b->has_area_monitor_callback()) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - + if (colliding_a) { if (area_a->has_area_monitor_callback()) { area_a->remove_area_from_query(area_b, shape_b, shape_a); } } + if (colliding_b) { + if (area_b->has_area_monitor_callback()) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + area_a->remove_constraint(this); area_b->remove_constraint(this); } + +//////////////////////////////////////////////////// + +bool AreaSoftBodyPair3DSW::setup(real_t p_step) { + bool result = false; + if ( + area->collides_with(soft_body) && + CollisionSolver3DSW::solve_static( + soft_body->get_shape(soft_body_shape), + soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape), + area->get_shape(area_shape), + area->get_transform() * area->get_shape_transform(area_shape), + nullptr, + this)) { + result = true; + } + + process_collision = false; + if (result != colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + process_collision = true; + } else if (area->has_monitor_callback()) { + process_collision = true; + } + + colliding = result; + } + + return process_collision; +} + +bool AreaSoftBodyPair3DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + soft_body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_soft_body_to_query(soft_body, soft_body_shape, area_shape); + } + } else { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + soft_body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape); + } + } + + return false; // Never do any post solving. +} + +void AreaSoftBodyPair3DSW::solve(real_t p_step) { + // Nothing to do. +} + +AreaSoftBodyPair3DSW::AreaSoftBodyPair3DSW(SoftBody3DSW *p_soft_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape) { + soft_body = p_soft_body; + area = p_area; + soft_body_shape = p_soft_body_shape; + area_shape = p_area_shape; + soft_body->add_constraint(this); + area->add_constraint(this); +} + +AreaSoftBodyPair3DSW::~AreaSoftBodyPair3DSW() { + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + soft_body->remove_area(area); + } + if (area->has_monitor_callback()) { + area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape); + } + } + soft_body->remove_constraint(this); + area->remove_constraint(this); +} diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h index 596d893082..4572dcbb23 100644 --- a/servers/physics_3d/area_pair_3d_sw.h +++ b/servers/physics_3d/area_pair_3d_sw.h @@ -34,6 +34,7 @@ #include "area_3d_sw.h" #include "body_3d_sw.h" #include "constraint_3d_sw.h" +#include "soft_body_3d_sw.h" class AreaPair3DSW : public Constraint3DSW { Body3DSW *body; @@ -57,8 +58,10 @@ class Area2Pair3DSW : public Constraint3DSW { Area3DSW *area_b; int shape_a; int shape_b; - bool colliding = false; - bool process_collision = false; + bool colliding_a = false; + bool colliding_b = false; + bool process_collision_a = false; + bool process_collision_b = false; public: virtual bool setup(real_t p_step) override; @@ -69,4 +72,21 @@ public: ~Area2Pair3DSW(); }; +class AreaSoftBodyPair3DSW : public Constraint3DSW { + SoftBody3DSW *soft_body; + Area3DSW *area; + int soft_body_shape; + int area_shape; + bool colliding = false; + bool process_collision = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + AreaSoftBodyPair3DSW(SoftBody3DSW *p_sof_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape); + ~AreaSoftBodyPair3DSW(); +}; + #endif // AREA_PAIR__SW_H diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index ea6064cb4c..397a38079b 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -379,17 +379,10 @@ void Body3DSW::set_space(Space3DSW *p_space) { first_integration = true; } -void Body3DSW::_compute_area_gravity_and_dampenings(const Area3DSW *p_area) { - if (p_area->is_gravity_point()) { - if (p_area->get_gravity_distance_scale() > 0) { - Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin(); - gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2)); - } else { - gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity(); - } - } else { - gravity += p_area->get_gravity_vector() * p_area->get_gravity(); - } +void Body3DSW::_compute_area_gravity_and_damping(const Area3DSW *p_area) { + Vector3 area_gravity; + p_area->compute_gravity(get_transform().get_origin(), area_gravity); + gravity += area_gravity; area_linear_damp += p_area->get_linear_damp(); area_angular_damp += p_area->get_angular_damp(); @@ -431,7 +424,7 @@ void Body3DSW::integrate_forces(real_t p_step) { switch (mode) { case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_dampenings(aa[i].area); + _compute_area_gravity_and_damping(aa[i].area); stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; } break; case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: @@ -439,7 +432,7 @@ void Body3DSW::integrate_forces(real_t p_step) { gravity = Vector3(0, 0, 0); area_angular_damp = 0; area_linear_damp = 0; - _compute_area_gravity_and_dampenings(aa[i].area); + _compute_area_gravity_and_damping(aa[i].area); stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; } break; default: { @@ -449,7 +442,7 @@ void Body3DSW::integrate_forces(real_t p_step) { } if (!stopped) { - _compute_area_gravity_and_dampenings(def_area); + _compute_area_gravity_and_damping(def_area); } gravity *= gravity_scale; @@ -578,7 +571,7 @@ void Body3DSW::integrate_velocities(real_t p_step) { real_t ang_vel = total_angular_velocity.length(); Transform3D transform = get_transform(); - if (ang_vel != 0.0) { + 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); diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index 0fa31c5037..130be2d42f 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -96,18 +96,6 @@ class Body3DSW : public CollisionObject3DSW { Map<Constraint3DSW *, int> constraint_map; - struct AreaCMP { - Area3DSW *area; - int refCount; - _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } - _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } - _FORCE_INLINE_ AreaCMP() {} - _FORCE_INLINE_ AreaCMP(Area3DSW *p_area) { - area = p_area; - refCount = 1; - } - }; - Vector<AreaCMP> areas; struct Contact { @@ -134,7 +122,7 @@ class Body3DSW : public CollisionObject3DSW { uint64_t island_step; - _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area3DSW *p_area); + _FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area3DSW *p_area); _FORCE_INLINE_ void _update_transform_dependant(); @@ -389,6 +377,8 @@ public: virtual void set_transform(const Transform3D &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); } virtual Transform3D get_transform() const override { return body->get_transform(); } + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override { return body->get_velocity_in_local_point(p_position); } + virtual void add_central_force(const Vector3 &p_force) override { body->add_central_force(p_force); } virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override { body->add_force(p_force, p_position); diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index 6a7f2b73c5..de81348b4e 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -848,7 +848,7 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform3D &p //capsule sphere 1, sphere - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5); + Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; @@ -956,9 +956,12 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo const Vector3 *vertices = mesh.vertices.ptr(); int vertex_count = mesh.vertices.size(); + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); if (!separator.test_axis(axis)) { return; @@ -1204,7 +1207,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_tr // capsule balls, edges of A for (int i = 0; i < 2; i++) { - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5); + Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -1379,9 +1382,12 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform3 } } + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); if (!separator.test_axis(axis)) { return; @@ -1601,8 +1607,8 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform3D & // some values - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5); - Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5); + Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis; Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis; @@ -1673,8 +1679,8 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1); - Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5); - Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5); + Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_1).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) { return; @@ -1733,9 +1739,12 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf int edge_count = mesh.edges.size(); const Vector3 *vertices = mesh.vertices.ptr(); + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); if (!separator.test_axis(axis)) { return; @@ -1759,7 +1768,7 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf for (int i = 0; i < 2; i++) { // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5); + Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -1799,7 +1808,7 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_t // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5); + Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); for (int i = 0; i < 3; i++) { // edge-cylinder @@ -2057,20 +2066,24 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const const Vector3 *vertices_B = mesh_B.vertices.ptr(); int vertex_count_B = mesh_B.vertices.size(); + // Precalculating this makes the transforms faster. + Basis a_xform_normal = p_transform_b.basis.inverse().transposed(); + // faces of A for (int i = 0; i < face_count_A; i++) { - Vector3 axis = p_transform_a.xform(faces_A[i].plane).normal; - //Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized(); + Vector3 axis = a_xform_normal.xform(faces_A[i].plane.normal).normalized(); if (!separator.test_axis(axis)) { return; } } + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + // faces of B for (int i = 0; i < face_count_B; i++) { - Vector3 axis = p_transform_b.xform(faces_B[i].plane).normal; - //Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized(); + Vector3 axis = b_xform_normal.xform(faces_B[i].plane.normal).normalized(); if (!separator.test_axis(axis)) { return; @@ -2260,13 +2273,13 @@ bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_ PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_PLANE, false); - ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_RAY, false); + ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); ERR_FAIL_COND_V(p_shape_A->is_concave(), false); PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_PLANE, false); - ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_RAY, false); + ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); ERR_FAIL_COND_V(p_shape_B->is_concave(), false); static const CollisionFunc collision_table[6][6] = { diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index 67330d497e..4a4a8164d3 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -89,11 +89,11 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T return found; } -bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - const RayShape3DSW *ray = static_cast<const RayShape3DSW *>(p_shape_A); +bool CollisionSolver3DSW::solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { + const SeparationRayShape3DSW *ray = static_cast<const SeparationRayShape3DSW *>(p_shape_A); Vector3 from = p_transform_A.origin; - Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length(); + Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin); Vector3 support_A = to; Transform3D ai = p_transform_B.affine_inverse(); @@ -106,8 +106,18 @@ bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform3 return false; } + // Discard contacts when the ray is fully contained inside the shape. + if (n == Vector3()) { + return false; + } + + // Discard contacts in the wrong direction. + if (n.dot(from - to) < CMP_EPSILON) { + return false; + } + Vector3 support_B = p_transform_B.xform(p); - if (ray->get_slips_on_slope()) { + if (ray->get_slide_on_slope()) { Vector3 global_n = ai.basis.xform_inv(n).normalized(); support_B = support_A + (support_B - support_A).length() * global_n; } @@ -135,6 +145,10 @@ void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, i ++cinfo.contact_count; + if (!cinfo.result_callback) { + return; + } + if (cinfo.swap_result) { cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata); } else { @@ -164,17 +178,17 @@ bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void * transform_B.origin = query_cinfo.node_transform.xform(node_position); query_cinfo.contact_info.node_index = p_node_index; - solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info); + bool collided = solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info); #ifdef DEBUG_ENABLED ++query_cinfo.node_query_count; #endif - // Continue with the query. - return false; + // Stop at first collision if contacts are not needed. + return (collided && !query_cinfo.contact_info.result_callback); } -void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) { +bool CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) { _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); query_cinfo.shape_A = p_convex; @@ -196,9 +210,14 @@ void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW query_cinfo.soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo); + bool collided = (query_cinfo.contact_info.contact_count > 0); + #ifdef DEBUG_ENABLED ++query_cinfo.convex_query_count; #endif + + // Stop at first collision if contacts are not needed. + return (collided && !query_cinfo.contact_info.result_callback); } bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { @@ -272,17 +291,20 @@ struct _ConcaveCollisionInfo { Vector3 close_A, close_B; }; -void CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) { +bool CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) { _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); cinfo.aabb_tests++; bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, nullptr, cinfo.margin_A, cinfo.margin_B); if (!collided) { - return; + return false; } cinfo.collided = true; cinfo.collisions++; + + // Stop at first collision if contacts are not needed. + return !cinfo.result_callback; } bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { @@ -347,7 +369,7 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo if (type_B == PhysicsServer3D::SHAPE_PLANE) { return false; } - if (type_B == PhysicsServer3D::SHAPE_RAY) { + if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { return false; } if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { @@ -360,15 +382,15 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); } - } else if (type_A == PhysicsServer3D::SHAPE_RAY) { - if (type_B == PhysicsServer3D::SHAPE_RAY) { + } else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { return false; } if (swap) { - return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + return solve_separation_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_B); } else { - return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + return solve_separation_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A); } } else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { @@ -399,19 +421,18 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } } -void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *p_convex) { +bool CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *p_convex) { _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); cinfo.aabb_tests++; - if (cinfo.collided) { - return; - } Vector3 close_A, close_B; cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B); if (cinfo.collided) { - return; + // No need to process any more result. + return true; } + if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) { cinfo.close_A = close_A; cinfo.close_B = close_B; @@ -419,6 +440,7 @@ void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW } cinfo.collisions++; + return false; } bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h index a5dd7d48eb..c13614ab3e 100644 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -40,13 +40,13 @@ public: private: static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata); 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 void soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex); - static void concave_callback(void *p_userdata, Shape3DSW *p_convex); + static bool soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex); + static bool concave_callback(void *p_userdata, Shape3DSW *p_convex); static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *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 Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static bool solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *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); - static void concave_distance_callback(void *p_userdata, Shape3DSW *p_convex); + static bool concave_distance_callback(void *p_userdata, Shape3DSW *p_convex); static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); public: diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index 2df991563d..f2f712193a 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -37,7 +37,7 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2008 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp index 56aba24b42..d2b64ce6e3 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h index d0f3dbbd35..c2a0443aff 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -40,7 +40,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index b928f18231..e2bf2845fe 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index 22eb2f4660..572c35266f 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -40,7 +40,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/jacobian_entry_3d_sw.h index 6afa70c816..30c80db23f 100644 --- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h +++ b/servers/physics_3d/joints/jacobian_entry_3d_sw.h @@ -37,7 +37,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index 8eb84d1c2f..7a713c1161 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h index 3d91452850..09deefc5c4 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -40,7 +40,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index 1895fe1e2e..9f01196c30 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index f357bbd67a..f09476f570 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -40,7 +40,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index c1a9d6259d..fbc6f7eee8 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -48,8 +48,8 @@ RID PhysicsServer3DSW::plane_shape_create() { shape->set_self(rid); return rid; } -RID PhysicsServer3DSW::ray_shape_create() { - Shape3DSW *shape = memnew(RayShape3DSW); +RID PhysicsServer3DSW::separation_ray_shape_create() { + Shape3DSW *shape = memnew(SeparationRayShape3DSW); RID rid = shape_owner.make_rid(shape); shape->set_self(rid); return rid; @@ -854,7 +854,7 @@ void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { body->set_ray_pickable(p_enable); } -bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, MotionResult *r_result, bool p_exclude_raycast_shapes) { +bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -862,18 +862,7 @@ bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes); -} - -int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin) { - Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, false); - ERR_FAIL_COND_V(!body->get_space(), false); - ERR_FAIL_COND_V(body->get_space()->is_locked(), false); - - _update_shapes(); - - return body->get_space()->test_body_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude); } PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index 0ccd15fbb2..a18593c90c 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -83,7 +83,7 @@ public: static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); virtual RID plane_shape_create() override; - virtual RID ray_shape_create() override; + virtual RID separation_ray_shape_create() override; virtual RID sphere_shape_create() override; virtual RID box_shape_create() override; virtual RID capsule_shape_create() override; @@ -242,8 +242,7 @@ public: virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; - virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override; - virtual int body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override; + virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override; // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h index 9beec22bcd..674c52d4a3 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -79,7 +79,7 @@ public: //FUNC1RID(shape,ShapeType); todo fix FUNCRID(plane_shape) - FUNCRID(ray_shape) + FUNCRID(separation_ray_shape) FUNCRID(sphere_shape) FUNCRID(box_shape) FUNCRID(capsule_shape) @@ -250,14 +250,9 @@ public: FUNC2(body_set_ray_pickable, RID, bool); - bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override { + bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes); - } - - int body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_3d_server->body_test_ray_separation(p_body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); + return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude); } // this function only works on physics process, errors and returns null otherwise diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index 04a174f9c8..b81d3272c3 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -39,7 +39,7 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org +Copyright (c) 2003-2009 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -166,21 +166,21 @@ PlaneShape3DSW::PlaneShape3DSW() { // -real_t RayShape3DSW::get_length() const { +real_t SeparationRayShape3DSW::get_length() const { return length; } -bool RayShape3DSW::get_slips_on_slope() const { - return slips_on_slope; +bool SeparationRayShape3DSW::get_slide_on_slope() const { + return slide_on_slope; } -void RayShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { +void SeparationRayShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { // don't think this will be even used r_min = 0; r_max = 1; } -Vector3 RayShape3DSW::get_support(const Vector3 &p_normal) const { +Vector3 SeparationRayShape3DSW::get_support(const Vector3 &p_normal) const { if (p_normal.z > 0) { return Vector3(0, 0, length); } else { @@ -188,7 +188,7 @@ Vector3 RayShape3DSW::get_support(const Vector3 &p_normal) const { } } -void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { +void SeparationRayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; r_type = FEATURE_EDGE; @@ -205,15 +205,15 @@ void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s } } -bool RayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool SeparationRayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; //simply not possible } -bool RayShape3DSW::intersect_point(const Vector3 &p_point) const { +bool SeparationRayShape3DSW::intersect_point(const Vector3 &p_point) const { return false; //simply not possible } -Vector3 RayShape3DSW::get_closest_point_to(const Vector3 &p_point) const { +Vector3 SeparationRayShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 s[2] = { Vector3(0, 0, 0), Vector3(0, 0, length) @@ -222,31 +222,31 @@ Vector3 RayShape3DSW::get_closest_point_to(const Vector3 &p_point) const { return Geometry3D::get_closest_point_to_segment(p_point, s); } -Vector3 RayShape3DSW::get_moment_of_inertia(real_t p_mass) const { +Vector3 SeparationRayShape3DSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); } -void RayShape3DSW::_setup(real_t p_length, bool p_slips_on_slope) { +void SeparationRayShape3DSW::_setup(real_t p_length, bool p_slide_on_slope) { length = p_length; - slips_on_slope = p_slips_on_slope; + slide_on_slope = p_slide_on_slope; configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); } -void RayShape3DSW::set_data(const Variant &p_data) { +void SeparationRayShape3DSW::set_data(const Variant &p_data) { Dictionary d = p_data; - _setup(d["length"], d["slips_on_slope"]); + _setup(d["length"], d["slide_on_slope"]); } -Variant RayShape3DSW::get_data() const { +Variant SeparationRayShape3DSW::get_data() const { Dictionary d; d["length"] = length; - d["slips_on_slope"] = slips_on_slope; + d["slide_on_slope"] = slide_on_slope; return d; } -RayShape3DSW::RayShape3DSW() { +SeparationRayShape3DSW::SeparationRayShape3DSW() { length = 1; - slips_on_slope = false; + slide_on_slope = false; } /********** SPHERE *************/ @@ -509,10 +509,10 @@ BoxShape3DSW::BoxShape3DSW() { void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); - real_t h = (n.y > 0) ? height : -height; + real_t h = height * 0.5 - radius; n *= radius; - n.y += h * 0.5; + n.y += (n.y > 0) ? h : -h; r_max = p_normal.dot(p_transform.xform(n)); r_min = p_normal.dot(p_transform.xform(-n)); @@ -521,10 +521,10 @@ void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform3D Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const { Vector3 n = p_normal; - real_t h = (n.y > 0) ? height : -height; + real_t h = height * 0.5 - radius; n *= radius; - n.y += h * 0.5; + n.y += (n.y > 0) ? h : -h; return n; } @@ -542,15 +542,15 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 r_amount = 2; r_type = FEATURE_EDGE; r_supports[0] = n; - r_supports[0].y += height * 0.5; + r_supports[0].y += height * 0.5 - radius; r_supports[1] = n; - r_supports[1].y -= height * 0.5; + r_supports[1].y -= height * 0.5 - radius; } else { - real_t h = (d > 0) ? height : -height; + real_t h = height * 0.5 - radius; n *= radius; - n.y += h * 0.5; + n.y += (d > 0) ? h : -h; r_amount = 1; r_type = FEATURE_POINT; *r_supports = n; @@ -569,7 +569,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & // test against cylinder and spheres :-| - collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn, 1); + collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height - radius * 2.0, radius, &auxres, &auxn, 1); if (collided) { real_t d = norm.dot(auxres); @@ -581,7 +581,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } } - collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5, 0), radius, &auxres, &auxn); + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5 - radius, 0), radius, &auxres, &auxn); if (collided) { real_t d = norm.dot(auxres); @@ -593,7 +593,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } } - collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5, 0), radius, &auxres, &auxn); + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5 + radius, 0), radius, &auxres, &auxn); if (collided) { real_t d = norm.dot(auxres); @@ -614,19 +614,19 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } bool CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const { - if (Math::abs(p_point.y) < height * 0.5) { + if (Math::abs(p_point.y) < height * 0.5 - radius) { return Vector3(p_point.x, 0, p_point.z).length() < radius; } else { Vector3 p = p_point; - p.y = Math::abs(p.y) - height * 0.5; + p.y = Math::abs(p.y) - height * 0.5 + radius; return p.length() < radius; } } Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 s[2] = { - Vector3(0, -height * 0.5, 0), - Vector3(0, height * 0.5, 0), + Vector3(0, -height * 0.5 + radius, 0), + Vector3(0, height * 0.5 - radius, 0), }; Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); @@ -651,7 +651,7 @@ Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const { void CapsuleShape3DSW::_setup(real_t p_height, real_t p_radius) { height = p_height; radius = p_radius; - configure(AABB(Vector3(-radius, -height * 0.5 - radius, -radius), Vector3(radius * 2, height + radius * 2.0, radius * 2))); + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2, height, radius * 2))); } void CapsuleShape3DSW::set_data(const Variant &p_data) { @@ -1382,11 +1382,11 @@ Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) co return Vector3(); } -void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { +bool ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { const BVH *bvh = &p_params->bvh[p_idx]; if (!p_params->aabb.intersects(bvh->aabb)) { - return; + return false; } if (bvh->face_index >= 0) { @@ -1396,20 +1396,27 @@ void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { face->vertex[0] = p_params->vertices[f->indices[0]]; face->vertex[1] = p_params->vertices[f->indices[1]]; face->vertex[2] = p_params->vertices[f->indices[2]]; - p_params->callback(p_params->userdata, face); - + if (p_params->callback(p_params->userdata, face)) { + return true; + } } else { if (bvh->left >= 0) { - _cull(bvh->left, p_params); + if (_cull(bvh->left, p_params)) { + return true; + } } if (bvh->right >= 0) { - _cull(bvh->right, p_params); + if (_cull(bvh->right, p_params)) { + return true; + } } } + + return false; } -void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { +void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { // make matrix local to concave if (faces.size() == 0) { return; @@ -1635,7 +1642,7 @@ ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() { /* HEIGHT MAP SHAPE */ -Vector<float> HeightMapShape3DSW::get_heights() const { +Vector<real_t> HeightMapShape3DSW::get_heights() const { return heights; } @@ -1875,7 +1882,7 @@ void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, i r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5); } -void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { +void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { if (heights.is_empty()) { return; } @@ -1911,13 +1918,17 @@ void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, voi _get_point(x + 1, z, face.vertex[1]); _get_point(x, z + 1, face.vertex[2]); face.normal = Plane(face.vertex[0], face.vertex[2], face.vertex[1]).normal; - p_callback(p_userdata, &face); + if (p_callback(p_userdata, &face)) { + return; + } // Second triangle. face.vertex[0] = face.vertex[1]; _get_point(x + 1, z + 1, face.vertex[1]); face.normal = Plane(face.vertex[0], face.vertex[2], face.vertex[1]).normal; - p_callback(p_userdata, &face); + if (p_callback(p_userdata, &face)) { + return; + } } } } @@ -1932,7 +1943,7 @@ Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const { (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); } -void HeightMapShape3DSW::_setup(const Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { +void HeightMapShape3DSW::_setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { heights = p_heights; width = p_width; depth = p_depth; @@ -1966,8 +1977,12 @@ void HeightMapShape3DSW::set_data(const Variant &p_data) { ERR_FAIL_COND(depth <= 0.0); Variant heights_variant = d["heights"]; - Vector<float> heights_buffer; + Vector<real_t> heights_buffer; +#ifdef REAL_T_IS_DOUBLE + if (heights_variant.get_type() == Variant::PACKED_FLOAT64_ARRAY) { +#else if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) { +#endif // Ready-to-use heights can be passed. heights_buffer = heights_variant; } else if (heights_variant.get_type() == Variant::OBJECT) { @@ -1980,13 +1995,17 @@ void HeightMapShape3DSW::set_data(const Variant &p_data) { PackedByteArray im_data = image->get_data(); heights_buffer.resize(image->get_width() * image->get_height()); - float *w = heights_buffer.ptrw(); - float *rp = (float *)im_data.ptr(); + real_t *w = heights_buffer.ptrw(); + real_t *rp = (real_t *)im_data.ptr(); for (int i = 0; i < heights_buffer.size(); ++i) { w[i] = rp[i]; } } else { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_MSG("Expected PackedFloat64Array or float Image."); +#else ERR_FAIL_MSG("Expected PackedFloat32Array or float Image."); +#endif } // Compute min and max heights or use precomputed values. diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index 0d1b7cc3d7..b05f65f268 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -33,17 +33,6 @@ #include "core/math/geometry_3d.h" #include "servers/physics_server_3d.h" -/* - -SHAPE_LINE, ///< plane:"plane" -SHAPE_SEGMENT, ///< real_t:"length" -SHAPE_CIRCLE, ///< real_t:"radius" -SHAPE_RECTANGLE, ///< vec3:"extents" -SHAPE_CONVEX_POLYGON, ///< array of planes:"planes" -SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array) -SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error - -*/ class Shape3DSW; @@ -111,11 +100,13 @@ public: class ConcaveShape3DSW : public Shape3DSW { public: - virtual bool is_concave() const { return true; } - typedef void (*Callback)(void *p_userdata, Shape3DSW *p_convex); - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } + virtual bool is_concave() const override { return true; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } - virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; + // Returns true to stop the query. + typedef bool (*QueryCallback)(void *p_userdata, Shape3DSW *p_convex); + + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; ConcaveShape3DSW() {} }; @@ -128,49 +119,49 @@ class PlaneShape3DSW : public Shape3DSW { public: Plane get_plane() const; - virtual real_t get_area() const { return INFINITY; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_PLANE; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } + virtual real_t get_area() const override { return INFINITY; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_PLANE; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; PlaneShape3DSW(); }; -class RayShape3DSW : public Shape3DSW { +class SeparationRayShape3DSW : public Shape3DSW { real_t length; - bool slips_on_slope; + bool slide_on_slope; - void _setup(real_t p_length, bool p_slips_on_slope); + void _setup(real_t p_length, bool p_slide_on_slope); public: real_t get_length() const; - bool get_slips_on_slope() const; + bool get_slide_on_slope() const; - virtual real_t get_area() const { return 0.0; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_RAY; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; + virtual real_t get_area() const override { return 0.0; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SEPARATION_RAY; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; - RayShape3DSW(); + SeparationRayShape3DSW(); }; class SphereShape3DSW : public Shape3DSW { @@ -181,21 +172,21 @@ class SphereShape3DSW : public Shape3DSW { public: real_t get_radius() const; - virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } + virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SPHERE; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SPHERE; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; SphereShape3DSW(); }; @@ -206,21 +197,21 @@ class BoxShape3DSW : public Shape3DSW { public: _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; } - virtual real_t get_area() const { return 8 * half_extents.x * half_extents.y * half_extents.z; } + virtual real_t get_area() const override { return 8 * half_extents.x * half_extents.y * half_extents.z; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_BOX; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_BOX; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; BoxShape3DSW(); }; @@ -235,21 +226,21 @@ public: _FORCE_INLINE_ real_t get_height() const { return height; } _FORCE_INLINE_ real_t get_radius() const { return radius; } - virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CAPSULE; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CAPSULE; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; CapsuleShape3DSW(); }; @@ -264,21 +255,21 @@ public: _FORCE_INLINE_ real_t get_height() const { return height; } _FORCE_INLINE_ real_t get_radius() const { return radius; } - virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CYLINDER; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CYLINDER; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; CylinderShape3DSW(); }; @@ -291,19 +282,19 @@ struct ConvexPolygonShape3DSW : public Shape3DSW { public: const Geometry3D::MeshData &get_mesh() const { return mesh; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; ConvexPolygonShape3DSW(); }; @@ -334,7 +325,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { struct _CullParams { AABB aabb; - Callback callback = nullptr; + QueryCallback callback = nullptr; void *userdata = nullptr; const Face *faces = nullptr; const Vector3 *vertices = nullptr; @@ -360,7 +351,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { bool backface_collision = false; void _cull_segment(int p_idx, _SegmentCullParams *p_params) const; - void _cull(int p_idx, _CullParams *p_params) const; + bool _cull(int p_idx, _CullParams *p_params) const; void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx); @@ -369,32 +360,32 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { public: Vector<Vector3> get_faces() const; - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; ConcavePolygonShape3DSW(); }; struct HeightMapShape3DSW : public ConcaveShape3DSW { - Vector<float> heights; + Vector<real_t> heights; int width = 0; int depth = 0; Vector3 local_origin; - _FORCE_INLINE_ float _get_height(int p_x, int p_z) const { + _FORCE_INLINE_ real_t _get_height(int p_x, int p_z) const { return heights[(p_z * width) + p_x]; } @@ -406,27 +397,27 @@ struct HeightMapShape3DSW : public ConcaveShape3DSW { void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const; - void _setup(const Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); + void _setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); public: - Vector<float> get_heights() const; + Vector<real_t> get_heights() const; int get_width() const; int get_depth() const; - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_HEIGHTMAP; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_HEIGHTMAP; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; HeightMapShape3DSW(); }; @@ -437,21 +428,21 @@ struct FaceShape3DSW : public Shape3DSW { Vector3 vertex[3]; bool backface_collision = false; - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; } - void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data) {} - virtual Variant get_data() const { return Variant(); } + virtual void set_data(const Variant &p_data) override {} + virtual Variant get_data() const override { return Variant(); } FaceShape3DSW(); }; @@ -460,9 +451,9 @@ struct MotionShape3DSW : public Shape3DSW { Shape3DSW *shape; Vector3 motion; - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } - void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override { Vector3 cast = p_transform.basis.xform(motion); real_t mina, maxa; real_t minb, maxb; @@ -474,22 +465,23 @@ struct MotionShape3DSW : public Shape3DSW { r_max = MAX(maxa, maxb); } - Vector3 get_support(const Vector3 &p_normal) const { + virtual Vector3 get_support(const Vector3 &p_normal) const override { Vector3 support = shape->get_support(p_normal); if (p_normal.dot(motion) > 0) { support += motion; } return support; } - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } - bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; } - virtual bool intersect_point(const Vector3 &p_point) const { return false; } - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; } - Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override { return false; } + virtual bool intersect_point(const Vector3 &p_point) const override { return false; } + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override { return p_point; } + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); } - virtual void set_data(const Variant &p_data) {} - virtual Variant get_data() const { return Variant(); } + virtual void set_data(const Variant &p_data) override {} + virtual Variant get_data() const override { return Variant(); } MotionShape3DSW() { configure(AABB()); } }; diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp index 724125bea8..d7e13867bf 100644 --- a/servers/physics_3d/soft_body_3d_sw.cpp +++ b/servers/physics_3d/soft_body_3d_sw.cpp @@ -38,7 +38,7 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -165,7 +165,7 @@ void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_s p_rendering_server_handler->set_aabb(bounds); } -void SoftBody3DSW::update_normals() { +void SoftBody3DSW::update_normals_and_centroids() { uint32_t i, ni; for (i = 0, ni = nodes.size(); i < ni; ++i) { @@ -180,6 +180,7 @@ void SoftBody3DSW::update_normals() { face.n[2]->n += n; face.normal = n; face.normal.normalize(); + face.centroid = 0.33333333333 * (face.n[0]->x + face.n[1]->x + face.n[2]->x); } for (i = 0, ni = nodes.size(); i < ni; ++i) { @@ -310,7 +311,7 @@ void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) { face_tree.clear(); - update_normals(); + update_normals_and_centroids(); update_bounds(); update_constants(); } @@ -574,7 +575,7 @@ bool SoftBody3DSW::create_from_trimesh(const Vector<int> &p_indices, const Vecto reoptimize_link_order(); update_constants(); - update_normals(); + update_normals_and_centroids(); update_bounds(); return true; @@ -898,36 +899,88 @@ void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) { } } -void SoftBody3DSW::apply_forces() { - if (pressure_coefficient < CMP_EPSILON) { - return; - } +void SoftBody3DSW::apply_forces(bool p_has_wind_forces) { + int ac = areas.size(); if (nodes.is_empty()) { return; } uint32_t i, ni; + int32_t j; - // Calculate volume. real_t volume = 0.0; const Vector3 &org = nodes[0].x; + + // Iterate over faces (try not to iterate elsewhere if possible). for (i = 0, ni = faces.size(); i < ni; ++i) { + bool stopped = false; const Face &face = faces[i]; + + Vector3 wind_force(0, 0, 0); + + // Compute volume. volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org)); + + // Compute nodal forces from area winds. + if (ac && p_has_wind_forces) { + const AreaCMP *aa = &areas[0]; + for (j = ac - 1; j >= 0 && !stopped; j--) { + PhysicsServer3D::AreaSpaceOverrideMode mode = aa[j].area->get_space_override_mode(); + switch (mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + wind_force += _compute_area_windforce(aa[j].area, &face); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + wind_force = _compute_area_windforce(aa[j].area, &face); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + + for (j = 0; j < 3; j++) { + Node *current_node = face.n[j]; + current_node->f += wind_force; + } + } } volume /= 6.0; - // Apply per node forces. - real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient; - for (i = 0, ni = nodes.size(); i < ni; ++i) { - Node &node = nodes[i]; - if (node.im > 0) { - node.f += node.n * (node.area * ivolumetp); + // Apply nodal pressure forces. + if (pressure_coefficient > CMP_EPSILON) { + real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + if (node.im > 0) { + node.f += node.n * (node.area * ivolumetp); + } } } } +void SoftBody3DSW::_compute_area_gravity(const Area3DSW *p_area) { + Vector3 area_gravity; + p_area->compute_gravity(get_transform().get_origin(), area_gravity); + gravity += area_gravity; +} + +Vector3 SoftBody3DSW::_compute_area_windforce(const Area3DSW *p_area, const Face *p_face) { + real_t wfm = p_area->get_wind_force_magnitude(); + real_t waf = p_area->get_wind_attenuation_factor(); + const Vector3 &wd = p_area->get_wind_direction(); + const Vector3 &ws = p_area->get_wind_source(); + real_t projection_on_tri_normal = vec3_dot(p_face->normal, wd); + real_t projection_toward_centroid = vec3_dot(p_face->centroid - ws, wd); + real_t attenuation_over_distance = pow(projection_toward_centroid, -waf); + real_t nodal_force_magnitude = wfm * 0.33333333333 * p_face->ra * projection_on_tri_normal * attenuation_over_distance; + return nodal_force_magnitude * p_face->normal; +} + void SoftBody3DSW::predict_motion(real_t p_delta) { const real_t inv_delta = 1.0 / p_delta; @@ -935,11 +988,43 @@ void SoftBody3DSW::predict_motion(real_t p_delta) { Area3DSW *def_area = get_space()->get_default_area(); ERR_FAIL_COND(!def_area); + gravity = def_area->get_gravity_vector() * def_area->get_gravity(); + + int ac = areas.size(); + bool stopped = false; + bool has_wind_forces = false; + + if (ac) { + areas.sort(); + const AreaCMP *aa = &areas[0]; + for (int i = ac - 1; i >= 0 && !stopped; i--) { + // Avoids unnecessary loop in apply_forces(). + has_wind_forces = has_wind_forces || aa[i].area->get_wind_force_magnitude() > CMP_EPSILON; + + PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); + switch (mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + _compute_area_gravity(aa[i].area); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = Vector3(0, 0, 0); + _compute_area_gravity(aa[i].area); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } // Apply forces. - Vector3 gravity = def_area->get_gravity_vector() * def_area->get_gravity(); add_velocity(gravity * p_delta); - apply_forces(); + if (pressure_coefficient > CMP_EPSILON || has_wind_forces) { + apply_forces(has_wind_forces); + } // Avoid soft body from 'exploding' so use some upper threshold of maximum motion // that a node can travel per frame. @@ -1018,7 +1103,7 @@ void SoftBody3DSW::solve_constraints(real_t p_delta) { node.q = node.x; } - update_normals(); + update_normals_and_centroids(); } void SoftBody3DSW::solve_links(real_t kst, real_t ti) { diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h index ac8bcbf0b9..58fd234fde 100644 --- a/servers/physics_3d/soft_body_3d_sw.h +++ b/servers/physics_3d/soft_body_3d_sw.h @@ -31,6 +31,7 @@ #ifndef SOFT_BODY_3D_SW_H #define SOFT_BODY_3D_SW_H +#include "area_3d_sw.h" #include "collision_object_3d_sw.h" #include "core/math/aabb.h" @@ -70,6 +71,7 @@ class SoftBody3DSW : public CollisionObject3DSW { }; struct Face { + Vector3 centroid; Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers Vector3 normal; // Normal real_t ra = 0.0; // Rest area @@ -100,14 +102,21 @@ class SoftBody3DSW : public CollisionObject3DSW { real_t drag_coefficient = 0.0; // [0,1] LocalVector<int> pinned_vertices; + Vector3 gravity; + SelfList<SoftBody3DSW> active_list; Set<Constraint3DSW *> constraints; + Vector<AreaCMP> areas; + VSet<RID> exceptions; uint64_t island_step = 0; + _FORCE_INLINE_ void _compute_area_gravity(const Area3DSW *p_area); + _FORCE_INLINE_ Vector3 _compute_area_windforce(const Area3DSW *p_area, const Face *p_face); + public: SoftBody3DSW(); @@ -129,6 +138,25 @@ public: _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + _FORCE_INLINE_ void add_area(Area3DSW *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount += 1; + } else { + areas.ordered_insert(AreaCMP(p_area)); + } + } + + _FORCE_INLINE_ void remove_area(Area3DSW *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount -= 1; + if (areas[index].refCount < 1) { + areas.remove(index); + } + } + } + virtual void set_space(Space3DSW *p_space); void set_mesh(const Ref<Mesh> &p_mesh); @@ -194,7 +222,7 @@ protected: virtual void _shapes_changed(); private: - void update_normals(); + void update_normals_and_centroids(); void update_bounds(); void update_constants(); void update_area(); @@ -205,7 +233,7 @@ private: void add_velocity(const Vector3 &p_velocity); - void apply_forces(); + void apply_forces(bool p_has_wind_forces); bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices); void generate_bending_constraints(int p_distance); diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 1037243d3b..37dee436df 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -569,158 +569,7 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) { return amount; } -int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin) { - AABB body_aabb; - - bool shapes_found = false; - - for (int i = 0; i < p_body->get_shape_count(); i++) { - if (p_body->is_shape_disabled(i)) { - continue; - } - - if (!shapes_found) { - body_aabb = p_body->get_shape_aabb(i); - shapes_found = true; - } else { - body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); - } - } - - if (!shapes_found) { - return 0; - } - // Undo the currently transform the physics server is aware of and apply the provided one - body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_margin); - - Transform3D body_transform = p_transform; - - for (int i = 0; i < p_result_max; i++) { - //reset results - r_results[i].collision_depth = 0; - } - - int rays_found = 0; - - { - // raycast AND separate - - const int max_results = 32; - int recover_attempts = 4; - Vector3 sr[max_results * 2]; - PhysicsServer3DSW::CollCbkData cbk; - cbk.max = max_results; - PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; - CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; - - do { - Vector3 recover_motion; - - bool collided = false; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_disabled(j)) { - continue; - } - - Shape3DSW *body_shape = p_body->get_shape(j); - - if (body_shape->get_type() != PhysicsServer3D::SHAPE_RAY) { - continue; - } - - Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); - - for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - cbk.amount = 0; - cbk.ptr = sr; - - if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) { - const Body3DSW *b = static_cast<const Body3DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } - - Shape3DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { - if (cbk.amount > 0) { - collided = true; - } - - int ray_index = -1; //reuse shape - for (int k = 0; k < rays_found; k++) { - if (r_results[k].collision_local_shape == j) { - ray_index = k; - } - } - - if (ray_index == -1 && rays_found < p_result_max) { - ray_index = rays_found; - rays_found++; - } - - if (ray_index != -1) { - PhysicsServer3D::SeparationResult &result = r_results[ray_index]; - - for (int k = 0; k < cbk.amount; k++) { - Vector3 a = sr[k * 2 + 0]; - Vector3 b = sr[k * 2 + 1]; - - recover_motion += (b - a) / cbk.amount; - - real_t depth = a.distance_to(b); - if (depth > result.collision_depth) { - result.collision_depth = depth; - result.collision_point = b; - result.collision_normal = (b - a).normalized(); - result.collision_local_shape = j; - result.collider = col_obj->get_self(); - result.collider_id = col_obj->get_instance_id(); - result.collider_shape = shape_idx; - //result.collider_metadata = col_obj->get_shape_metadata(shape_idx); - if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) { - Body3DSW *body = (Body3DSW *)col_obj; - Vector3 rel_vec = b - (body->get_transform().origin + body->get_center_of_mass()); - result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); - } - } - } - } - } - } - } - - if (!collided || recover_motion == Vector3()) { - break; - } - - body_transform.origin += recover_motion; - body_aabb.position += recover_motion; - - recover_attempts--; - } while (recover_attempts); - } - - //optimize results (remove non colliding) - for (int i = 0; i < rays_found; i++) { - if (r_results[i].collision_depth == 0) { - rays_found--; - SWAP(r_results[i], r_results[rays_found]); - } - } - - r_recover_motion = body_transform.origin - p_transform.origin; - return rays_found; -} - -bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes) { +bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) { //give me back regular physics engine logic //this is madness //and most people using this function will think @@ -751,7 +600,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co if (!shapes_found) { if (r_result) { *r_result = PhysicsServer3D::MotionResult(); - r_result->motion = p_motion; + r_result->travel = p_motion; } return false; @@ -795,21 +644,15 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); Shape3DSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { - continue; - } for (int i = 0; i < amount; i++) { const CollisionObject3DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) { - const Body3DSW *b = static_cast<const Body3DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } + if (p_exclude.has(col_obj->get_self())) { + continue; } + int shape_idx = intersection_query_subindex_results[i]; + if (CollisionSolver3DSW::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, p_margin)) { collided = cbk.amount > 0; } @@ -871,13 +714,19 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co continue; } - Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); Shape3DSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { - continue; + // Colliding separation rays allows to properly snap to the ground, + // otherwise it's not needed in regular motion. + if (!p_collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) { + // When slide on slope is on, separation ray shape acts like a regular shape. + if (!static_cast<SeparationRayShape3DSW *>(body_shape)->get_slide_on_slope()) { + continue; + } } + Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); + Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse(); MotionShape3DSW mshape; mshape.shape = body_shape; @@ -890,15 +739,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co for (int i = 0; i < amount; i++) { const CollisionObject3DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) { - const Body3DSW *b = static_cast<const Body3DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } + if (p_exclude.has(col_obj->get_self())) { + continue; } + int shape_idx = intersection_query_subindex_results[i]; + //test initial overlap, does it collide if going all the way? Vector3 point_A, point_B; Vector3 sep_axis = motion_normal; @@ -1005,24 +851,16 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j); Shape3DSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { - continue; - } - body_aabb.position += p_motion * unsafe; int amount = _cull_aabb_for_body(p_body, body_aabb); for (int i = 0; i < amount; i++) { const CollisionObject3DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) { - const Body3DSW *b = static_cast<const Body3DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } + if (p_exclude.has(col_obj->get_self())) { + continue; } + int shape_idx = intersection_query_subindex_results[i]; rcd.object = col_obj; rcd.shape = shape_idx; @@ -1051,9 +889,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass()); r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); - r_result->motion = safe * p_motion; + r_result->travel = safe * p_motion; r_result->remainder = p_motion - safe * p_motion; - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + r_result->travel += (body_transform.get_origin() - p_from.get_origin()); } collided = true; @@ -1061,9 +899,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co } if (!collided && r_result) { - r_result->motion = p_motion; + r_result->travel = p_motion; r_result->remainder = Vector3(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + r_result->travel += (body_transform.get_origin() - p_from.get_origin()); } return collided; @@ -1093,7 +931,9 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A)); return area2_pair; } else if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) { - // Area/Soft Body, not supported. + SoftBody3DSW *softbody = static_cast<SoftBody3DSW *>(B); + AreaSoftBodyPair3DSW *soft_area_pair = memnew(AreaSoftBodyPair3DSW(softbody, p_subindex_B, area, p_subindex_A)); + return soft_area_pair; } else { Body3DSW *body = static_cast<Body3DSW *>(B); AreaPair3DSW *area_pair = memnew(AreaPair3DSW(body, p_subindex_B, area, p_subindex_A)); diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index 18e93c90cc..1c3d1cf9f6 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -203,8 +203,7 @@ public: void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } - int test_body_ray_separation(Body3DSW *p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin); - bool test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes); + bool test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()); Space3DSW(); ~Space3DSW(); |