diff options
Diffstat (limited to 'servers/physics_3d')
46 files changed, 1367 insertions, 1075 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index b6c5b3003c..a9f5c4aec3 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -52,7 +52,7 @@ void Area3DSW::_shapes_changed() { } } -void Area3DSW::set_transform(const Transform &p_transform) { +void Area3DSW::set_transform(const Transform3D &p_transform) { if (!moved_list.in_list() && get_space()) { get_space()->area_add_to_moved_list(&moved_list); } @@ -215,7 +215,9 @@ void Area3DSW::call_queries() { for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) { if (E->get().state == 0) { // Nothing happened - E = E->next(); + Map<BodyKey, BodyState>::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; continue; } @@ -250,7 +252,9 @@ void Area3DSW::call_queries() { for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) { if (E->get().state == 0) { // Nothing happened - E = E->next(); + Map<BodyKey, BodyState>::Element *next = E->next(); + monitored_areas.erase(E); + E = next; continue; } diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h index 8a0a1e963b..12f7545c08 100644 --- a/servers/physics_3d/area_3d_sw.h +++ b/servers/physics_3d/area_3d_sw.h @@ -156,7 +156,7 @@ public: void set_monitorable(bool p_monitorable); _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } - void set_transform(const Transform &p_transform); + void set_transform(const Transform3D &p_transform); void set_space(Space3DSW *p_space); diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp index 4de5f1ba47..2073df7dee 100644 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -40,31 +40,48 @@ bool AreaPair3DSW::setup(real_t p_step) { result = true; } + process_collision = false; if (result != colliding) { - if (result) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - body->add_area(area); - } - if (area->has_monitor_callback()) { - area->add_body_to_query(body, body_shape, area_shape); - } - - } else { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } + 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 false; //never do any post solving + return process_collision; +} + +bool AreaPair3DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_body_to_query(body, body_shape, area_shape); + } + } else { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + + return false; // Never do any post solving. } void AreaPair3DSW::solve(real_t p_step) { + // Nothing to do. } AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape) { @@ -72,7 +89,6 @@ AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, area = p_area; body_shape = p_body_shape; area_shape = p_area_shape; - colliding = false; body->add_constraint(this, 0); area->add_constraint(this); if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { @@ -103,33 +119,48 @@ bool Area2Pair3DSW::setup(real_t p_step) { result = true; } + process_collision = false; if (result != colliding) { - if (result) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - 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()) { - 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); - } + 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()) { + process_collision = true; } colliding = result; } - return false; //never do any post solving + return process_collision; +} + +bool Area2Pair3DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + 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()) { + 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. } void Area2Pair3DSW::solve(real_t p_step) { + // Nothing to do. } Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b) { @@ -137,7 +168,6 @@ Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area area_b = p_area_b; shape_a = p_shape_a; shape_b = p_shape_b; - colliding = false; area_a->add_constraint(this); area_b->add_constraint(this); } diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h index fbdaa25cbb..596d893082 100644 --- a/servers/physics_3d/area_pair_3d_sw.h +++ b/servers/physics_3d/area_pair_3d_sw.h @@ -40,11 +40,13 @@ class AreaPair3DSW : public Constraint3DSW { Area3DSW *area; int body_shape; int area_shape; - bool colliding; + bool colliding = false; + bool process_collision = false; public: - bool setup(real_t p_step); - void solve(real_t p_step); + 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; AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape); ~AreaPair3DSW(); @@ -55,11 +57,13 @@ class Area2Pair3DSW : public Constraint3DSW { Area3DSW *area_b; int shape_a; int shape_b; - bool colliding; + bool colliding = false; + bool process_collision = false; public: - bool setup(real_t p_step); - void solve(real_t p_step); + 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; Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b); ~Area2Pair3DSW(); diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index 64ba0cb09d..ea6064cb4c 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -54,7 +54,7 @@ void Body3DSW::update_inertias() { // Update shapes and motions. switch (mode) { - case PhysicsServer3D::BODY_MODE_RIGID: { + case PhysicsServer3D::BODY_MODE_DYNAMIC: { // Update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet) real_t total_area = 0; @@ -65,16 +65,18 @@ void Body3DSW::update_inertias() { // We have to recompute the center of mass. center_of_mass_local.zero(); - for (int i = 0; i < get_shape_count(); i++) { - real_t area = get_shape_area(i); + if (total_area != 0.0) { + for (int i = 0; i < get_shape_count(); i++) { + real_t area = get_shape_area(i); - real_t mass = area * this->mass / total_area; + real_t mass = area * this->mass / total_area; - // NOTE: we assume that the shape origin is also its center of mass. - center_of_mass_local += mass * get_shape_transform(i).origin; - } + // NOTE: we assume that the shape origin is also its center of mass. + center_of_mass_local += mass * get_shape_transform(i).origin; + } - center_of_mass_local /= mass; + center_of_mass_local /= mass; + } // Recompute the inertia tensor. Basis inertia_tensor; @@ -86,16 +88,19 @@ void Body3DSW::update_inertias() { continue; } + real_t area = get_shape_area(i); + if (area == 0.0) { + continue; + } + inertia_set = true; const Shape3DSW *shape = get_shape(i); - real_t area = get_shape_area(i); - real_t mass = area * this->mass / total_area; Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix(); - Transform shape_transform = get_shape_transform(i); + Transform3D shape_transform = get_shape_transform(i); Basis shape_basis = shape_transform.basis.orthonormalized(); // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor! @@ -127,7 +132,7 @@ void Body3DSW::update_inertias() { _inv_inertia_tensor.set_zero(); _inv_mass = 0; } break; - case PhysicsServer3D::BODY_MODE_CHARACTER: { + case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: { _inv_inertia_tensor.set_zero(); _inv_mass = 1.0 / mass; @@ -145,31 +150,17 @@ void Body3DSW::set_active(bool p_active) { } active = p_active; - if (!p_active) { - if (get_space()) { - get_space()->body_remove_from_active_list(&active_list); - } - } else { + + if (active) { if (mode == PhysicsServer3D::BODY_MODE_STATIC) { - return; //static bodies can't become active - } - if (get_space()) { + // Static bodies can't be active. + active = false; + } else if (get_space()) { get_space()->body_add_to_active_list(&active_list); } - - //still_time=0; - } - /* - if (!space) - return; - - for(int i=0;i<get_shape_count();i++) { - Shape &s=shapes[i]; - if (s.bpid>0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); } -*/ } void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) { @@ -248,13 +239,13 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { } } break; - case PhysicsServer3D::BODY_MODE_RIGID: { + case PhysicsServer3D::BODY_MODE_DYNAMIC: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _set_static(false); set_active(true); } break; - case PhysicsServer3D::BODY_MODE_CHARACTER: { + case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _set_static(false); set_active(true); @@ -295,7 +286,7 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va _set_inv_transform(get_transform().affine_inverse()); wakeup_neighbours(); } else { - Transform t = p_variant; + Transform3D t = p_variant; t.orthonormalize(); new_transform = get_transform(); //used as old to compute motion if (new_transform == t) { @@ -308,24 +299,15 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va } break; case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { - /* - if (mode==PhysicsServer3D::BODY_MODE_STATIC) - break; - */ linear_velocity = p_variant; wakeup(); } break; case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { - /* - if (mode!=PhysicsServer3D::BODY_MODE_RIGID) - break; - */ angular_velocity = p_variant; wakeup(); } break; case PhysicsServer3D::BODY_STATE_SLEEPING: { - //? if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { break; } @@ -342,7 +324,7 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va } break; case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { can_sleep = p_variant; - if (mode == PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep) { + if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) { set_active(true); } @@ -392,13 +374,6 @@ void Body3DSW::set_space(Space3DSW *p_space) { if (active) { get_space()->body_add_to_active_list(&active_list); } - /* - _update_queries(); - if (is_active()) { - active=false; - set_active(true); - } - */ } first_integration = true; @@ -601,7 +576,7 @@ void Body3DSW::integrate_velocities(real_t p_step) { Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; real_t ang_vel = total_angular_velocity.length(); - Transform transform = get_transform(); + Transform3D transform = get_transform(); if (ang_vel != 0.0) { Vector3 ang_vel_axis = total_angular_velocity / ang_vel; @@ -633,8 +608,8 @@ void Body3DSW::integrate_velocities(real_t p_step) { } /* -void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { - Transform inv_xform = p_xform.affine_inverse(); +void BodySW::simulate_motion(const Transform3D& p_xform,real_t p_step) { + Transform3D inv_xform = p_xform.affine_inverse(); if (!get_space()) { _set_transform(p_xform); _set_inv_transform(inv_xform); @@ -675,7 +650,7 @@ void Body3DSW::wakeup_neighbours() { continue; } Body3DSW *b = n[i]; - if (b->mode != PhysicsServer3D::BODY_MODE_RIGID) { + if (b->mode != PhysicsServer3D::BODY_MODE_DYNAMIC) { continue; } @@ -693,24 +668,23 @@ void Body3DSW::call_queries() { Variant v = dbs; - Object *obj = ObjectDB::get_instance(fi_callback->id); + Object *obj = fi_callback->callable.get_object(); if (!obj) { - set_force_integration_callback(ObjectID(), StringName()); + set_force_integration_callback(Callable()); } else { const Variant *vp[2] = { &v, &fi_callback->udata }; Callable::CallError ce; int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2; - obj->call(fi_callback->method, vp, argc, ce); + Variant rv; + fi_callback->callable.call(vp, argc, rv, ce); } } } bool Body3DSW::sleep_test(real_t p_step) { if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { - return true; // - } else if (mode == PhysicsServer3D::BODY_MODE_CHARACTER) { - return !active; // characters don't sleep unless asked to sleep + return true; } else if (!can_sleep) { return false; } @@ -725,44 +699,35 @@ bool Body3DSW::sleep_test(real_t p_step) { } } -void Body3DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { +void Body3DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { if (fi_callback) { memdelete(fi_callback); fi_callback = nullptr; } - if (p_id.is_valid()) { + if (p_callable.get_object()) { fi_callback = memnew(ForceIntegrationCallback); - fi_callback->id = p_id; - fi_callback->method = p_method; + fi_callback->callable = p_callable; fi_callback->udata = p_udata; } } -void Body3DSW::set_kinematic_margin(real_t p_margin) { - kinematic_safe_margin = p_margin; -} - Body3DSW::Body3DSW() : CollisionObject3DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { - mode = PhysicsServer3D::BODY_MODE_RIGID; + mode = PhysicsServer3D::BODY_MODE_DYNAMIC; active = true; mass = 1; - kinematic_safe_margin = 0.001; - //_inv_inertia=Transform(); _inv_mass = 1; bounce = 0; friction = 1; omit_force_integration = false; //applied_torque=0; island_step = 0; - island_next = nullptr; - island_list_next = nullptr; first_time_kinematic = false; first_integration = false; _set_static(false); diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index e87ff2364b..0fa31c5037 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -55,7 +55,6 @@ class Body3DSW : public CollisionObject3DSW { uint16_t locked_axis = 0; - real_t kinematic_safe_margin; real_t _inv_mass; Vector3 _inv_inertia; // Relative to the principal axes of inertia @@ -93,7 +92,7 @@ class Body3DSW : public CollisionObject3DSW { bool first_time_kinematic; void _update_inertia(); virtual void _shapes_changed(); - Transform new_transform; + Transform3D new_transform; Map<Constraint3DSW *, int> constraint_map; @@ -127,16 +126,13 @@ class Body3DSW : public CollisionObject3DSW { int contact_count; struct ForceIntegrationCallback { - ObjectID id; - StringName method; + Callable callable; Variant udata; }; ForceIntegrationCallback *fi_callback; uint64_t island_step; - Body3DSW *island_next; - Body3DSW *island_list_next; _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area3DSW *p_area); @@ -145,10 +141,7 @@ class Body3DSW : public CollisionObject3DSW { friend class PhysicsDirectBodyState3DSW; // i give up, too many functions to expose public: - void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); - - void set_kinematic_margin(real_t p_margin); - _FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; } + void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); _FORCE_INLINE_ void add_area(Area3DSW *p_area) { int index = areas.find(AreaCMP(p_area)); @@ -189,12 +182,6 @@ 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_ Body3DSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Body3DSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ Body3DSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Body3DSW *p_next) { island_list_next = p_next; } - _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraint_map.erase(p_constraint); } const Map<Constraint3DSW *, int> &get_constraint_map() const { return constraint_map; } @@ -320,7 +307,7 @@ public: return p_axis.dot(_inv_inertia_tensor.xform_inv(p_axis)); } - //void simulate_motion(const Transform& p_xform,real_t p_step); + //void simulate_motion(const Transform3D& p_xform,real_t p_step); void call_queries(); void wakeup_neighbours(); @@ -399,8 +386,8 @@ public: virtual void set_angular_velocity(const Vector3 &p_velocity) override { body->set_angular_velocity(p_velocity); } virtual Vector3 get_angular_velocity() const override { return body->get_angular_velocity(); } - virtual void set_transform(const Transform &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); } - virtual Transform get_transform() const override { return body->get_transform(); } + 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 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 { diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index 36114c0c91..aed4815c5e 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -161,7 +161,7 @@ void BodyPair3DSW::validate_contacts() { } } -bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B) { +bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform3D &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform3D &p_xform_B) { Vector3 motion = p_A->get_linear_velocity() * p_step; real_t mlen = motion.length(); if (mlen < CMP_EPSILON) { @@ -184,7 +184,7 @@ bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Vector3 from = p_xform_A.xform(s); Vector3 to = from + motion; - Transform from_inv = p_xform_B.affine_inverse(); + Transform3D from_inv = p_xform_B.affine_inverse(); Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box Vector3 local_to = from_inv.xform(to); @@ -212,14 +212,16 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) { } bool BodyPair3DSW::setup(real_t p_step) { - //cannot collide + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { collided = false; return false; } - bool report_contacts_only = false; - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + report_contacts_only = false; + if (!dynamic_A && !dynamic_B) { if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { report_contacts_only = true; } else { @@ -237,38 +239,48 @@ bool BodyPair3DSW::setup(real_t p_step) { validate_contacts(); - Vector3 offset_A = A->get_transform().get_origin(); - Transform xform_Au = Transform(A->get_transform().basis, Vector3()); - Transform xform_A = xform_Au * A->get_shape_transform(shape_A); + const Vector3 &offset_A = A->get_transform().get_origin(); + Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3()); + Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A); - Transform xform_Bu = B->get_transform(); + Transform3D xform_Bu = B->get_transform(); xform_Bu.origin -= offset_A; - Transform xform_B = xform_Bu * B->get_shape_transform(shape_B); + Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B); Shape3DSW *shape_A_ptr = A->get_shape(shape_A); Shape3DSW *shape_B_ptr = B->get_shape(shape_B); - bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); - this->collided = collided; + collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); if (!collided) { //test ccd (currently just a raycast) - if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) { + if (A->is_continuous_collision_detection_enabled() && dynamic_A && !dynamic_B) { _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); } - if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) { + if (B->is_continuous_collision_detection_enabled() && dynamic_B && !dynamic_A) { _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); } return false; } + return true; +} + +bool BodyPair3DSW::pre_solve(real_t p_step) { + if (!collided) { + return false; + } + real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t bias = (real_t)0.3; + Shape3DSW *shape_A_ptr = A->get_shape(shape_A); + Shape3DSW *shape_B_ptr = B->get_shape(shape_B); + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { if (shape_A_ptr->get_custom_bias() == 0) { bias = shape_B_ptr->get_custom_bias(); @@ -281,22 +293,28 @@ bool BodyPair3DSW::setup(real_t p_step) { real_t inv_dt = 1.0 / p_step; + bool do_process = false; + + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; c.active = false; - Vector3 global_A = xform_Au.xform(c.local_A); - Vector3 global_B = xform_Bu.xform(c.local_B); + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; - real_t depth = c.normal.dot(global_A - global_B); + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); if (depth <= 0) { continue; } #ifdef DEBUG_ENABLED - if (space->is_debugging_contacts()) { + const Vector3 &offset_A = A->get_transform().get_origin(); space->add_debug_contact(global_A + offset_A); space->add_debug_contact(global_B + offset_A); } @@ -323,6 +341,7 @@ bool BodyPair3DSW::setup(real_t p_step) { } c.active = true; + do_process = true; // Precompute normal mass, tangent mass, and bias. Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal)); @@ -335,8 +354,12 @@ bool BodyPair3DSW::setup(real_t p_step) { c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); - B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); + if (dynamic_A) { + A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); + } + if (dynamic_B) { + B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); + } c.acc_bias_impulse = 0; c.acc_bias_impulse_center_of_mass = 0; @@ -350,7 +373,7 @@ bool BodyPair3DSW::setup(real_t p_step) { } } - return true; + return do_process; } void BodyPair3DSW::solve(real_t p_step) { @@ -358,6 +381,8 @@ void BodyPair3DSW::solve(real_t p_step) { return; } + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; if (!c.active) { @@ -381,8 +406,12 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); - B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); + if (dynamic_A) { + A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av); + } + if (dynamic_B) { + B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av); + } crbA = A->get_biased_angular_velocity().cross(c.rA); crbB = B->get_biased_angular_velocity().cross(c.rB); @@ -397,8 +426,12 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); - A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); - B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); + if (dynamic_A) { + A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); + } + if (dynamic_B) { + B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); + } } c.active = true; @@ -418,8 +451,12 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - A->apply_impulse(-j, c.rA + A->get_center_of_mass()); - B->apply_impulse(j, c.rB + B->get_center_of_mass()); + if (dynamic_A) { + A->apply_impulse(-j, c.rA + A->get_center_of_mass()); + } + if (dynamic_B) { + B->apply_impulse(j, c.rB + B->get_center_of_mass()); + } c.active = true; } @@ -461,8 +498,12 @@ void BodyPair3DSW::solve(real_t p_step) { jt = c.acc_tangent_impulse - jtOld; - A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); - B->apply_impulse(jt, c.rB + B->get_center_of_mass()); + if (dynamic_A) { + A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); + } + if (dynamic_B) { + B->apply_impulse(jt, c.rB + B->get_center_of_mass()); + } c.active = true; } @@ -478,8 +519,6 @@ BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_sh space = A->get_space(); A->add_constraint(this, 0); B->add_constraint(this, 1); - contact_count = 0; - collided = false; } BodyPair3DSW::~BodyPair3DSW() { @@ -532,7 +571,7 @@ void BodySoftBodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int void BodySoftBodyPair3DSW::validate_contacts() { // Make sure to erase contacts that are no longer valid. - const Transform &transform_A = body->get_transform(); + const Transform3D &transform_A = body->get_transform(); real_t contact_max_separation = space->get_contact_max_separation(); @@ -561,6 +600,8 @@ void BodySoftBodyPair3DSW::validate_contacts() { } bool BodySoftBodyPair3DSW::setup(real_t p_step) { + body_dynamic = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + if (!body->test_collision_mask(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) { collided = false; return false; @@ -571,29 +612,43 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { return false; } - const Transform &xform_Au = body->get_transform(); - Transform xform_A = xform_Au * body->get_shape_transform(body_shape); + const Transform3D &xform_Au = body->get_transform(); + Transform3D xform_A = xform_Au * body->get_shape_transform(body_shape); - Transform xform_Bu = soft_body->get_transform(); - Transform xform_B = xform_Bu * soft_body->get_shape_transform(0); + Transform3D xform_Bu = soft_body->get_transform(); + Transform3D xform_B = xform_Bu * soft_body->get_shape_transform(0); validate_contacts(); Shape3DSW *shape_A_ptr = body->get_shape(body_shape); Shape3DSW *shape_B_ptr = soft_body->get_shape(0); - bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); - this->collided = collided; + collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); + + return collided; +} + +bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) { + if (!collided) { + return false; + } real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t bias = (real_t)0.3; + + Shape3DSW *shape_A_ptr = body->get_shape(body_shape); + if (shape_A_ptr->get_custom_bias()) { bias = shape_A_ptr->get_custom_bias(); } real_t inv_dt = 1.0 / p_step; + bool do_process = false; + + const Transform3D &transform_A = body->get_transform(); + uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { Contact &c = contacts[contact_index]; @@ -604,16 +659,17 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { continue; } - Vector3 global_A = xform_Au.xform(c.local_A); + Vector3 global_A = transform_A.xform(c.local_A); Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; - - real_t depth = c.normal.dot(global_A - global_B); + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); if (depth <= 0) { continue; } c.active = true; + do_process = true; #ifdef DEBUG_ENABLED @@ -623,7 +679,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { } #endif - c.rA = global_A - xform_Au.origin - body->get_center_of_mass(); + c.rA = global_A - transform_A.origin - body->get_center_of_mass(); c.rB = global_B; if (body->can_report_contacts()) { @@ -631,7 +687,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA); } - if (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { + if (body_dynamic) { body->set_active(true); } @@ -645,7 +701,9 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - body->apply_impulse(c.rA + body->get_center_of_mass(), -j_vec); + if (body_dynamic) { + body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass()); + } soft_body->apply_node_impulse(c.index_B, j_vec); c.acc_bias_impulse = 0; c.acc_bias_impulse_center_of_mass = 0; @@ -661,7 +719,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { } } - return true; + return do_process; } void BodySoftBodyPair3DSW::solve(real_t p_step) { @@ -669,6 +727,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { return; } + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { Contact &c = contacts[contact_index]; @@ -691,7 +751,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - body->apply_bias_impulse(c.rA + body->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step); + if (body_dynamic) { + body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av); + } soft_body->apply_node_bias_impulse(c.index_B, jb); crbA = body->get_biased_angular_velocity().cross(c.rA); @@ -706,8 +768,10 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); - body->apply_bias_impulse(body->get_center_of_mass(), -jb_com, 0.0f); - soft_body->apply_node_bias_impulse(c.index_B, -jb_com); + if (body_dynamic) { + body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f); + } + soft_body->apply_node_bias_impulse(c.index_B, jb_com); } c.active = true; @@ -726,7 +790,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - body->apply_impulse(c.rA + body->get_center_of_mass(), -j); + if (body_dynamic) { + body->apply_impulse(-j, c.rA + body->get_center_of_mass()); + } soft_body->apply_node_impulse(c.index_B, j); c.active = true; @@ -767,7 +833,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { jt = c.acc_tangent_impulse - jtOld; - body->apply_impulse(c.rA + body->get_center_of_mass(), -jt); + if (body_dynamic) { + body->apply_impulse(-jt, c.rA + body->get_center_of_mass()); + } soft_body->apply_node_impulse(c.index_B, jt); c.active = true; @@ -775,7 +843,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { } } -BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) { +BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) : + BodyContact3DSW(&body, 1) { body = p_A; soft_body = p_B; body_shape = p_shape_A; diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 74dddfa6aa..976982d1f1 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -57,9 +57,9 @@ protected: }; Vector3 sep_axis; - bool collided; + bool collided = false; - Space3DSW *space; + Space3DSW *space = nullptr; BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : Constraint3DSW(p_body_ptr, p_body_count) { @@ -77,37 +77,45 @@ class BodyPair3DSW : public BodyContact3DSW { Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = { nullptr, nullptr }; }; - int shape_A; - int shape_B; + int shape_A = 0; + int shape_B = 0; + + bool dynamic_A = false; + bool dynamic_B = false; + + bool report_contacts_only = false; Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection Contact contacts[MAX_CONTACTS]; - int contact_count; + int contact_count = 0; static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B); void validate_contacts(); - bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B); + bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform3D &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform3D &p_xform_B); public: - bool setup(real_t p_step); - void solve(real_t p_step); + 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; BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B); ~BodyPair3DSW(); }; class BodySoftBodyPair3DSW : public BodyContact3DSW { - Body3DSW *body; - SoftBody3DSW *soft_body; + Body3DSW *body = nullptr; + SoftBody3DSW *soft_body = nullptr; - int body_shape; + int body_shape = 0; + + bool body_dynamic = false; LocalVector<Contact> contacts; @@ -118,8 +126,12 @@ class BodySoftBodyPair3DSW : public BodyContact3DSW { void validate_contacts(); public: - bool setup(real_t p_step); - void solve(real_t p_step); + 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; + + virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const override { return soft_body; } + virtual int get_soft_body_count() const override { return 1; } BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B); ~BodySoftBodyPair3DSW(); diff --git a/servers/physics_3d/broad_phase_3d_basic.cpp b/servers/physics_3d/broad_phase_3d_basic.cpp deleted file mode 100644 index b41c2530da..0000000000 --- a/servers/physics_3d/broad_phase_3d_basic.cpp +++ /dev/null @@ -1,212 +0,0 @@ -/*************************************************************************/ -/* broad_phase_3d_basic.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "broad_phase_3d_basic.h" -#include "core/string/print_string.h" -#include "core/templates/list.h" - -BroadPhase3DSW::ID BroadPhase3DBasic::create(CollisionObject3DSW *p_object, int p_subindex) { - ERR_FAIL_COND_V(p_object == nullptr, 0); - - current++; - - Element e; - e.owner = p_object; - e._static = false; - e.subindex = p_subindex; - - element_map[current] = e; - return current; -} - -void BroadPhase3DBasic::move(ID p_id, const AABB &p_aabb) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - E->get().aabb = p_aabb; -} - -void BroadPhase3DBasic::set_static(ID p_id, bool p_static) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - E->get()._static = p_static; -} - -void BroadPhase3DBasic::remove(ID p_id) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - List<PairKey> to_erase; - //unpair must be done immediately on removal to avoid potential invalid pointers - for (Map<PairKey, void *>::Element *F = pair_map.front(); F; F = F->next()) { - if (F->key().a == p_id || F->key().b == p_id) { - if (unpair_callback) { - Element *elem_A = &element_map[F->key().a]; - Element *elem_B = &element_map[F->key().b]; - unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, F->get(), unpair_userdata); - } - to_erase.push_back(F->key()); - } - } - while (to_erase.size()) { - pair_map.erase(to_erase.front()->get()); - to_erase.pop_front(); - } - element_map.erase(E); -} - -CollisionObject3DSW *BroadPhase3DBasic::get_object(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, nullptr); - return E->get().owner; -} - -bool BroadPhase3DBasic::is_static(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, false); - return E->get()._static; -} - -int BroadPhase3DBasic::get_subindex(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, -1); - return E->get().subindex; -} - -int BroadPhase3DBasic::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; - - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const AABB aabb = E->get().aabb; - if (aabb.has_point(p_point)) { - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) { - break; - } - } - } - - return rc; -} - -int BroadPhase3DBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; - - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const AABB aabb = E->get().aabb; - if (aabb.intersects_segment(p_from, p_to)) { - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) { - break; - } - } - } - - return rc; -} - -int BroadPhase3DBasic::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; - - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const AABB aabb = E->get().aabb; - if (aabb.intersects(p_aabb)) { - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) { - break; - } - } - } - - return rc; -} - -void BroadPhase3DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_userdata = p_userdata; - pair_callback = p_pair_callback; -} - -void BroadPhase3DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_userdata = p_userdata; - unpair_callback = p_unpair_callback; -} - -void BroadPhase3DBasic::update() { - // recompute pairs - for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) { - for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) { - Element *elem_A = &I->get(); - Element *elem_B = &J->get(); - - if (elem_A->owner == elem_B->owner) { - continue; - } - - bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static); - - PairKey key(I->key(), J->key()); - - Map<PairKey, void *>::Element *E = pair_map.find(key); - - if (!pair_ok && E) { - if (unpair_callback) { - unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata); - } - pair_map.erase(key); - } - - if (pair_ok && !E) { - void *data = nullptr; - if (pair_callback) { - data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata); - if (data) { - pair_map.insert(key, data); - } - } - } - } - } -} - -BroadPhase3DSW *BroadPhase3DBasic::_create() { - return memnew(BroadPhase3DBasic); -} - -BroadPhase3DBasic::BroadPhase3DBasic() { - current = 1; - unpair_callback = nullptr; - unpair_userdata = nullptr; - pair_callback = nullptr; - pair_userdata = nullptr; -} diff --git a/servers/physics_3d/broad_phase_3d_basic.h b/servers/physics_3d/broad_phase_3d_basic.h deleted file mode 100644 index 54d34e005f..0000000000 --- a/servers/physics_3d/broad_phase_3d_basic.h +++ /dev/null @@ -1,105 +0,0 @@ -/*************************************************************************/ -/* broad_phase_3d_basic.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef BROAD_PHASE_BASIC_H -#define BROAD_PHASE_BASIC_H - -#include "broad_phase_3d_sw.h" -#include "core/templates/map.h" - -class BroadPhase3DBasic : public BroadPhase3DSW { - struct Element { - CollisionObject3DSW *owner; - bool _static; - AABB aabb; - int subindex; - }; - - Map<ID, Element> element_map; - - ID current; - - struct PairKey { - union { - struct { - ID a; - ID b; - }; - uint64_t key; - }; - - _FORCE_INLINE_ bool operator<(const PairKey &p_key) const { - return key < p_key.key; - } - - PairKey() { key = 0; } - PairKey(ID p_a, ID p_b) { - if (p_a > p_b) { - a = p_b; - b = p_a; - } else { - a = p_a; - b = p_b; - } - } - }; - - Map<PairKey, void *> pair_map; - - PairCallback pair_callback; - void *pair_userdata; - UnpairCallback unpair_callback; - void *unpair_userdata; - -public: - // 0 is an invalid ID - virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0); - virtual void move(ID p_id, const AABB &p_aabb); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObject3DSW *get_object(ID p_id) const; - virtual bool is_static(ID p_id) const; - virtual int get_subindex(ID p_id) const; - - virtual int cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); - - virtual void update(); - - static BroadPhase3DSW *_create(); - BroadPhase3DBasic(); -}; - -#endif // BROAD_PHASE_BASIC_H diff --git a/servers/physics_3d/broad_phase_octree.cpp b/servers/physics_3d/broad_phase_3d_bvh.cpp index 11324fa4e4..f9f64f786d 100644 --- a/servers/physics_3d/broad_phase_octree.cpp +++ b/servers/physics_3d/broad_phase_3d_bvh.cpp @@ -1,5 +1,5 @@ /*************************************************************************/ -/* broad_phase_octree.cpp */ +/* broad_phase_3d_bvh.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,55 +28,55 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "broad_phase_octree.h" +#include "broad_phase_3d_bvh.h" #include "collision_object_3d_sw.h" -BroadPhase3DSW::ID BroadPhaseOctree::create(CollisionObject3DSW *p_object, int p_subindex) { - ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0); - return oid; +BroadPhase3DBVH::ID BroadPhase3DBVH::create(CollisionObject3DSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) { + ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? + return oid + 1; } -void BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) { - octree.move(p_id, p_aabb); +void BroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) { + bvh.move(p_id - 1, p_aabb); } -void BroadPhaseOctree::set_static(ID p_id, bool p_static) { - CollisionObject3DSW *it = octree.get(p_id); - octree.set_pairable(p_id, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1? +void BroadPhase3DBVH::set_static(ID p_id, bool p_static) { + CollisionObject3DSW *it = bvh.get(p_id - 1); + bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? } -void BroadPhaseOctree::remove(ID p_id) { - octree.erase(p_id); +void BroadPhase3DBVH::remove(ID p_id) { + bvh.erase(p_id - 1); } -CollisionObject3DSW *BroadPhaseOctree::get_object(ID p_id) const { - CollisionObject3DSW *it = octree.get(p_id); +CollisionObject3DSW *BroadPhase3DBVH::get_object(ID p_id) const { + CollisionObject3DSW *it = bvh.get(p_id - 1); ERR_FAIL_COND_V(!it, nullptr); return it; } -bool BroadPhaseOctree::is_static(ID p_id) const { - return !octree.is_pairable(p_id); +bool BroadPhase3DBVH::is_static(ID p_id) const { + return !bvh.is_pairable(p_id - 1); } -int BroadPhaseOctree::get_subindex(ID p_id) const { - return octree.get_subindex(p_id); +int BroadPhase3DBVH::get_subindex(ID p_id) const { + return bvh.get_subindex(p_id - 1); } -int BroadPhaseOctree::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - return octree.cull_point(p_point, p_results, p_max_results, p_result_indices); +int BroadPhase3DBVH::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices); } -int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); +int BroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); } -int BroadPhaseOctree::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - return octree.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); +int BroadPhase3DBVH::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); } -void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B) { - BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); +void *BroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B) { + BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self); if (!bpo->pair_callback) { return nullptr; } @@ -84,8 +84,8 @@ void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, Collisio return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); } -void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) { - BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); +void BroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) { + BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self); if (!bpo->unpair_callback) { return; } @@ -93,27 +93,27 @@ void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, Collisi bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); } -void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { +void BroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { pair_callback = p_pair_callback; pair_userdata = p_userdata; } -void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { +void BroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { unpair_callback = p_unpair_callback; unpair_userdata = p_userdata; } -void BroadPhaseOctree::update() { - // does.. not? +void BroadPhase3DBVH::update() { + bvh.update(); } -BroadPhase3DSW *BroadPhaseOctree::_create() { - return memnew(BroadPhaseOctree); +BroadPhase3DSW *BroadPhase3DBVH::_create() { + return memnew(BroadPhase3DBVH); } -BroadPhaseOctree::BroadPhaseOctree() { - octree.set_pair_callback(_pair_callback, this); - octree.set_unpair_callback(_unpair_callback, this); +BroadPhase3DBVH::BroadPhase3DBVH() { + bvh.set_pair_callback(_pair_callback, this); + bvh.set_unpair_callback(_unpair_callback, this); pair_callback = nullptr; pair_userdata = nullptr; unpair_userdata = nullptr; diff --git a/servers/physics_3d/broad_phase_octree.h b/servers/physics_3d/broad_phase_3d_bvh.h index ee681dda96..30b8b7f2aa 100644 --- a/servers/physics_3d/broad_phase_octree.h +++ b/servers/physics_3d/broad_phase_3d_bvh.h @@ -1,5 +1,5 @@ /*************************************************************************/ -/* broad_phase_octree.h */ +/* broad_phase_3d_bvh.h */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,17 +28,17 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef BROAD_PHASE_OCTREE_H -#define BROAD_PHASE_OCTREE_H +#ifndef BROAD_PHASE_3D_BVH_H +#define BROAD_PHASE_3D_BVH_H #include "broad_phase_3d_sw.h" -#include "core/math/octree.h" +#include "core/math/bvh.h" -class BroadPhaseOctree : public BroadPhase3DSW { - Octree<CollisionObject3DSW, true> octree; +class BroadPhase3DBVH : public BroadPhase3DSW { + BVH_Manager<CollisionObject3DSW, true, 128> bvh; - static void *_pair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int); - static void _unpair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int, void *); + static void *_pair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int); + static void _unpair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int, void *); PairCallback pair_callback; void *pair_userdata; @@ -47,7 +47,7 @@ class BroadPhaseOctree : public BroadPhase3DSW { public: // 0 is an invalid ID - virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0); + virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false); virtual void move(ID p_id, const AABB &p_aabb); virtual void set_static(ID p_id, bool p_static); virtual void remove(ID p_id); @@ -66,7 +66,7 @@ public: virtual void update(); static BroadPhase3DSW *_create(); - BroadPhaseOctree(); + BroadPhase3DBVH(); }; -#endif // BROAD_PHASE_OCTREE_H +#endif // BROAD_PHASE_3D_BVH_H diff --git a/servers/physics_3d/broad_phase_3d_sw.h b/servers/physics_3d/broad_phase_3d_sw.h index 283c087b96..98313cb216 100644 --- a/servers/physics_3d/broad_phase_3d_sw.h +++ b/servers/physics_3d/broad_phase_3d_sw.h @@ -48,7 +48,7 @@ public: typedef void (*UnpairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_userdata); // 0 is an invalid ID - virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0) = 0; + virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0; virtual void move(ID p_id, const AABB &p_aabb) = 0; virtual void set_static(ID p_id, bool p_static) = 0; virtual void remove(ID p_id) = 0; diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp index 293a7e6606..51e2432071 100644 --- a/servers/physics_3d/collision_object_3d_sw.cpp +++ b/servers/physics_3d/collision_object_3d_sw.cpp @@ -32,7 +32,7 @@ #include "servers/physics_3d/physics_server_3d_sw.h" #include "space_3d_sw.h" -void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform &p_transform, bool p_disabled) { +void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform3D &p_transform, bool p_disabled) { Shape s; s.shape = p_shape; s.xform = p_transform; @@ -62,7 +62,7 @@ void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) { //_shapes_changed(); } -void CollisionObject3DSW::set_shape_transform(int p_index, const Transform &p_transform) { +void CollisionObject3DSW::set_shape_transform(int p_index, const Transform3D &p_transform) { ERR_FAIL_INDEX(p_index, shapes.size()); shapes.write[p_index].xform = p_transform; @@ -146,22 +146,23 @@ void CollisionObject3DSW::_update_shapes() { for (int i = 0; i < shapes.size(); i++) { Shape &s = shapes.write[i]; - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i); - space->get_broadphase()->set_static(s.bpid, _static); - } //not quite correct, should compute the next matrix.. AABB shape_aabb = s.shape->get_aabb(); - Transform xform = transform * s.xform; + Transform3D xform = transform * s.xform; shape_aabb = xform.xform(shape_aabb); + shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); s.aabb_cache = shape_aabb; - s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); Vector3 scale = xform.get_basis().get_scale(); s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z; - space->get_broadphase()->move(s.bpid, s.aabb_cache); + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); } } @@ -172,18 +173,19 @@ void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) { for (int i = 0; i < shapes.size(); i++) { Shape &s = shapes.write[i]; - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i); - space->get_broadphase()->set_static(s.bpid, _static); - } //not quite correct, should compute the next matrix.. AABB shape_aabb = s.shape->get_aabb(); - Transform xform = transform * s.xform; + Transform3D xform = transform * s.xform; shape_aabb = xform.xform(shape_aabb); - shape_aabb = shape_aabb.merge(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion + shape_aabb.merge_with(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion s.aabb_cache = shape_aabb; + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + space->get_broadphase()->move(s.bpid, shape_aabb); } } diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index 85221b7746..5505fec3da 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -60,8 +60,8 @@ private: uint32_t collision_mask; struct Shape { - Transform xform; - Transform xform_inv; + Transform3D xform; + Transform3D xform_inv; BroadPhase3DSW::ID bpid; AABB aabb_cache; //for rayqueries real_t area_cache; @@ -73,8 +73,8 @@ private: Vector<Shape> shapes; Space3DSW *space; - Transform transform; - Transform inv_transform; + Transform3D transform; + Transform3D inv_transform; bool _static; SelfList<CollisionObject3DSW> pending_shape_update_list; @@ -85,7 +85,7 @@ protected: void _update_shapes_with_motion(const Vector3 &p_motion); void _unregister_shapes(); - _FORCE_INLINE_ void _set_transform(const Transform &p_transform, bool p_update_shapes = true) { + _FORCE_INLINE_ void _set_transform(const Transform3D &p_transform, bool p_update_shapes = true) { #ifdef DEBUG_ENABLED ERR_FAIL_COND_MSG(p_transform.origin.length_squared() > MAX_OBJECT_DISTANCE_X2, "Object went too far away (more than '" + itos(MAX_OBJECT_DISTANCE) + "' units from origin)."); @@ -96,7 +96,7 @@ protected: _update_shapes(); } } - _FORCE_INLINE_ void _set_inv_transform(const Transform &p_transform) { inv_transform = p_transform; } + _FORCE_INLINE_ void _set_inv_transform(const Transform3D &p_transform) { inv_transform = p_transform; } void _set_static(bool p_static); virtual void _shapes_changed() = 0; @@ -116,22 +116,22 @@ public: void _shape_changed(); _FORCE_INLINE_ Type get_type() const { return type; } - void add_shape(Shape3DSW *p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); + void add_shape(Shape3DSW *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false); void set_shape(int p_index, Shape3DSW *p_shape); - void set_shape_transform(int p_index, const Transform &p_transform); + void set_shape_transform(int p_index, const Transform3D &p_transform); _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } _FORCE_INLINE_ bool is_shape_disabled(int p_index) const { CRASH_BAD_INDEX(p_index, shapes.size()); return shapes[p_index].disabled; } _FORCE_INLINE_ Shape3DSW *get_shape(int p_index) const { return shapes[p_index].shape; } - _FORCE_INLINE_ const Transform &get_shape_transform(int p_index) const { return shapes[p_index].xform; } - _FORCE_INLINE_ const Transform &get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } + _FORCE_INLINE_ const Transform3D &get_shape_transform(int p_index) const { return shapes[p_index].xform; } + _FORCE_INLINE_ const Transform3D &get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } _FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } _FORCE_INLINE_ real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; } - _FORCE_INLINE_ const Transform &get_transform() const { return transform; } - _FORCE_INLINE_ const Transform &get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ const Transform3D &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform3D &get_inv_transform() const { return inv_transform; } _FORCE_INLINE_ Space3DSW *get_space() const { return space; } _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; } diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index 9d5448dbfa..b362f1ff17 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -608,8 +608,8 @@ template <class ShapeA, class ShapeB, bool withMargin = false> class SeparatorAxisTest { const ShapeA *shape_A; const ShapeB *shape_B; - const Transform *transform_A; - const Transform *transform_B; + const Transform3D *transform_A; + const Transform3D *transform_B; real_t best_depth; Vector3 best_axis; _CollectorCallback *callback; @@ -690,7 +690,7 @@ public: Vector3 axis = (p_point_B - p_point_A); real_t depth = axis.length(); - // Filter out bogus directions with a treshold and re-testing axis. + // Filter out bogus directions with a threshold and re-testing axis. if (separator->best_depth - depth > 0.001) { separator->test_axis(axis / depth); } @@ -750,7 +750,7 @@ public: callback->collided = true; } - _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform &p_transform_A, const ShapeB *p_shape_B, const Transform &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { + _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform3D &p_transform_A, const ShapeB *p_shape_B, const Transform3D &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { best_depth = 1e15; shape_A = p_shape_A; shape_B = p_shape_B; @@ -764,10 +764,10 @@ public: /****** SAT TESTS *******/ -typedef void (*CollisionFunc)(const Shape3DSW *, const Transform &, const Shape3DSW *, const Transform &, _CollectorCallback *p_callback, real_t, real_t); +typedef void (*CollisionFunc)(const Shape3DSW *, const Transform3D &, const Shape3DSW *, const Transform3D &, _CollectorCallback *p_callback, real_t, real_t); template <bool withMargin> -static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const SphereShape3DSW *sphere_B = static_cast<const SphereShape3DSW *>(p_b); @@ -787,7 +787,7 @@ static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_tr } template <bool withMargin> -static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_box(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); @@ -838,7 +838,7 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans } template <bool withMargin> -static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); @@ -880,7 +880,7 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t } template <bool withMargin> -static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); @@ -939,7 +939,7 @@ static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform &p_ } template <bool withMargin> -static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -999,7 +999,7 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo } template <bool withMargin> -static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -1044,7 +1044,7 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_tran } template <bool withMargin> -static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_box_box(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); @@ -1142,7 +1142,7 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor } template <bool withMargin> -static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); @@ -1240,7 +1240,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran } template <bool withMargin> -static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); @@ -1353,7 +1353,7 @@ static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform &p_tra } template <bool withMargin> -static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -1468,7 +1468,7 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform } template <bool withMargin> -static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_box_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -1591,7 +1591,7 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo } template <bool withMargin> -static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); @@ -1655,7 +1655,7 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_ } template <bool withMargin> -static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); @@ -1717,7 +1717,7 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p } template <bool withMargin> -static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -1781,7 +1781,7 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf } template <bool withMargin> -static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -1855,7 +1855,7 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra } template <bool withMargin> -static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); @@ -1909,7 +1909,7 @@ static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform & } template <bool withMargin> -static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -1926,7 +1926,7 @@ static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Trans } template <bool withMargin> -static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -2031,7 +2031,7 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr } template <bool withMargin> -static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -2140,7 +2140,7 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const } template <bool withMargin> -static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -2258,7 +2258,7 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform separator.generate_contacts(); } -bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { +bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_PLANE, false); @@ -2358,8 +2358,8 @@ bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_tr const Shape3DSW *A = p_shape_A; const Shape3DSW *B = p_shape_B; - const Transform *transform_A = &p_transform_A; - const Transform *transform_B = &p_transform_B; + const Transform3D *transform_A = &p_transform_A; + const Transform3D *transform_B = &p_transform_B; real_t margin_A = p_margin_a; real_t margin_B = p_margin_b; diff --git a/servers/physics_3d/collision_solver_3d_sat.h b/servers/physics_3d/collision_solver_3d_sat.h index 97454c0b4a..e50da7b101 100644 --- a/servers/physics_3d/collision_solver_3d_sat.h +++ b/servers/physics_3d/collision_solver_3d_sat.h @@ -33,6 +33,6 @@ #include "collision_solver_3d_sw.h" -bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); +bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); #endif // COLLISION_SOLVER_SAT_H diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index f655c4626c..67330d497e 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -37,7 +37,7 @@ #define collision_solver sat_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool CollisionSolver3DSW::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) { const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { return false; @@ -89,14 +89,14 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T return found; } -bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +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); Vector3 from = p_transform_A.origin; Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length(); Vector3 support_A = to; - Transform ai = p_transform_B.affine_inverse(); + Transform3D ai = p_transform_B.affine_inverse(); from = ai.xform(from); to = ai.xform(to); @@ -146,8 +146,8 @@ struct _SoftBodyQueryInfo { SoftBody3DSW *soft_body = nullptr; const Shape3DSW *shape_A = nullptr; const Shape3DSW *shape_B = nullptr; - Transform transform_A; - Transform node_transform; + Transform3D transform_A; + Transform3D node_transform; _SoftBodyContactCollisionInfo contact_info; #ifdef DEBUG_ENABLED int node_query_count = 0; @@ -160,7 +160,7 @@ bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void * Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index); - Transform transform_B; + Transform3D transform_B; transform_B.origin = query_cinfo.node_transform.xform(node_position); query_cinfo.contact_info.node_index = p_node_index; @@ -201,11 +201,11 @@ void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW #endif } -bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +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) { const SoftBodyShape3DSW *soft_body_shape_B = static_cast<const SoftBodyShape3DSW *>(p_shape_B); SoftBody3DSW *soft_body = soft_body_shape_B->get_soft_body(); - const Transform &world_to_local = soft_body->get_inv_transform(); + const Transform3D &world_to_local = soft_body->get_inv_transform(); const real_t collision_margin = soft_body->get_collision_margin(); @@ -257,9 +257,9 @@ bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Tran } struct _ConcaveCollisionInfo { - const Transform *transform_A; + const Transform3D *transform_A; const Shape3DSW *shape_A; - const Transform *transform_B; + const Transform3D *transform_B; CollisionSolver3DSW::CallbackResult result_callback; void *userdata; bool swap_result; @@ -285,7 +285,7 @@ void CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex cinfo.collisions++; } -bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { +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) { const ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(p_shape_B); _ConcaveCollisionInfo cinfo; @@ -302,7 +302,7 @@ bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transf cinfo.aabb_tests = 0; - Transform rel_transform = p_transform_A; + Transform3D rel_transform = p_transform_A; rel_transform.origin -= p_transform_B.origin; //quickly compute a local AABB @@ -329,7 +329,7 @@ bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transf return cinfo.collided; } -bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { +bool CollisionSolver3DSW::solve_static(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, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); bool concave_A = p_shape_A->is_concave(); @@ -421,7 +421,7 @@ void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW cinfo.collisions++; } -bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { +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) { const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { return false; @@ -473,7 +473,7 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const return collided; } -bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) { +bool CollisionSolver3DSW::solve_distance(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, const AABB &p_concave_hint, Vector3 *r_sep_axis) { if (p_shape_A->is_concave()) { return false; } @@ -504,7 +504,7 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans cinfo.aabb_tests = 0; cinfo.tested = false; - Transform rel_transform = p_transform_A; + Transform3D rel_transform = p_transform_A; rel_transform.origin -= p_transform_B.origin; //quickly compute a local AABB diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h index 34ac2c6d3f..a5dd7d48eb 100644 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -42,16 +42,16 @@ private: static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); static void soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex); static void concave_callback(void *p_userdata, Shape3DSW *p_convex); - static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_concave(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &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 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_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 solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); + 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: - static bool solve_static(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr); + static bool solve_static(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, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_distance(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, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr); }; #endif // COLLISION_SOLVER__SW_H diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h index 2571335c43..7b44726ef5 100644 --- a/servers/physics_3d/constraint_3d_sw.h +++ b/servers/physics_3d/constraint_3d_sw.h @@ -31,14 +31,13 @@ #ifndef CONSTRAINT_SW_H #define CONSTRAINT_SW_H -#include "body_3d_sw.h" +class Body3DSW; +class SoftBody3DSW; class Constraint3DSW { Body3DSW **_body_ptr; int _body_count; uint64_t island_step; - Constraint3DSW *island_next; - Constraint3DSW *island_list_next; int priority; bool disabled_collisions_between_bodies; @@ -60,15 +59,12 @@ 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_ Constraint3DSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Constraint3DSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ Constraint3DSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Constraint3DSW *p_next) { island_list_next = p_next; } - _FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; } _FORCE_INLINE_ int get_body_count() const { return _body_count; } + virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const { return nullptr; } + virtual int get_soft_body_count() const { return 0; } + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ int get_priority() const { return priority; } @@ -76,6 +72,7 @@ public: _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } virtual bool setup(real_t p_step) = 0; + virtual bool pre_solve(real_t p_step) = 0; virtual void solve(real_t p_step) = 0; virtual ~Constraint3DSW() {} diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index 1a8c7f704f..2df991563d 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -107,16 +107,16 @@ typedef unsigned char U1; struct MinkowskiDiff { const Shape3DSW* m_shapes[2]; - Transform transform_A; - Transform transform_B; + Transform3D transform_A; + Transform3D transform_B; real_t margin_A = 0.0; real_t margin_B = 0.0; Vector3 (*get_support)(const Shape3DSW*, const Vector3&, real_t); - void Initialize(const Shape3DSW* shape0, const Transform& wtrs0, const real_t margin0, - const Shape3DSW* shape1, const Transform& wtrs1, const real_t margin1) { + void Initialize(const Shape3DSW* shape0, const Transform3D& wtrs0, const real_t margin0, + const Shape3DSW* shape1, const Transform3D& wtrs1, const real_t margin1) { m_shapes[0] = shape0; m_shapes[1] = shape1; transform_A = wtrs0; @@ -862,8 +862,8 @@ struct GJK }; // - static void Initialize( const Shape3DSW* shape0, const Transform& wtrs0, real_t margin0, - const Shape3DSW* shape1, const Transform& wtrs1, real_t margin1, + static void Initialize( const Shape3DSW* shape0, const Transform3D& wtrs0, real_t margin0, + const Shape3DSW* shape1, const Transform3D& wtrs1, real_t margin1, sResults& results, tShape& shape) { @@ -885,10 +885,10 @@ struct GJK // bool Distance( const Shape3DSW* shape0, - const Transform& wtrs0, + const Transform3D& wtrs0, real_t margin0, const Shape3DSW* shape1, - const Transform& wtrs1, + const Transform3D& wtrs1, real_t margin1, const Vector3& guess, sResults& results) @@ -926,10 +926,10 @@ bool Distance( const Shape3DSW* shape0, // bool Penetration( const Shape3DSW* shape0, - const Transform& wtrs0, + const Transform3D& wtrs0, real_t margin0, const Shape3DSW* shape1, - const Transform& wtrs1, + const Transform3D& wtrs1, real_t margin1, const Vector3& guess, sResults& results @@ -993,7 +993,7 @@ bool Penetration( const Shape3DSW* shape0, /* clang-format on */ -bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { +bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { GjkEpa2::sResults res; if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) { @@ -1005,7 +1005,7 @@ bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_t return false; } -bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) { +bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) { GjkEpa2::sResults res; if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) { diff --git a/servers/physics_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h index a7e2e1719e..69e85d2bc0 100644 --- a/servers/physics_3d/gjk_epa.h +++ b/servers/physics_3d/gjk_epa.h @@ -34,7 +34,7 @@ #include "collision_solver_3d_sw.h" #include "shape_3d_sw.h" -bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); -bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); +bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); +bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); #endif diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp index 9c4493f4a2..7315e9c709 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -84,7 +84,7 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { return (y < 0.0f) ? -angle : angle; } -ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) : +ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) : Joint3DSW(_arr, 2) { A = rbA; B = rbB; @@ -109,6 +109,13 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans } bool ConeTwistJoint3DSW::setup(real_t p_timestep) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + m_appliedImpulse = real_t(0.); //set bias, sign, clear accumulator @@ -204,7 +211,7 @@ bool ConeTwistJoint3DSW::setup(real_t p_timestep) { // Twist limits if (m_twistSpan >= real_t(0.)) { Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1)); - Quat rotationArc = Quat(b2Axis1, b1Axis1); + Quaternion rotationArc = Quaternion(b2Axis1, b1Axis1); Vector3 TwistRef = rotationArc.xform(b2Axis22); real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); @@ -261,8 +268,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } } } @@ -283,8 +294,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { Vector3 impulse = m_swingAxis * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } } // solve twist limit @@ -299,8 +314,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { Vector3 impulse = m_twistAxis * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } } } } diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h index 4e4d4e7f0c..608847352c 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h @@ -73,8 +73,8 @@ public: JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints real_t m_appliedImpulse; - Transform m_rbAFrame; - Transform m_rbBFrame; + Transform3D m_rbAFrame; + Transform3D m_rbBFrame; real_t m_limitSoftness; real_t m_biasFactor; @@ -102,12 +102,12 @@ public: bool m_solveSwingLimit; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; - ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame); + ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame); void setAngularOnly(bool angularOnly) { m_angularOnly = angularOnly; 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 13b389251f..56aba24b42 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -82,7 +82,7 @@ int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) { real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( real_t timeStep, Vector3 &axis, real_t jacDiagABInv, - Body3DSW *body0, Body3DSW *body1) { + Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) { if (!needApplyTorques()) { return 0.0f; } @@ -138,8 +138,10 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( Vector3 motorImp = clippedMotorImpulse * axis; - body0->apply_torque_impulse(motorImp); - if (body1) { + if (p_body0_dynamic) { + body0->apply_torque_impulse(motorImp); + } + if (body1 && p_body1_dynamic) { body1->apply_torque_impulse(-motorImp); } @@ -154,6 +156,7 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( real_t jacDiagABInv, Body3DSW *body1, const Vector3 &pointInA, Body3DSW *body2, const Vector3 &pointInB, + bool p_body1_dynamic, bool p_body2_dynamic, int limit_index, const Vector3 &axis_normal_on_a, const Vector3 &anchorPos) { @@ -205,14 +208,18 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; Vector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1->apply_impulse(impulse_vector, rel_pos1); - body2->apply_impulse(-impulse_vector, rel_pos2); + if (p_body1_dynamic) { + body1->apply_impulse(impulse_vector, rel_pos1); + } + if (p_body2_dynamic) { + body2->apply_impulse(-impulse_vector, rel_pos2); + } return normalImpulse; } //////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// -Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) : +Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) : Joint3DSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB), @@ -303,6 +310,13 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) { } bool Generic6DOFJoint3DSW::setup(real_t p_timestep) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + // Clear accumulated impulses for the next simulation step m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.)); int i; @@ -380,6 +394,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) { jacDiagABInv, A, pointInA, B, pointInB, + dynamic_A, dynamic_B, i, linear_axis, m_AnchorPos); } } @@ -394,7 +409,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) { angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); - m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B); + m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B); } } } 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 d61a033231..d46437e782 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -81,7 +81,7 @@ public: //! temp_variables //!@{ - real_t m_currentLimitError; //! How much is violated this limit + real_t m_currentLimitError; //!< How much is violated this limit int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit real_t m_accumulatedImpulse; //!@} @@ -113,14 +113,14 @@ public: return (m_enableMotor || m_currentLimit != 0); } - //! calculates error + //! calculates error /*! calculates m_currentLimit and m_currentLimitError. */ int testLimitValue(real_t test_value); //! apply the correction impulses for two bodies - real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1); + real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic); }; class G6DOFTranslationalLimitMotor3DSW { @@ -166,6 +166,7 @@ public: real_t jacDiagABInv, Body3DSW *body1, const Vector3 &pointInA, Body3DSW *body2, const Vector3 &pointInB, + bool p_body1_dynamic, bool p_body2_dynamic, int limit_index, const Vector3 &axis_normal_on_a, const Vector3 &anchorPos); @@ -184,8 +185,8 @@ protected: //! relative_frames //!@{ - Transform m_frameInA; //!< the constraint space w.r.t body A - Transform m_frameInB; //!< the constraint space w.r.t body B + Transform3D m_frameInA; //!< the constraint space w.r.t body A + Transform3D m_frameInB; //!< the constraint space w.r.t body B //!@} //! Jacobians @@ -208,8 +209,8 @@ protected: //! temporal variables //!@{ real_t m_timeStep; - Transform m_calculatedTransformA; - Transform m_calculatedTransformB; + Transform3D m_calculatedTransformA; + Transform3D m_calculatedTransformB; Vector3 m_calculatedAxisAngleDiff; Vector3 m_calculatedAxis[3]; @@ -232,12 +233,12 @@ protected: void calculateAngleInfo(); public: - Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); + Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA); - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_6DOF; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; } - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; //! Calcs global transform of the offsets /*! @@ -250,7 +251,7 @@ public: /*! \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. */ - const Transform &getCalculatedTransformA() const { + const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; } @@ -258,23 +259,23 @@ public: /*! \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. */ - const Transform &getCalculatedTransformB() const { + const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; } - const Transform &getFrameOffsetA() const { + const Transform3D &getFrameOffsetA() const { return m_frameInA; } - const Transform &getFrameOffsetB() const { + const Transform3D &getFrameOffsetB() const { return m_frameInB; } - Transform &getFrameOffsetA() { + Transform3D &getFrameOffsetA() { return m_frameInA; } - Transform &getFrameOffsetB() { + Transform3D &getFrameOffsetB() { return m_frameInB; } @@ -326,7 +327,7 @@ public: return &m_angularLimits[index]; } - //! Retrieves the limit informacion + //! Retrieves the limit informacion G6DOFTranslationalLimitMotor3DSW *getTranslationalLimitMotor() { return &m_linearLimits; } diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index 2b9f0038b4..b928f18231 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -67,7 +67,7 @@ static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { } } -HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameA, const Transform &frameB) : +HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB) : Joint3DSW(_arr, 2) { A = rbA; B = rbB; @@ -126,7 +126,7 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo rbAxisA1.y, rbAxisA2.y, axisInA.y, rbAxisA1.z, rbAxisA2.z, axisInA.z); - Quat rotationArc = Quat(axisInA, axisInB); + Quaternion rotationArc = Quaternion(axisInA, axisInB); Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1); Vector3 rbAxisB2 = axisInB.cross(rbAxisB1); @@ -155,6 +155,13 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo } bool HingeJoint3DSW::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + m_appliedImpulse = real_t(0.); if (!m_angularOnly) { @@ -275,8 +282,12 @@ void HingeJoint3DSW::solve(real_t p_step) { real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } } } @@ -318,8 +329,12 @@ void HingeJoint3DSW::solve(real_t p_step) { angularError *= (real_t(1.) / denom2) * relaxation; } - A->apply_torque_impulse(-velrelOrthog + angularError); - B->apply_torque_impulse(velrelOrthog - angularError); + if (dynamic_A) { + A->apply_torque_impulse(-velrelOrthog + angularError); + } + if (dynamic_B) { + B->apply_torque_impulse(velrelOrthog - angularError); + } // solve limit if (m_solveLimit) { @@ -333,8 +348,12 @@ void HingeJoint3DSW::solve(real_t p_step) { impulseMag = m_accLimitImpulse - temp; Vector3 impulse = axisA * impulseMag * m_limitSign; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } } } @@ -355,8 +374,12 @@ void HingeJoint3DSW::solve(real_t p_step) { clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; Vector3 motorImp = clippedMotorImpulse * axisA; - A->apply_torque_impulse(motorImp + angularLimit); - B->apply_torque_impulse(-motorImp - angularLimit); + if (dynamic_A) { + A->apply_torque_impulse(motorImp + angularLimit); + } + if (dynamic_B) { + B->apply_torque_impulse(-motorImp - angularLimit); + } } } } diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index b6117aa0bc..22eb2f4660 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -66,8 +66,8 @@ class HingeJoint3DSW : public Joint3DSW { JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints JacobianEntry3DSW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - Transform m_rbAFrame; // constraint axii. Assumes z is hinge axis. - Transform m_rbBFrame; + Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis. + Transform3D m_rbBFrame; real_t m_motorTargetVelocity; real_t m_maxMotorImpulse; @@ -96,10 +96,10 @@ class HingeJoint3DSW : public Joint3DSW { real_t m_appliedImpulse; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_HINGE; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; real_t get_hinge_angle(); @@ -109,7 +109,7 @@ public: void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value); bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const; - HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameA, const Transform &frameB); + HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB); HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); }; diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/jacobian_entry_3d_sw.h index 2829a5caf7..6afa70c816 100644 --- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h +++ b/servers/physics_3d/joints/jacobian_entry_3d_sw.h @@ -50,7 +50,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#include "core/math/transform.h" +#include "core/math/transform_3d.h" class JacobianEntry3DSW { public: diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index 9f708ce151..8eb84d1c2f 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -50,6 +50,13 @@ subject to the following restrictions: #include "pin_joint_3d_sw.h" bool PinJoint3DSW::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + m_appliedImpulse = real_t(0.); Vector3 normal(0, 0, 0); @@ -119,8 +126,12 @@ void PinJoint3DSW::solve(real_t p_step) { m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } normal[i] = 0; } diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h index 1875983527..3d91452850 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -74,10 +74,10 @@ class PinJoint3DSW : public Joint3DSW { Vector3 m_pivotInB; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_PIN; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value); real_t get_param(PhysicsServer3D::PinJointParam p_param) const; diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index 0adc471797..db9bdb2986 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -111,7 +111,7 @@ void SliderJoint3DSW::initParams() { //----------------------------------------------------------------------------- -SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB) : +SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB) : Joint3DSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB) { @@ -127,6 +127,13 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform & //----------------------------------------------------------------------------- bool SliderJoint3DSW::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + //calculate transforms m_calculatedTransformA = A->get_transform() * m_frameInA; m_calculatedTransformB = B->get_transform() * m_frameInB; @@ -196,8 +203,12 @@ void SliderJoint3DSW::solve(real_t p_step) { // calcutate and apply impulse real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; Vector3 impulse_vector = normal * normalImpulse; - A->apply_impulse(impulse_vector, m_relPosA); - B->apply_impulse(-impulse_vector, m_relPosB); + if (dynamic_A) { + A->apply_impulse(impulse_vector, m_relPosA); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, m_relPosB); + } if (m_poweredLinMotor && (!i)) { // apply linear motor if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { real_t desiredMotorVel = m_targetLinMotorVelocity; @@ -217,8 +228,12 @@ void SliderJoint3DSW::solve(real_t p_step) { m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; - A->apply_impulse(impulse_vector, m_relPosA); - B->apply_impulse(-impulse_vector, m_relPosB); + if (dynamic_A) { + A->apply_impulse(impulse_vector, m_relPosA); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, m_relPosB); + } } } } @@ -252,8 +267,12 @@ void SliderJoint3DSW::solve(real_t p_step) { angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; } // apply impulse - A->apply_torque_impulse(-velrelOrthog + angularError); - B->apply_torque_impulse(velrelOrthog - angularError); + if (dynamic_A) { + A->apply_torque_impulse(-velrelOrthog + angularError); + } + if (dynamic_B) { + B->apply_torque_impulse(velrelOrthog - angularError); + } real_t impulseMag; //solve angular limits if (m_solveAngLim) { @@ -264,8 +283,12 @@ void SliderJoint3DSW::solve(real_t p_step) { impulseMag *= m_kAngle * m_softnessDirAng; } Vector3 impulse = axisA * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } //apply angular motor if (m_poweredAngMotor) { if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { @@ -290,8 +313,12 @@ void SliderJoint3DSW::solve(real_t p_step) { m_accumulatedAngMotorImpulse = new_acc; // apply clamped impulse Vector3 motorImp = angImpulse * axisA; - A->apply_torque_impulse(motorImp); - B->apply_torque_impulse(-motorImp); + if (dynamic_A) { + A->apply_torque_impulse(motorImp); + } + if (dynamic_B) { + B->apply_torque_impulse(-motorImp); + } } } } // SliderJointSW::solveConstraint() diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index f52f6ace27..f357bbd67a 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -76,8 +76,8 @@ protected: Body3DSW *_arr[2]; }; - Transform m_frameInA; - Transform m_frameInB; + Transform3D m_frameInA; + Transform3D m_frameInB; // linear limits real_t m_lowerLinLimit; @@ -120,8 +120,8 @@ protected: JacobianEntry3DSW m_jacAng[3]; real_t m_timeStep; - Transform m_calculatedTransformA; - Transform m_calculatedTransformB; + Transform3D m_calculatedTransformA; + Transform3D m_calculatedTransformB; Vector3 m_sliderAxis; Vector3 m_realPivotAInW; @@ -152,19 +152,19 @@ protected: public: // constructors - SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB); + SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB); //SliderJointSW(); // overrides // access const Body3DSW *getRigidBodyA() const { return A; } const Body3DSW *getRigidBodyB() const { return B; } - const Transform &getCalculatedTransformA() const { return m_calculatedTransformA; } - const Transform &getCalculatedTransformB() const { return m_calculatedTransformB; } - const Transform &getFrameOffsetA() const { return m_frameInA; } - const Transform &getFrameOffsetB() const { return m_frameInB; } - Transform &getFrameOffsetA() { return m_frameInA; } - Transform &getFrameOffsetB() { return m_frameInB; } + const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; } + const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; } + const Transform3D &getFrameOffsetA() const { return m_frameInA; } + const Transform3D &getFrameOffsetB() const { return m_frameInB; } + Transform3D &getFrameOffsetA() { return m_frameInA; } + Transform3D &getFrameOffsetB() { return m_frameInB; } real_t getLowerLinLimit() { return m_lowerLinLimit; } void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; } real_t getUpperLinLimit() { return m_upperLinLimit; } @@ -240,10 +240,10 @@ public: void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value); real_t get_param(PhysicsServer3D::SliderJointParam p_param) const; - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_SLIDER; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; } }; #endif // SLIDER_JOINT_SW_H diff --git a/servers/physics_3d/joints_3d_sw.h b/servers/physics_3d/joints_3d_sw.h index 225a71aca9..e2514674ea 100644 --- a/servers/physics_3d/joints_3d_sw.h +++ b/servers/physics_3d/joints_3d_sw.h @@ -35,9 +35,14 @@ #include "constraint_3d_sw.h" class Joint3DSW : public Constraint3DSW { +protected: + bool dynamic_A = false; + bool dynamic_B = false; + public: - virtual bool setup(real_t p_step) { return false; } - virtual void solve(real_t p_step) {} + virtual bool setup(real_t p_step) override { return false; } + virtual bool pre_solve(real_t p_step) override { return true; } + virtual void solve(real_t p_step) override {} void copy_settings_from(Joint3DSW *p_joint) { set_self(p_joint->get_self()); diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index 3d0063b0fa..7a95a8abc8 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -30,8 +30,7 @@ #include "physics_server_3d_sw.h" -#include "broad_phase_3d_basic.h" -#include "broad_phase_octree.h" +#include "broad_phase_3d_bvh.h" #include "core/debugger/engine_debugger.h" #include "core/os/os.h" #include "joints/cone_twist_joint_3d_sw.h" @@ -263,7 +262,7 @@ PhysicsServer3D::AreaSpaceOverrideMode PhysicsServer3DSW::area_get_space_overrid return area->get_space_override_mode(); } -void PhysicsServer3DSW::area_add_shape(RID p_area, RID p_shape, const Transform &p_transform, bool p_disabled) { +void PhysicsServer3DSW::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -284,7 +283,7 @@ void PhysicsServer3DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) area->set_shape(p_shape_idx, shape); } -void PhysicsServer3DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) { +void PhysicsServer3DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -308,9 +307,9 @@ RID PhysicsServer3DSW::area_get_shape(RID p_area, int p_shape_idx) const { return shape->get_self(); } -Transform PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { +Transform3D PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { Area3DSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, Transform()); + ERR_FAIL_COND_V(!area, Transform3D()); return area->get_shape_transform(p_shape_idx); } @@ -369,7 +368,7 @@ void PhysicsServer3DSW::area_set_param(RID p_area, AreaParameter p_param, const area->set_param(p_param, p_value); }; -void PhysicsServer3DSW::area_set_transform(RID p_area, const Transform &p_transform) { +void PhysicsServer3DSW::area_set_transform(RID p_area, const Transform3D &p_transform) { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); area->set_transform(p_transform); @@ -386,9 +385,9 @@ Variant PhysicsServer3DSW::area_get_param(RID p_area, AreaParameter p_param) con return area->get_param(p_param); }; -Transform PhysicsServer3DSW::area_get_transform(RID p_area) const { +Transform3D PhysicsServer3DSW::area_get_transform(RID p_area) const { Area3DSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, Transform()); + ERR_FAIL_COND_V(!area, Transform3D()); return area->get_transform(); }; @@ -488,7 +487,7 @@ PhysicsServer3D::BodyMode PhysicsServer3DSW::body_get_mode(RID p_body) const { return body->get_mode(); }; -void PhysicsServer3DSW::body_add_shape(RID p_body, RID p_shape, const Transform &p_transform, bool p_disabled) { +void PhysicsServer3DSW::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -508,8 +507,7 @@ void PhysicsServer3DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) body->set_shape(p_shape_idx, shape); } - -void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) { +void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -542,9 +540,9 @@ void PhysicsServer3DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo body->set_shape_as_disabled(p_shape_idx, p_disabled); } -Transform PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { +Transform3D PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, Transform()); + ERR_FAIL_COND_V(!body, Transform3D()); return body->get_shape_transform(p_shape_idx); } @@ -658,19 +656,6 @@ real_t PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) cons return body->get_param(p_param); }; -void PhysicsServer3DSW::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) { - Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - body->set_kinematic_margin(p_margin); -} - -real_t PhysicsServer3DSW::body_get_kinematic_safe_margin(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_kinematic_margin(); -} - void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -857,10 +842,10 @@ int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } -void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) { +void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); - body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method, p_udata); + body->set_force_integration_callback(p_callable, p_udata); } void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { @@ -869,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 Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) { +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) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -877,10 +862,10 @@ bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform &p_from, co _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result, p_exclude_raycast_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 Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin) { +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); @@ -1011,7 +996,7 @@ Variant PhysicsServer3DSW::soft_body_get_state(RID p_body, BodyState p_state) co return soft_body->get_state(p_state); } -void PhysicsServer3DSW::soft_body_set_transform(RID p_body, const Transform &p_transform) { +void PhysicsServer3DSW::soft_body_set_transform(RID p_body, const Transform3D &p_transform) { SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); ERR_FAIL_COND(!soft_body); @@ -1254,7 +1239,7 @@ Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { return pin_joint->get_position_b(); } -void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { +void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND(!body_A); @@ -1379,7 +1364,7 @@ PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) cons return joint->get_type(); } -void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND(!body_A); @@ -1419,7 +1404,7 @@ real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p return slider_joint->get_param(p_param); } -void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND(!body_A); @@ -1459,7 +1444,7 @@ real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJoint return cone_twist_joint->get_param(p_param); } -void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND(!body_A); @@ -1755,7 +1740,8 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, PhysicsServer3DSW *PhysicsServer3DSW::singletonsw = nullptr; PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) { singletonsw = this; - BroadPhase3DSW::create_func = BroadPhaseOctree::_create; + BroadPhase3DSW::create_func = BroadPhase3DBVH::_create; + island_count = 0; active_objects = 0; collision_pairs = 0; diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index f92652bfad..57b6385758 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -130,13 +130,13 @@ public: virtual void area_set_space(RID p_area, RID p_space) override; virtual RID area_get_space(RID p_area) const override; - virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false) override; + virtual void area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override; virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override; - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) override; + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) override; virtual int area_get_shape_count(RID p_area) const override; virtual RID area_get_shape(RID p_area, int p_shape_idx) const override; - virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const override; + virtual Transform3D area_get_shape_transform(RID p_area, int p_shape_idx) const override; virtual void area_remove_shape(RID p_area, int p_shape_idx) override; virtual void area_clear_shapes(RID p_area) override; @@ -147,10 +147,10 @@ public: virtual ObjectID area_get_object_instance_id(RID p_area) const override; virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override; - virtual void area_set_transform(RID p_area, const Transform &p_transform) override; + virtual void area_set_transform(RID p_area, const Transform3D &p_transform) override; virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; - virtual Transform area_get_transform(RID p_area) const override; + virtual Transform3D area_get_transform(RID p_area) const override; virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; @@ -173,13 +173,13 @@ public: virtual void body_set_mode(RID p_body, BodyMode p_mode) override; virtual BodyMode body_get_mode(RID p_body) const override; - virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false) override; + virtual void body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override; virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override; - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) override; + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) override; virtual int body_get_shape_count(RID p_body) const override; virtual RID body_get_shape(RID p_body, int p_shape_idx) const override; - virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const override; + virtual Transform3D body_get_shape_transform(RID p_body, int p_shape_idx) const override; virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; @@ -204,9 +204,6 @@ public: virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) override; virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override; - virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin) override; - virtual real_t body_get_kinematic_safe_margin(RID p_body) const override; - virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; virtual Variant body_get_state(RID p_body, BodyState p_state) const override; @@ -241,12 +238,12 @@ public: virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; virtual int body_get_max_contacts_reported(RID p_body) const override; - virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) override; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override; - virtual int body_test_ray_separation(RID p_body, const Transform &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, 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; // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; @@ -273,7 +270,7 @@ public: virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override; - virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) override; + virtual void soft_body_set_transform(RID p_body, const Transform3D &p_transform) override; virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override; @@ -323,7 +320,7 @@ public: virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) override; virtual Vector3 pin_joint_get_local_b(RID p_joint) const override; - virtual void joint_make_hinge(RID p_joint, RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) override; + virtual void joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) override; virtual void joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) override; virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) override; @@ -332,17 +329,17 @@ public: virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) override; virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const override; - virtual void joint_make_slider(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A + virtual void joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) override; virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const override; - virtual void joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A + virtual void joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) override; virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const override; - virtual void joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A + virtual void joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value) override; virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) const override; diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.cpp b/servers/physics_3d/physics_server_3d_wrap_mt.cpp index f73f67a756..0a89c1a9c9 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.cpp +++ b/servers/physics_3d/physics_server_3d_wrap_mt.cpp @@ -56,7 +56,7 @@ void PhysicsServer3DWrapMT::thread_loop() { step_thread_up = true; while (!exit) { // flush commands one by one, until exit is requested - command_queue.wait_and_flush_one(); + command_queue.wait_and_flush(); } command_queue.flush_all(); // flush all diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h index 49ae60db92..bda2e30dd1 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -142,14 +142,14 @@ public: FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode); FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID); - FUNC4(area_add_shape, RID, RID, const Transform &, bool); + FUNC4(area_add_shape, RID, RID, const Transform3D &, bool); FUNC3(area_set_shape, RID, int, RID); - FUNC3(area_set_shape_transform, RID, int, const Transform &); + FUNC3(area_set_shape_transform, RID, int, const Transform3D &); FUNC3(area_set_shape_disabled, RID, int, bool); FUNC1RC(int, area_get_shape_count, RID); FUNC2RC(RID, area_get_shape, RID, int); - FUNC2RC(Transform, area_get_shape_transform, RID, int); + FUNC2RC(Transform3D, area_get_shape_transform, RID, int); FUNC2(area_remove_shape, RID, int); FUNC1(area_clear_shapes, RID); @@ -157,10 +157,10 @@ public: FUNC1RC(ObjectID, area_get_object_instance_id, RID); FUNC3(area_set_param, RID, AreaParameter, const Variant &); - FUNC2(area_set_transform, RID, const Transform &); + FUNC2(area_set_transform, RID, const Transform3D &); FUNC2RC(Variant, area_get_param, RID, AreaParameter); - FUNC1RC(Transform, area_get_transform, RID); + FUNC1RC(Transform3D, area_get_transform, RID); FUNC2(area_set_collision_mask, RID, uint32_t); FUNC2(area_set_collision_layer, RID, uint32_t); @@ -182,12 +182,12 @@ public: FUNC2(body_set_mode, RID, BodyMode); FUNC1RC(BodyMode, body_get_mode, RID); - FUNC4(body_add_shape, RID, RID, const Transform &, bool); + FUNC4(body_add_shape, RID, RID, const Transform3D &, bool); FUNC3(body_set_shape, RID, int, RID); - FUNC3(body_set_shape_transform, RID, int, const Transform &); + FUNC3(body_set_shape_transform, RID, int, const Transform3D &); FUNC1RC(int, body_get_shape_count, RID); - FUNC2RC(Transform, body_get_shape_transform, RID, int); + FUNC2RC(Transform3D, body_get_shape_transform, RID, int); FUNC2RC(RID, body_get_shape, RID, int); FUNC3(body_set_shape_disabled, RID, int, bool); @@ -213,9 +213,6 @@ public: FUNC3(body_set_param, RID, BodyParameter, real_t); FUNC2RC(real_t, body_get_param, RID, BodyParameter); - FUNC2(body_set_kinematic_safe_margin, RID, real_t); - FUNC1RC(real_t, body_get_kinematic_safe_margin, RID); - FUNC3(body_set_state, RID, BodyState, const Variant &); FUNC2RC(Variant, body_get_state, RID, BodyState); @@ -249,16 +246,16 @@ public: FUNC2(body_set_omit_force_integration, RID, bool); FUNC1RC(bool, body_is_omitting_force_integration, RID); - FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, const Variant &); + FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); FUNC2(body_set_ray_pickable, RID, bool); - bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, 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, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) 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, r_result, p_exclude_raycast_shapes); + 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 Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override { + 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); } @@ -293,7 +290,7 @@ public: FUNC3(soft_body_set_state, RID, BodyState, const Variant &); FUNC2RC(Variant, soft_body_get_state, RID, BodyState); - FUNC2(soft_body_set_transform, RID, const Transform &); + FUNC2(soft_body_set_transform, RID, const Transform3D &); FUNC2(soft_body_set_simulation_precision, RID, int); FUNC1RC(int, soft_body_get_simulation_precision, RID); @@ -341,7 +338,7 @@ public: FUNC2(pin_joint_set_local_b, RID, const Vector3 &) FUNC1RC(Vector3, pin_joint_get_local_b, RID) - FUNC5(joint_make_hinge, RID, RID, const Transform &, RID, const Transform &) + FUNC5(joint_make_hinge, RID, RID, const Transform3D &, RID, const Transform3D &) FUNC7(joint_make_hinge_simple, RID, RID, const Vector3 &, const Vector3 &, RID, const Vector3 &, const Vector3 &) FUNC3(hinge_joint_set_param, RID, HingeJointParam, real_t) @@ -350,17 +347,17 @@ public: FUNC3(hinge_joint_set_flag, RID, HingeJointFlag, bool) FUNC2RC(bool, hinge_joint_get_flag, RID, HingeJointFlag) - FUNC5(joint_make_slider, RID, RID, const Transform &, RID, const Transform &) + FUNC5(joint_make_slider, RID, RID, const Transform3D &, RID, const Transform3D &) FUNC3(slider_joint_set_param, RID, SliderJointParam, real_t) FUNC2RC(real_t, slider_joint_get_param, RID, SliderJointParam) - FUNC5(joint_make_cone_twist, RID, RID, const Transform &, RID, const Transform &) + FUNC5(joint_make_cone_twist, RID, RID, const Transform3D &, RID, const Transform3D &) FUNC3(cone_twist_joint_set_param, RID, ConeTwistJointParam, real_t) FUNC2RC(real_t, cone_twist_joint_get_param, RID, ConeTwistJointParam) - FUNC5(joint_make_generic_6dof, RID, RID, const Transform &, RID, const Transform &) + FUNC5(joint_make_generic_6dof, RID, RID, const Transform3D &, RID, const Transform3D &) FUNC4(generic_6dof_joint_set_param, RID, Vector3::Axis, G6DOFJointAxisParam, real_t) FUNC3RC(real_t, generic_6dof_joint_get_param, RID, Vector3::Axis, G6DOFJointAxisParam) diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index 687a7b288f..2ffab0c923 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -30,10 +30,28 @@ #include "shape_3d_sw.h" +#include "core/io/image.h" +#include "core/math/convex_hull.h" #include "core/math/geometry_3d.h" -#include "core/math/quick_hull.h" #include "core/templates/sort_array.h" +// HeightMapShape3DSW is based on Bullet btHeightfieldTerrainShape. + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://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. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 @@ -96,7 +114,7 @@ Plane PlaneShape3DSW::get_plane() const { return plane; } -void PlaneShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void PlaneShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { // gibberish, a plane is infinity r_min = -1e7; r_max = 1e7; @@ -156,7 +174,7 @@ bool RayShape3DSW::get_slips_on_slope() const { return slips_on_slope; } -void RayShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void RayShape3DSW::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; @@ -237,7 +255,7 @@ real_t SphereShape3DSW::get_radius() const { return radius; } -void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { real_t d = p_normal.dot(p_transform.origin); // figure out scale at point @@ -299,7 +317,7 @@ SphereShape3DSW::SphereShape3DSW() { /********** BOX *************/ -void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { // no matter the angle, the box is mirrored anyway Vector3 local_normal = p_transform.basis.xform_inv(p_normal); @@ -489,7 +507,7 @@ BoxShape3DSW::BoxShape3DSW() { /********** CAPSULE *************/ -void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +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; @@ -627,7 +645,7 @@ Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const { return Vector3( (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); } void CapsuleShape3DSW::_setup(real_t p_height, real_t p_radius) { @@ -656,7 +674,7 @@ CapsuleShape3DSW::CapsuleShape3DSW() { /********** CYLINDER *************/ -void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized(); real_t axis_dot = cylinder_axis.dot(p_normal); @@ -807,7 +825,7 @@ Vector3 CylinderShape3DSW::get_moment_of_inertia(real_t p_mass) const { return Vector3( (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); } void CylinderShape3DSW::_setup(real_t p_height, real_t p_radius) { @@ -836,7 +854,7 @@ CylinderShape3DSW::CylinderShape3DSW() { /********** CONVEX POLYGON *************/ -void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { int vertex_count = mesh.vertices.size(); if (vertex_count == 0) { return; @@ -891,6 +909,9 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve const Vector3 *vertices = mesh.vertices.ptr(); int vc = mesh.vertices.size(); + r_amount = 0; + ERR_FAIL_COND_MSG(vc == 0, "Convex polygon shape has no vertices."); + //find vertex first real_t max = 0; int vtx = 0; @@ -1064,13 +1085,13 @@ Vector3 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const { return Vector3( (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); } void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) { - Error err = QuickHull::build(p_vertices, mesh); + Error err = ConvexHullComputer::convex_hull(p_vertices, mesh); if (err != OK) { - ERR_PRINT("Failed to build QuickHull"); + ERR_PRINT("Failed to build convex hull"); } AABB _aabb; @@ -1099,7 +1120,7 @@ ConvexPolygonShape3DSW::ConvexPolygonShape3DSW() { /********** FACE POLYGON *************/ -void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { for (int i = 0; i < 3; i++) { Vector3 v = p_transform.xform(vertex[i]); real_t d = p_normal.dot(v); @@ -1229,7 +1250,7 @@ Vector<Vector3> ConcavePolygonShape3DSW::get_faces() const { return rfaces; } -void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { int count = vertices.size(); if (count == 0) { r_min = 0; @@ -1424,7 +1445,7 @@ Vector3 ConcavePolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const { return Vector3( (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); } struct _VolumeSW_BVH_Element { @@ -1614,7 +1635,7 @@ ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() { /* HEIGHT MAP SHAPE */ -Vector<real_t> HeightMapShape3DSW::get_heights() const { +Vector<float> HeightMapShape3DSW::get_heights() const { return heights; } @@ -1626,11 +1647,7 @@ int HeightMapShape3DSW::get_depth() const { return depth; } -real_t HeightMapShape3DSW::get_cell_size() const { - return cell_size; -} - -void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { //not very useful, but not very used either p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max); } @@ -1640,7 +1657,198 @@ Vector3 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const { return get_aabb().get_support(p_normal); } +struct _HeightmapSegmentCullParams { + Vector3 from; + Vector3 to; + Vector3 dir; + + Vector3 result; + Vector3 normal; + + const HeightMapShape3DSW *heightmap = nullptr; + FaceShape3DSW *face = nullptr; +}; + +_FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) { + Vector3 res; + Vector3 normal; + if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal)) { + p_params.result = res; + p_params.normal = normal; + return true; + } + + return false; +} + +_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, int p_x, int p_z) { + // First triangle. + p_params.heightmap->_get_point(p_x, p_z, p_params.face->vertex[0]); + p_params.heightmap->_get_point(p_x + 1, p_z, p_params.face->vertex[1]); + p_params.heightmap->_get_point(p_x, p_z + 1, p_params.face->vertex[2]); + p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal; + if (_heightmap_face_cull_segment(p_params)) { + return true; + } + + // Second triangle. + p_params.face->vertex[0] = p_params.face->vertex[1]; + p_params.heightmap->_get_point(p_x + 1, p_z + 1, p_params.face->vertex[1]); + p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal; + if (_heightmap_face_cull_segment(p_params)) { + return true; + } + + return false; +} + bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { + if (heights.is_empty()) { + return false; + } + + Vector3 local_begin = p_begin + local_origin; + Vector3 local_end = p_end + local_origin; + + FaceShape3DSW face; + face.backface_collision = false; + + _HeightmapSegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = (p_end - p_begin).normalized(); + params.heightmap = this; + params.face = &face; + + // Quantize the ray begin/end. + int begin_x = floor(local_begin.x); + int begin_z = floor(local_begin.z); + int end_x = floor(local_end.x); + int end_z = floor(local_end.z); + + if ((begin_x == end_x) && (begin_z == end_z)) { + // Simple case for rays that don't traverse the grid horizontally. + // Just perform a test on the given cell. + int x = CLAMP(begin_x, 0, width - 2); + int z = CLAMP(begin_z, 0, depth - 2); + if (_heightmap_cell_cull_segment(params, x, z)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + } else { + // Perform grid query from projected ray. + Vector2 ray_dir_proj(local_end.x - local_begin.x, local_end.z - local_begin.z); + real_t ray_dist_proj = ray_dir_proj.length(); + + if (ray_dist_proj < CMP_EPSILON) { + ray_dir_proj = Vector2(); + } else { + ray_dir_proj /= ray_dist_proj; + } + + const int x_step = (ray_dir_proj.x > CMP_EPSILON) ? 1 : ((ray_dir_proj.x < -CMP_EPSILON) ? -1 : 0); + const int z_step = (ray_dir_proj.y > CMP_EPSILON) ? 1 : ((ray_dir_proj.y < -CMP_EPSILON) ? -1 : 0); + + const real_t infinite = 1e20; + const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_proj.x) : infinite; + const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_proj.y) : infinite; + + real_t cross_x; // At which value of `param` we will cross a x-axis lane? + real_t cross_z; // At which value of `param` we will cross a z-axis lane? + + // X initialization. + if (x_step != 0) { + if (x_step == 1) { + cross_x = (ceil(local_begin.x) - local_begin.x) * delta_x; + } else { + cross_x = (local_begin.x - floor(local_begin.x)) * delta_x; + } + } else { + cross_x = infinite; // Will never cross on X. + } + + // Z initialization. + if (z_step != 0) { + if (z_step == 1) { + cross_z = (ceil(local_begin.z) - local_begin.z) * delta_z; + } else { + cross_z = (local_begin.z - floor(local_begin.z)) * delta_z; + } + } else { + cross_z = infinite; // Will never cross on Z. + } + + int x = floor(local_begin.x); + int z = floor(local_begin.z); + + // Workaround cases where the ray starts at an integer position. + if (Math::abs(cross_x) < CMP_EPSILON) { + cross_x += delta_x; + // If going backwards, we should ignore the position we would get by the above flooring, + // because the ray is not heading in that direction. + if (x_step == -1) { + x -= 1; + } + } + + if (Math::abs(cross_z) < CMP_EPSILON) { + cross_z += delta_z; + if (z_step == -1) { + z -= 1; + } + } + + // Start inside the grid. + int x_start = CLAMP(x, 0, width - 2); + int z_start = CLAMP(z, 0, depth - 2); + + // Adjust initial cross values. + cross_x += delta_x * x_step * (x_start - x); + cross_z += delta_z * z_step * (z_start - z); + + x = x_start; + z = z_start; + + if (_heightmap_cell_cull_segment(params, x, z)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + + real_t dist = 0.0; + while (true) { + if (cross_x < cross_z) { + // X lane. + x += x_step; + // Assign before advancing the param, + // to be in sync with the initialization step. + dist = cross_x; + cross_x += delta_x; + } else { + // Z lane. + z += z_step; + dist = cross_z; + cross_z += delta_z; + } + + // Stop when outside the grid. + if ((x < 0) || (z < 0) || (x >= width - 1) || (z >= depth - 1)) { + break; + } + + if (_heightmap_cell_cull_segment(params, x, z)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + + if (dist > ray_dist_proj) { + break; + } + } + } + return false; } @@ -1652,7 +1860,66 @@ Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const { return Vector3(); } +void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const { + const AABB &aabb = get_aabb(); + + Vector3 pos_local = aabb.position + local_origin; + + Vector3 clamped_point(p_point); + clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x); + clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y); + clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z); + + r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5); + r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5); + 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 { + if (heights.is_empty()) { + return; + } + + AABB local_aabb = p_local_aabb; + local_aabb.position += local_origin; + + // Quantize the aabb, and adjust the start/end ranges. + int aabb_min[3]; + int aabb_max[3]; + _get_cell(local_aabb.position, aabb_min[0], aabb_min[1], aabb_min[2]); + _get_cell(local_aabb.position + local_aabb.size, aabb_max[0], aabb_max[1], aabb_max[2]); + + // Expand the min/max quantized values. + // This is to catch the case where the input aabb falls between grid points. + for (int i = 0; i < 3; ++i) { + aabb_min[i]--; + aabb_max[i]++; + } + + int start_x = MAX(0, aabb_min[0]); + int end_x = MIN(width - 1, aabb_max[0]); + int start_z = MAX(0, aabb_min[2]); + int end_z = MIN(depth - 1, aabb_max[2]); + + FaceShape3DSW face; + face.backface_collision = true; + + for (int z = start_z; z < end_z; z++) { + for (int x = start_x; x < end_x; x++) { + // First triangle. + _get_point(x, z, face.vertex[0]); + _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); + + // 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); + } + } } Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const { @@ -1662,61 +1929,105 @@ Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const { return Vector3( (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); } -void HeightMapShape3DSW::_setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) { +void HeightMapShape3DSW::_setup(const Vector<float> &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; - cell_size = p_cell_size; - - const real_t *r = heights.ptr(); + // Initialize aabb. AABB aabb; + aabb.position = Vector3(0.0, p_min_height, 0.0); + aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1); - for (int i = 0; i < depth; i++) { - for (int j = 0; j < width; j++) { - real_t h = r[i * width + j]; + // Initialize origin as the aabb center. + local_origin = aabb.position + 0.5 * aabb.size; + local_origin.y = 0.0; - Vector3 pos(j * cell_size, h, i * cell_size); - if (i == 0 || j == 0) { - aabb.position = pos; - } else { - aabb.expand_to(pos); - } - } - } + aabb.position -= local_origin; configure(aabb); } void HeightMapShape3DSW::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + Dictionary d = p_data; ERR_FAIL_COND(!d.has("width")); ERR_FAIL_COND(!d.has("depth")); - ERR_FAIL_COND(!d.has("cell_size")); ERR_FAIL_COND(!d.has("heights")); int width = d["width"]; int depth = d["depth"]; - real_t cell_size = d["cell_size"]; - Vector<real_t> heights = d["heights"]; - ERR_FAIL_COND(width <= 0); - ERR_FAIL_COND(depth <= 0); - ERR_FAIL_COND(cell_size <= CMP_EPSILON); - ERR_FAIL_COND(heights.size() != (width * depth)); - _setup(heights, width, depth, cell_size); + ERR_FAIL_COND(width <= 0.0); + ERR_FAIL_COND(depth <= 0.0); + + Variant heights_variant = d["heights"]; + Vector<float> heights_buffer; + if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) { + // Ready-to-use heights can be passed. + heights_buffer = heights_variant; + } else if (heights_variant.get_type() == Variant::OBJECT) { + // If an image is passed, we have to convert it. + // This would be expensive to do with a script, so it's nice to have it here. + Ref<Image> image = heights_variant; + ERR_FAIL_COND(image.is_null()); + ERR_FAIL_COND(image->get_format() != Image::FORMAT_RF); + + 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(); + for (int i = 0; i < heights_buffer.size(); ++i) { + w[i] = rp[i]; + } + } else { + ERR_FAIL_MSG("Expected PackedFloat32Array or float Image."); + } + + // Compute min and max heights or use precomputed values. + real_t min_height = 0.0; + real_t max_height = 0.0; + if (d.has("min_height") && d.has("max_height")) { + min_height = d["min_height"]; + max_height = d["max_height"]; + } else { + int heights_size = heights.size(); + for (int i = 0; i < heights_size; ++i) { + float h = heights[i]; + if (h < min_height) { + min_height = h; + } else if (h > max_height) { + max_height = h; + } + } + } + + ERR_FAIL_COND(min_height > max_height); + + ERR_FAIL_COND(heights_buffer.size() != (width * depth)); + + // If specified, min and max height will be used as precomputed values. + _setup(heights_buffer, width, depth, min_height, max_height); } Variant HeightMapShape3DSW::get_data() const { - ERR_FAIL_V(Variant()); + Dictionary d; + d["width"] = width; + d["depth"] = depth; + + const AABB &aabb = get_aabb(); + d["min_height"] = aabb.position.y; + d["max_height"] = aabb.position.y + aabb.size.y; + + d["heights"] = heights; + + return d; } HeightMapShape3DSW::HeightMapShape3DSW() { - width = 0; - depth = 0; - cell_size = 0; } diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index 988e76c699..bc8bd3e695 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -81,12 +81,12 @@ public: virtual PhysicsServer3D::ShapeType get_type() const = 0; - _FORCE_INLINE_ AABB get_aabb() const { return aabb; } + _FORCE_INLINE_ const AABB &get_aabb() const { return aabb; } _FORCE_INLINE_ bool is_configured() const { return configured; } virtual bool is_concave() const { return false; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const = 0; 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 = 0; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0; @@ -130,7 +130,7 @@ public: virtual real_t get_area() const { return Math_INF; } virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_PLANE; } - virtual void project_range(const Vector3 &p_normal, const Transform &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; 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; } @@ -157,7 +157,7 @@ public: 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 Transform &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; 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; @@ -185,7 +185,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SPHERE; } - virtual void project_range(const Vector3 &p_normal, const Transform &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; 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; @@ -210,7 +210,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_BOX; } - virtual void project_range(const Vector3 &p_normal, const Transform &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; 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; @@ -239,7 +239,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CAPSULE; } - virtual void project_range(const Vector3 &p_normal, const Transform &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; 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; @@ -268,7 +268,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CYLINDER; } - virtual void project_range(const Vector3 &p_normal, const Transform &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; 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; @@ -293,7 +293,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } - virtual void project_range(const Vector3 &p_normal, const Transform &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; 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; @@ -371,7 +371,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } - virtual void project_range(const Vector3 &p_normal, const Transform &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; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; @@ -389,25 +389,33 @@ public: }; struct HeightMapShape3DSW : public ConcaveShape3DSW { - Vector<real_t> heights; - int width; - int depth; - real_t cell_size; + Vector<float> heights; + int width = 0; + int depth = 0; + Vector3 local_origin; - //void _cull_segment(int p_idx,_SegmentCullParams *p_params) const; - //void _cull(int p_idx,_CullParams *p_params) const; + _FORCE_INLINE_ float _get_height(int p_x, int p_z) const { + return heights[(p_z * width) + p_x]; + } + + _FORCE_INLINE_ void _get_point(int p_x, int p_z, Vector3 &r_point) const { + r_point.x = p_x - 0.5 * (width - 1.0); + r_point.y = _get_height(p_x, p_z); + r_point.z = p_z - 0.5 * (depth - 1.0); + } + + void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const; - void _setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size); + void _setup(const Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); public: - Vector<real_t> get_heights() const; + Vector<float> get_heights() const; int get_width() const; int get_depth() const; - real_t get_cell_size() const; virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_HEIGHTMAP; } - virtual void project_range(const Vector3 &p_normal, const Transform &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; 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; @@ -433,7 +441,7 @@ struct FaceShape3DSW : public Shape3DSW { const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; } - void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + 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; @@ -454,11 +462,11 @@ struct MotionShape3DSW : public Shape3DSW { virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } - void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { + void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { Vector3 cast = p_transform.basis.xform(motion); real_t mina, maxa; real_t minb, maxb; - Transform ofsb = p_transform; + Transform3D ofsb = p_transform; ofsb.origin += cast; shape->project_range(p_normal, p_transform, mina, maxa); shape->project_range(p_normal, ofsb, minb, maxb); diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp index f63a470cbe..63a0fe11ba 100644 --- a/servers/physics_3d/soft_body_3d_sw.cpp +++ b/servers/physics_3d/soft_body_3d_sw.cpp @@ -289,7 +289,7 @@ void SoftBody3DSW::update_link_constants() { } } -void SoftBody3DSW::apply_nodes_transform(const Transform &p_transform) { +void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) { if (soft_mesh.is_null()) { return; } @@ -684,7 +684,7 @@ void SoftBody3DSW::generate_bending_constraints(int p_distance) { // // This function takes in a list of interdependent Links and tries // to maximize the distance between calculation -// of dependent links. This increases the amount of parallelism that can +// of dependent links. This increases the amount of parallelism that can // be exploited by out-of-order instruction processors with large but // (inevitably) finite instruction windows. // diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h index 6c0451ff45..ac8bcbf0b9 100644 --- a/servers/physics_3d/soft_body_3d_sw.h +++ b/servers/physics_3d/soft_body_3d_sw.h @@ -106,6 +106,8 @@ class SoftBody3DSW : public CollisionObject3DSW { VSet<RID> exceptions; + uint64_t island_step = 0; + public: SoftBody3DSW(); @@ -124,6 +126,9 @@ public: _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } _FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; } + _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; } + virtual void set_space(Space3DSW *p_space); void set_mesh(const Ref<Mesh> &p_mesh); @@ -196,7 +201,7 @@ private: void reset_link_rest_lengths(); void update_link_constants(); - void apply_nodes_transform(const Transform &p_transform); + void apply_nodes_transform(const Transform3D &p_transform); void add_velocity(const Vector3 &p_velocity); @@ -226,7 +231,7 @@ public: SoftBody3DSW *get_soft_body() const { return soft_body; } virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SOFT_BODY; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; } virtual Vector3 get_support(const Vector3 &p_normal) const { return Vector3(); } virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 2df824b320..c1e09c9a22 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -59,7 +59,7 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int cc = 0; - //Transform ai = p_xform.affine_inverse(); + //Transform3D ai = p_xform.affine_inverse(); for (int i = 0; i < amount; i++) { if (cc >= p_result_max) { @@ -79,7 +79,7 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; - Transform inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + Transform3D inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); inv_xform.affine_invert(); if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point))) { @@ -136,7 +136,7 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; - Transform inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); + Transform3D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); Vector3 local_from = inv_xform.xform(begin); Vector3 local_to = inv_xform.xform(end); @@ -146,7 +146,7 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec Vector3 shape_point, shape_normal; if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { - Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + Transform3D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); shape_point = xform.xform(shape_point); real_t ld = normal.dot(shape_point); @@ -180,7 +180,7 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec return true; } -int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { if (p_result_max <= 0) { return 0; } @@ -194,7 +194,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans int cc = 0; - //Transform ai = p_xform.affine_inverse(); + //Transform3D ai = p_xform.affine_inverse(); for (int i = 0; i < amount; i++) { if (cc >= p_result_max) { @@ -239,7 +239,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans return cc; } -bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { +bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, false); @@ -252,7 +252,7 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor real_t best_safe = 1; real_t best_unsafe = 1; - Transform xform_inv = p_xform.affine_inverse(); + Transform3D xform_inv = p_xform.affine_inverse(); MotionShape3DSW mshape; mshape.shape = shape; mshape.motion = xform_inv.basis.xform(p_motion); @@ -280,7 +280,7 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor Vector3 point_A, point_B; Vector3 sep_axis = p_motion.normalized(); - Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? if (CollisionSolver3DSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { continue; @@ -348,7 +348,7 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor return true; } -bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { if (p_result_max <= 0) { return false; } @@ -432,7 +432,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vect rd->best_local_shape = rd->local_shape; } -bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); @@ -512,7 +512,7 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob continue; } - Transform shape_xform = obj->get_transform() * obj->get_shape_transform(i); + Transform3D shape_xform = obj->get_transform() * obj->get_shape_transform(i); Shape3DSW *shape = obj->get_shape(i); Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); @@ -573,7 +573,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 Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin) { +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; @@ -598,7 +598,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb)); body_aabb = body_aabb.grow(p_margin); - Transform body_transform = p_transform; + Transform3D body_transform = p_transform; for (int i = 0; i < p_result_max; i++) { //reset results @@ -636,7 +636,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra continue; } - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + 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]; @@ -724,7 +724,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra return rays_found; } -bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &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, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes) { //give me back regular physics engine logic //this is madness //and most people using this function will think @@ -768,7 +768,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons real_t motion_length = p_motion.length(); Vector3 motion_normal = p_motion / motion_length; - Transform body_transform = p_from; + Transform3D body_transform = p_from; bool recovered = false; @@ -797,7 +797,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons continue; } - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + 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; @@ -807,6 +807,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons 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 (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; } @@ -868,14 +875,14 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons continue; } - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + 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; } - Transform body_shape_xform_inv = body_shape_xform.affine_inverse(); + Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse(); MotionShape3DSW mshape; mshape.shape = body_shape; mshape.motion = body_shape_xform_inv.basis.xform(p_motion); @@ -889,11 +896,18 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons 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; + } + } + //test initial overlap, does it collide if going all the way? Vector3 point_A, point_B; Vector3 sep_axis = motion_normal; - Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { continue; @@ -960,7 +974,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } //it collided, let's get the rest info in unsafe advance - Transform ugt = body_transform; + Transform3D ugt = body_transform; ugt.origin += p_motion * unsafe; _RestCallbackData rcd; @@ -979,7 +993,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons continue; } - Transform body_shape_xform = ugt * p_body->get_shape_transform(j); + 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) { @@ -994,6 +1008,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons 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; + } + } + rcd.object = col_obj; rcd.shape = shape_idx; bool sc = 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), _rest_cbk_result, &rcd, nullptr, p_margin); diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index 3a8f452e54..18e93c90cc 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -50,10 +50,10 @@ public: virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override; - virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override; - virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual int intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override; + virtual bool collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override; PhysicsDirectSpaceState3DSW(); @@ -203,8 +203,8 @@ 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 Transform &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 Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes); + 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); Space3DSW(); ~Space3DSW(); diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index 9a73e11562..ba18bac611 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -33,109 +33,150 @@ #include "core/os/os.h" -void Step3DSW::_populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island) { +#define BODY_ISLAND_COUNT_RESERVE 128 +#define BODY_ISLAND_SIZE_RESERVE 512 +#define ISLAND_COUNT_RESERVE 128 +#define ISLAND_SIZE_RESERVE 512 +#define CONSTRAINT_COUNT_RESERVE 1024 + +void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) { p_body->set_island_step(_step); - p_body->set_island_next(*p_island); - *p_island = p_body; + + if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { + // Only dynamic bodies are tested for activation. + p_body_island.push_back(p_body); + } for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { - Constraint3DSW *c = (Constraint3DSW *)E->key(); - if (c->get_island_step() == _step) { - continue; //already processed + Constraint3DSW *constraint = (Constraint3DSW *)E->key(); + if (constraint->get_island_step() == _step) { + continue; // Already processed. } - c->set_island_step(_step); - c->set_island_next(*p_constraint_island); - *p_constraint_island = c; + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); - for (int i = 0; i < c->get_body_count(); i++) { + all_constraints.push_back(constraint); + + // Find connected rigid bodies. + for (int i = 0; i < constraint->get_body_count(); i++) { if (i == E->get()) { continue; } - Body3DSW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - continue; //no go + Body3DSW *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. + } + if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. } - _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); + _populate_island(other_body, p_body_island, p_constraint_island); + } + + // Find connected soft bodies. + for (int i = 0; i < constraint->get_soft_body_count(); i++) { + SoftBody3DSW *soft_body = constraint->get_soft_body_ptr(i); + if (soft_body->get_island_step() == _step) { + continue; // Already processed. + } + _populate_island_soft_body(soft_body, p_body_island, p_constraint_island); } } } -void Step3DSW::_setup_island(Constraint3DSW *p_island, real_t p_delta) { - Constraint3DSW *ci = p_island; - while (ci) { - ci->setup(p_delta); - //todo remove from island if process fails - ci = ci->get_island_next(); +void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) { + p_soft_body->set_island_step(_step); + + for (Set<Constraint3DSW *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) { + Constraint3DSW *constraint = (Constraint3DSW *)E->get(); + if (constraint->get_island_step() == _step) { + continue; // Already processed. + } + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + + all_constraints.push_back(constraint); + + // Find connected rigid bodies. + for (int i = 0; i < constraint->get_body_count(); i++) { + Body3DSW *body = constraint->get_body_ptr()[i]; + if (body->get_island_step() == _step) { + continue; // Already processed. + } + if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(body, p_body_island, p_constraint_island); + } + } +} + +void Step3DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { + Constraint3DSW *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void Step3DSW::_pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const { + uint32_t constraint_count = p_constraint_island.size(); + uint32_t valid_constraint_count = 0; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + Constraint3DSW *constraint = p_constraint_island[constraint_index]; + if (p_constraint_island[constraint_index]->pre_solve(delta)) { + // Keep this constraint for solving. + p_constraint_island[valid_constraint_count++] = constraint; + } } + p_constraint_island.resize(valid_constraint_count); } -void Step3DSW::_solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta) { - int at_priority = 1; +void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) { + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[p_island_index]; - while (p_island) { - for (int i = 0; i < p_iterations; i++) { - Constraint3DSW *ci = p_island; - while (ci) { - ci->solve(p_delta); - ci = ci->get_island_next(); + int current_priority = 1; + + uint32_t constraint_count = constraint_island.size(); + while (constraint_count > 0) { + for (int i = 0; i < iterations; i++) { + // Go through all iterations. + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + constraint_island[constraint_index]->solve(delta); } } - at_priority++; - - { - Constraint3DSW *ci = p_island; - Constraint3DSW *prev = nullptr; - while (ci) { - if (ci->get_priority() < at_priority) { - if (prev) { - prev->set_island_next(ci->get_island_next()); //remove - } else { - p_island = ci->get_island_next(); - } - } else { - prev = ci; - } - - ci = ci->get_island_next(); + // Check priority to keep only higher priority constraints. + uint32_t priority_constraint_count = 0; + ++current_priority; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + Constraint3DSW *constraint = constraint_island[constraint_index]; + if (constraint->get_priority() >= current_priority) { + // Keep this constraint for the next iteration. + constraint_island[priority_constraint_count++] = constraint; } } + constraint_count = priority_constraint_count; } } -void Step3DSW::_check_suspend(Body3DSW *p_island, real_t p_delta) { +void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) const { bool can_sleep = true; - Body3DSW *b = p_island; - while (b) { - if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - b = b->get_island_next(); - continue; //ignore for static - } + uint32_t body_count = p_body_island.size(); + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + Body3DSW *body = p_body_island[body_index]; - if (!b->sleep_test(p_delta)) { + if (!body->sleep_test(delta)) { can_sleep = false; } - - b = b->get_island_next(); } - //put all to sleep or wake up everyoen - - b = p_island; - while (b) { - if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - b = b->get_island_next(); - continue; //ignore for static - } + // Put all to sleep or wake up everyone. + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + Body3DSW *body = p_body_island[body_index]; - bool active = b->is_active(); + bool active = body->is_active(); if (active == can_sleep) { - b->set_active(!can_sleep); + body->set_active(!can_sleep); } - - b = b->get_island_next(); } } @@ -144,6 +185,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { p_space->setup(); //update inertias, etc + iterations = p_iterations; + delta = p_delta; + const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list(); const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list(); @@ -179,81 +223,121 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } - /* GENERATE CONSTRAINT ISLANDS */ + /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ + + uint32_t island_count = 0; + + const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + Constraint3DSW *constraint = E->get(); + if (constraint->get_island_step() == _step) { + continue; + } + constraint->set_island_step(_step); + + // Each constraint can be on a separate island for areas as there's no solving phase. + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + + all_constraints.push_back(constraint); + constraint_island.push_back(constraint); + } + p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ - Body3DSW *island_list = nullptr; - Constraint3DSW *constraint_island_list = nullptr; b = body_list->first(); - int island_count = 0; + uint32_t body_island_count = 0; while (b) { Body3DSW *body = b->self(); if (body->get_island_step() != _step) { - Body3DSW *island = nullptr; - Constraint3DSW *constraint_island = nullptr; - _populate_island(body, &island, &constraint_island); - - island->set_island_list_next(island_list); - island_list = island; + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); - if (constraint_island) { - constraint_island->set_island_list_next(constraint_island_list); - constraint_island_list = constraint_island; - island_count++; + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); } - } - b = b->next(); - } + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); - p_space->set_island_count(island_count); + _populate_island(body, body_island, constraint_island); - const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list(); + if (body_island.is_empty()) { + --body_island_count; + } - while (aml.first()) { - for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - Constraint3DSW *c = E->get(); - if (c->get_island_step() == _step) { - continue; + if (constraint_island.is_empty()) { + --island_count; } - c->set_island_step(_step); - c->set_island_next(nullptr); - c->set_island_list_next(constraint_island_list); - constraint_island_list = c; } - p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here + b = b->next(); } + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */ + sb = soft_body_list->first(); while (sb) { - for (const Set<Constraint3DSW *>::Element *E = sb->self()->get_constraints().front(); E; E = E->next()) { - Constraint3DSW *c = E->get(); - if (c->get_island_step() == _step) - continue; - c->set_island_step(_step); - c->set_island_next(NULL); - c->set_island_list_next(constraint_island_list); - constraint_island_list = c; + SoftBody3DSW *soft_body = sb->self(); + + if (soft_body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); + + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); + + _populate_island_soft_body(soft_body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; + } + + if (constraint_island.is_empty()) { + --island_count; + } } sb = sb->next(); } + p_space->set_island_count((int)island_count); + { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); profile_begtime = profile_endtime; } - /* SETUP CONSTRAINT ISLANDS */ + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ - { - Constraint3DSW *ci = constraint_island_list; - while (ci) { - _setup_island(ci, p_delta); - ci = ci->get_island_list_next(); - } - } + uint32_t total_contraint_count = all_constraints.size(); + work_pool.do_work(total_contraint_count, this, &Step3DSW::_setup_contraint, nullptr); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -261,15 +345,21 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } + /* PRE-SOLVE CONSTRAINT ISLANDS */ + + // Warning: This doesn't run on threads, because it involves thread-unsafe processing. + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _pre_solve_island(constraint_islands[island_index]); + } + /* SOLVE CONSTRAINT ISLANDS */ - { - Constraint3DSW *ci = constraint_island_list; - while (ci) { - //iterating each island separatedly improves cache efficiency - _solve_island(ci, p_iterations, p_delta); - ci = ci->get_island_list_next(); - } + // Warning: _solve_island modifies the constraint islands for optimization purpose, + // their content is not reliable after these calls and shouldn't be used anymore. + if (island_count > 1) { + work_pool.do_work(island_count, this, &Step3DSW::_solve_island, nullptr); + } else if (island_count > 0) { + _solve_island(0); } { //profile @@ -289,12 +379,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { /* SLEEP / WAKE UP ISLANDS */ - { - Body3DSW *bi = island_list; - while (bi) { - _check_suspend(bi, p_delta); - bi = bi->get_island_list_next(); - } + for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { + _check_suspend(body_islands[island_index]); } /* UPDATE SOFT BODY CONSTRAINTS */ @@ -311,6 +397,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } + all_constraints.clear(); + p_space->update(); p_space->unlock(); _step++; @@ -318,4 +406,14 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { Step3DSW::Step3DSW() { _step = 1; + + body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); + constraint_islands.reserve(ISLAND_COUNT_RESERVE); + all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); + + work_pool.init(); +} + +Step3DSW::~Step3DSW() { + work_pool.finish(); } diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h index 55c48ec0eb..9c60004b24 100644 --- a/servers/physics_3d/step_3d_sw.h +++ b/servers/physics_3d/step_3d_sw.h @@ -33,17 +33,32 @@ #include "space_3d_sw.h" +#include "core/templates/local_vector.h" +#include "core/templates/thread_work_pool.h" + class Step3DSW { uint64_t _step; - void _populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island); - void _setup_island(Constraint3DSW *p_island, real_t p_delta); - void _solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta); - void _check_suspend(Body3DSW *p_island, real_t p_delta); + int iterations = 0; + real_t delta = 0.0; + + ThreadWorkPool work_pool; + + LocalVector<LocalVector<Body3DSW *>> body_islands; + LocalVector<LocalVector<Constraint3DSW *>> constraint_islands; + LocalVector<Constraint3DSW *> all_constraints; + + void _populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island); + void _populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island); + void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr); + void _check_suspend(const LocalVector<Body3DSW *> &p_body_island) const; public: void step(Space3DSW *p_space, real_t p_delta, int p_iterations); Step3DSW(); + ~Step3DSW(); }; #endif // STEP__SW_H |