diff options
Diffstat (limited to 'servers/physics_3d')
47 files changed, 4793 insertions, 723 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index 571f1435de..bb4e0ed752 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -199,7 +199,7 @@ void Area3DSW::set_monitorable(bool p_monitorable) { } void Area3DSW::call_queries() { - if (monitor_callback_id.is_valid() && !monitored_bodies.empty()) { + if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { Variant res[5]; Variant *resptr[5]; for (int i = 0; i < 5; i++) { @@ -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; } @@ -234,7 +236,7 @@ void Area3DSW::call_queries() { } } - if (area_monitor_callback_id.is_valid() && !monitored_areas.empty()) { + if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { Variant res[5]; Variant *resptr[5]; for (int i = 0; i < 5; i++) { @@ -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 6af3976167..8a0a1e963b 100644 --- a/servers/physics_3d/area_3d_sw.h +++ b/servers/physics_3d/area_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -32,7 +32,7 @@ #define AREA_SW_H #include "collision_object_3d_sw.h" -#include "core/self_list.h" +#include "core/templates/self_list.h" #include "servers/physics_server_3d.h" //#include "servers/physics_3d/query_sw.h" diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp index a5fb20fe2b..4de5f1ba47 100644 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h index 992d4747b9..fbdaa25cbb 100644 --- a/servers/physics_3d/area_pair_3d_sw.h +++ b/servers/physics_3d/area_pair_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index d1f16cb4ae..64ba0cb09d 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -51,18 +51,18 @@ void Body3DSW::_update_transform_dependant() { } void Body3DSW::update_inertias() { - //update shapes and motions + // Update shapes and motions. switch (mode) { case PhysicsServer3D::BODY_MODE_RIGID: { - //update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet) + // Update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet) real_t total_area = 0; for (int i = 0; i < get_shape_count(); i++) { total_area += get_shape_area(i); } - // We have to recompute the center of mass + // We have to recompute the center of mass. center_of_mass_local.zero(); for (int i = 0; i < get_shape_count(); i++) { @@ -70,21 +70,24 @@ void Body3DSW::update_inertias() { real_t mass = area * this->mass / total_area; - // NOTE: we assume that the shape origin is also its center of mass + // 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; - // Recompute the inertia tensor + // Recompute the inertia tensor. Basis inertia_tensor; inertia_tensor.set_zero(); + bool inertia_set = false; for (int i = 0; i < get_shape_count(); i++) { if (is_shape_disabled(i)) { continue; } + inertia_set = true; + const Shape3DSW *shape = get_shape(i); real_t area = get_shape_area(i); @@ -102,7 +105,12 @@ void Body3DSW::update_inertias() { inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass; } - // Compute the principal axes of inertia + // Set the inertia to a valid value when there are no valid shapes. + if (!inertia_set) { + inertia_tensor.set_diagonal(Vector3(1.0, 1.0, 1.0)); + } + + // Compute the principal axes of inertia. principal_inertia_axes_local = inertia_tensor.diagonalize().transposed(); _inv_inertia = inertia_tensor.get_main_diagonal().inverse(); @@ -493,20 +501,18 @@ void Body3DSW::integrate_forces(real_t p_step) { if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { //compute motion, angular and etc. velocities from prev transform - linear_velocity = (new_transform.origin - get_transform().origin) / p_step; + motion = new_transform.origin - get_transform().origin; + do_motion = true; + linear_velocity = motion / p_step; //compute a FAKE angular velocity, not so easy - Basis rot = new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized(); + Basis rot = new_transform.basis.orthonormalized() * get_transform().basis.orthonormalized().transposed(); Vector3 axis; real_t angle; rot.get_axis_angle(axis, angle); axis.normalize(); - angular_velocity = axis.normalized() * (angle / p_step); - - motion = new_transform.origin - get_transform().origin; - do_motion = true; - + angular_velocity = axis * (angle / p_step); } else { if (!omit_force_integration && !first_integration) { //overridden by direct state query @@ -628,7 +634,6 @@ 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(); if (!get_space()) { _set_transform(p_xform); @@ -655,8 +660,6 @@ void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { get_space()->body_add_to_state_query_list(&direct_state_query_list); simulated_motion=true; _set_transform(p_xform); - - } */ diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index b642729404..e87ff2364b 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -33,7 +33,7 @@ #include "area_3d_sw.h" #include "collision_object_3d_sw.h" -#include "core/vset.h" +#include "core/templates/vset.h" class Constraint3DSW; @@ -178,7 +178,7 @@ public: } _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } - _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); } + _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } _FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos); _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } @@ -290,10 +290,10 @@ public: void update_inertias(); _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } - _FORCE_INLINE_ Vector3 get_inv_inertia() const { return _inv_inertia; } - _FORCE_INLINE_ Basis get_inv_inertia_tensor() const { return _inv_inertia_tensor; } + _FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; } + _FORCE_INLINE_ const Basis &get_inv_inertia_tensor() const { return _inv_inertia_tensor; } _FORCE_INLINE_ real_t get_friction() const { return friction; } - _FORCE_INLINE_ Vector3 get_gravity() const { return gravity; } + _FORCE_INLINE_ const Vector3 &get_gravity() const { return gravity; } _FORCE_INLINE_ real_t get_bounce() const { return bounce; } void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock); @@ -426,7 +426,7 @@ public: ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); return body->contacts[p_contact_idx].local_normal; } - virtual float get_contact_impulse(int p_contact_idx) const override { + virtual real_t get_contact_impulse(int p_contact_idx) const override { return 0.0f; // Only implemented for bullet } virtual int get_contact_local_shape(int p_contact_idx) const override { diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index 848138940e..36114c0c91 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -49,12 +49,12 @@ #define MIN_VELOCITY 0.0001 #define MAX_BIAS_ROTATION (Math_PI / 8) -void BodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { +void BodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { BodyPair3DSW *pair = (BodyPair3DSW *)p_userdata; - pair->contact_added_callback(p_point_A, p_point_B); + pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); } -void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B) { +void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) { // check if we already have the contact //Vector3 local_A = A->get_inv_transform().xform(p_point_A); @@ -73,6 +73,8 @@ void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, const Vector contact.acc_bias_impulse = 0; contact.acc_bias_impulse_center_of_mass = 0; contact.acc_tangent_impulse = Vector3(); + contact.index_A = p_index_A; + contact.index_B = p_index_B; contact.local_A = local_A; contact.local_B = local_B; contact.normal = (p_point_A - p_point_B).normalized(); @@ -211,11 +213,21 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) { bool BodyPair3DSW::setup(real_t p_step) { //cannot collide - if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) { + 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)) { + if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { + report_contacts_only = true; + } else { + collided = false; + return false; + } + } + if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) { collided = false; return false; @@ -279,12 +291,9 @@ bool BodyPair3DSW::setup(real_t p_step) { real_t depth = c.normal.dot(global_A - global_B); if (depth <= 0) { - c.active = false; continue; } - c.active = true; - #ifdef DEBUG_ENABLED if (space->is_debugging_contacts()) { @@ -308,6 +317,11 @@ bool BodyPair3DSW::setup(real_t p_step) { B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB); } + if (report_contacts_only) { + collided = false; + continue; + } + c.active = true; // Precompute normal mass, tangent mass, and bias. @@ -367,8 +381,8 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step); - B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb, MAX_BIAS_ROTATION / p_step); + 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); crbA = A->get_biased_angular_velocity().cross(c.rA); crbB = B->get_biased_angular_velocity().cross(c.rB); @@ -383,8 +397,8 @@ 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(A->get_center_of_mass(), -jb_com, 0.0f); - B->apply_bias_impulse(B->get_center_of_mass(), jb_com, 0.0f); + 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); } c.active = true; @@ -456,7 +470,7 @@ void BodyPair3DSW::solve(real_t p_step) { } BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B) : - Constraint3DSW(_arr, 2) { + BodyContact3DSW(_arr, 2) { A = p_A; B = p_B; shape_A = p_shape_A; @@ -472,3 +486,305 @@ BodyPair3DSW::~BodyPair3DSW() { A->remove_constraint(this); B->remove_constraint(this); } + +void BodySoftBodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { + BodySoftBodyPair3DSW *pair = (BodySoftBodyPair3DSW *)p_userdata; + pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); +} + +void BodySoftBodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) { + Vector3 local_A = body->get_inv_transform().xform(p_point_A); + Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B); + + Contact contact; + contact.index_A = p_index_A; + contact.index_B = p_index_B; + contact.acc_normal_impulse = 0; + contact.acc_bias_impulse = 0; + contact.acc_bias_impulse_center_of_mass = 0; + contact.acc_tangent_impulse = Vector3(); + contact.local_A = local_A; + contact.local_B = local_B; + contact.normal = (p_point_A - p_point_B).normalized(); + contact.mass_normal = 0; + + // Attempt to determine if the contact will be reused. + real_t contact_recycle_radius = space->get_contact_recycle_radius(); + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + if (c.index_B == p_index_B) { + if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) && + c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { + contact.acc_normal_impulse = c.acc_normal_impulse; + contact.acc_bias_impulse = c.acc_bias_impulse; + contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; + contact.acc_tangent_impulse = c.acc_tangent_impulse; + } + c = contact; + return; + } + } + + contacts.push_back(contact); +} + +void BodySoftBodyPair3DSW::validate_contacts() { + // Make sure to erase contacts that are no longer valid. + const Transform &transform_A = body->get_transform(); + + real_t contact_max_separation = space->get_contact_max_separation(); + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + + Vector3 global_A = transform_A.xform(c.local_A); + Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { + // Contact no longer needed, remove. + if ((contact_index + 1) < contact_count) { + // Swap with the last one. + SWAP(c, contacts[contact_count - 1]); + } + + contact_index--; + contact_count--; + } + } + + contacts.resize(contact_count); +} + +bool BodySoftBodyPair3DSW::setup(real_t p_step) { + 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; + } + + if (body->is_shape_set_as_disabled(body_shape)) { + collided = false; + return false; + } + + const Transform &xform_Au = body->get_transform(); + Transform 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); + + 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; + + real_t max_penetration = space->get_contact_max_allowed_penetration(); + + real_t bias = (real_t)0.3; + if (shape_A_ptr->get_custom_bias()) { + bias = shape_A_ptr->get_custom_bias(); + } + + real_t inv_dt = 1.0 / 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]; + c.active = false; + + real_t node_inv_mass = soft_body->get_node_inv_mass(c.index_B); + if (node_inv_mass == 0.0) { + continue; + } + + Vector3 global_A = xform_Au.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); + + if (depth <= 0) { + continue; + } + + c.active = true; + +#ifdef DEBUG_ENABLED + + if (space->is_debugging_contacts()) { + space->add_debug_contact(global_A); + space->add_debug_contact(global_B); + } +#endif + + c.rA = global_A - xform_Au.origin - body->get_center_of_mass(); + c.rB = global_B; + + if (body->can_report_contacts()) { + Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity(); + 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) { + body->set_active(true); + } + + // Precompute normal mass, tangent mass, and bias. + Vector3 inertia_A = body->get_inv_inertia_tensor().xform(c.rA.cross(c.normal)); + real_t kNormal = body->get_inv_mass() + node_inv_mass; + kNormal += c.normal.dot(inertia_A.cross(c.rA)); + c.mass_normal = 1.0f / kNormal; + + c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); + 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); + soft_body->apply_node_impulse(c.index_B, j_vec); + c.acc_bias_impulse = 0; + c.acc_bias_impulse_center_of_mass = 0; + + c.bounce = body->get_bounce(); + + if (c.bounce) { + Vector3 crA = body->get_angular_velocity().cross(c.rA); + Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA; + + // Normal impulse. + c.bounce = c.bounce * dv.dot(c.normal); + } + } + + return true; +} + +void BodySoftBodyPair3DSW::solve(real_t p_step) { + if (!collided) { + return; + } + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + if (!c.active) { + continue; + } + + c.active = false; + + // Bias impulse. + Vector3 crbA = body->get_biased_angular_velocity().cross(c.rA); + Vector3 dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA; + + real_t vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn = (-vbn + c.bias) * c.mass_normal; + real_t jbnOld = c.acc_bias_impulse; + c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); + + 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); + soft_body->apply_node_bias_impulse(c.index_B, jb); + + crbA = body->get_biased_angular_velocity().cross(c.rA); + dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA; + + vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn_com = (-vbn + c.bias) / (body->get_inv_mass() + soft_body->get_node_inv_mass(c.index_B)); + real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; + c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); + + 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); + } + + c.active = true; + } + + Vector3 crA = body->get_angular_velocity().cross(c.rA); + Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA; + + // Normal impulse. + real_t vn = dv.dot(c.normal); + + if (Math::abs(vn) > MIN_VELOCITY) { + real_t jn = -(c.bounce + vn) * c.mass_normal; + real_t jnOld = c.acc_normal_impulse; + c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); + + Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); + + body->apply_impulse(c.rA + body->get_center_of_mass(), -j); + soft_body->apply_node_impulse(c.index_B, j); + + c.active = true; + } + + // Friction impulse. + real_t friction = body->get_friction(); + + Vector3 lvA = body->get_linear_velocity() + body->get_angular_velocity().cross(c.rA); + Vector3 lvB = soft_body->get_node_velocity(c.index_B); + Vector3 dtv = lvB - lvA; + + real_t tn = c.normal.dot(dtv); + + // Tangential velocity. + Vector3 tv = dtv - c.normal * tn; + real_t tvl = tv.length(); + + if (tvl > MIN_VELOCITY) { + tv /= tvl; + + Vector3 temp1 = body->get_inv_inertia_tensor().xform(c.rA.cross(tv)); + + real_t t = -tvl / + (body->get_inv_mass() + soft_body->get_node_inv_mass(c.index_B) + tv.dot(temp1.cross(c.rA))); + + Vector3 jt = t * tv; + + Vector3 jtOld = c.acc_tangent_impulse; + c.acc_tangent_impulse += jt; + + real_t fi_len = c.acc_tangent_impulse.length(); + real_t jtMax = c.acc_normal_impulse * friction; + + if (fi_len > CMP_EPSILON && fi_len > jtMax) { + c.acc_tangent_impulse *= jtMax / fi_len; + } + + jt = c.acc_tangent_impulse - jtOld; + + body->apply_impulse(c.rA + body->get_center_of_mass(), -jt); + soft_body->apply_node_impulse(c.index_B, jt); + + c.active = true; + } + } +} + +BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) { + body = p_A; + soft_body = p_B; + body_shape = p_shape_A; + space = p_A->get_space(); + body->add_constraint(this, 0); + soft_body->add_constraint(this); +} + +BodySoftBodyPair3DSW::~BodySoftBodyPair3DSW() { + body->remove_constraint(this); + soft_body->remove_constraint(this); +} diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 59e36e7ea5..74dddfa6aa 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -28,33 +28,20 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef BODY_PAIR_SW_H -#define BODY_PAIR_SW_H +#ifndef BODY_PAIR_3D_SW_H +#define BODY_PAIR_3D_SW_H #include "body_3d_sw.h" #include "constraint_3d_sw.h" +#include "core/templates/local_vector.h" +#include "soft_body_3d_sw.h" -class BodyPair3DSW : public Constraint3DSW { - enum { - - MAX_CONTACTS = 4 - }; - - union { - struct { - Body3DSW *A; - Body3DSW *B; - }; - - Body3DSW *_arr[2]; - }; - - int shape_A; - int shape_B; - +class BodyContact3DSW : public Constraint3DSW { +protected: struct Contact { Vector3 position; Vector3 normal; + int index_A, index_B; Vector3 local_A, local_B; real_t acc_normal_impulse; // accumulated normal impulse (Pn) Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) @@ -69,22 +56,45 @@ class BodyPair3DSW : public Constraint3DSW { Vector3 rA, rB; // Offset in world orientation with respect to center of mass }; + Vector3 sep_axis; + bool collided; + + Space3DSW *space; + + BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : + Constraint3DSW(p_body_ptr, p_body_count) { + } +}; + +class BodyPair3DSW : public BodyContact3DSW { + enum { + MAX_CONTACTS = 4 + }; + + union { + struct { + Body3DSW *A; + Body3DSW *B; + }; + + Body3DSW *_arr[2]; + }; + + int shape_A; + int shape_B; + Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection - Vector3 sep_axis; Contact contacts[MAX_CONTACTS]; int contact_count; - bool collided; - static void _contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); + 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, const Vector3 &p_point_B); + 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); - Space3DSW *space; - public: bool setup(real_t p_step); void solve(real_t p_step); @@ -93,4 +103,26 @@ public: ~BodyPair3DSW(); }; -#endif // BODY_PAIR__SW_H +class BodySoftBodyPair3DSW : public BodyContact3DSW { + Body3DSW *body; + SoftBody3DSW *soft_body; + + int body_shape; + + LocalVector<Contact> contacts; + + 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(); + +public: + bool setup(real_t p_step); + void solve(real_t p_step); + + BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B); + ~BodySoftBodyPair3DSW(); +}; + +#endif // BODY_PAIR_3D_SW_H diff --git a/servers/physics_3d/broad_phase_3d_basic.cpp b/servers/physics_3d/broad_phase_3d_basic.cpp index f5ea1897a9..b41c2530da 100644 --- a/servers/physics_3d/broad_phase_3d_basic.cpp +++ b/servers/physics_3d/broad_phase_3d_basic.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -29,8 +29,8 @@ /*************************************************************************/ #include "broad_phase_3d_basic.h" -#include "core/list.h" -#include "core/print_string.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); diff --git a/servers/physics_3d/broad_phase_3d_basic.h b/servers/physics_3d/broad_phase_3d_basic.h index 4b644bf818..54d34e005f 100644 --- a/servers/physics_3d/broad_phase_3d_basic.h +++ b/servers/physics_3d/broad_phase_3d_basic.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -32,7 +32,7 @@ #define BROAD_PHASE_BASIC_H #include "broad_phase_3d_sw.h" -#include "core/map.h" +#include "core/templates/map.h" class BroadPhase3DBasic : public BroadPhase3DSW { struct Element { diff --git a/servers/physics_3d/broad_phase_3d_sw.cpp b/servers/physics_3d/broad_phase_3d_sw.cpp index 1a20fdd0cb..8aa64034ec 100644 --- a/servers/physics_3d/broad_phase_3d_sw.cpp +++ b/servers/physics_3d/broad_phase_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ diff --git a/servers/physics_3d/broad_phase_3d_sw.h b/servers/physics_3d/broad_phase_3d_sw.h index 081e75810f..283c087b96 100644 --- a/servers/physics_3d/broad_phase_3d_sw.h +++ b/servers/physics_3d/broad_phase_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ diff --git a/servers/physics_3d/broad_phase_octree.cpp b/servers/physics_3d/broad_phase_octree.cpp index 1ace1a4fcf..11324fa4e4 100644 --- a/servers/physics_3d/broad_phase_octree.cpp +++ b/servers/physics_3d/broad_phase_octree.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ diff --git a/servers/physics_3d/broad_phase_octree.h b/servers/physics_3d/broad_phase_octree.h index 761a90a051..ee681dda96 100644 --- a/servers/physics_3d/broad_phase_octree.h +++ b/servers/physics_3d/broad_phase_octree.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp index e12f0659e2..293a7e6606 100644 --- a/servers/physics_3d/collision_object_3d_sw.cpp +++ b/servers/physics_3d/collision_object_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -43,7 +43,7 @@ void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform &p_trans p_shape->add_owner(this); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); @@ -56,7 +56,7 @@ void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) { p_shape->add_owner(this); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); @@ -68,7 +68,7 @@ void CollisionObject3DSW::set_shape_transform(int p_index, const Transform &p_tr shapes.write[p_index].xform = p_transform; shapes.write[p_index].xform_inv = p_transform.affine_inverse(); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); @@ -77,7 +77,7 @@ void CollisionObject3DSW::set_shape_transform(int p_index, const Transform &p_tr void CollisionObject3DSW::set_shape_as_disabled(int p_idx, bool p_enable) { shapes.write[p_idx].disabled = p_enable; if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } } @@ -106,7 +106,7 @@ void CollisionObject3DSW::remove_shape(int p_index) { shapes.remove(p_index); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index a3a5787ced..85221b7746 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -32,7 +32,7 @@ #define COLLISION_OBJECT_SW_H #include "broad_phase_3d_sw.h" -#include "core/self_list.h" +#include "core/templates/self_list.h" #include "servers/physics_server_3d.h" #include "shape_3d_sw.h" @@ -48,7 +48,8 @@ class CollisionObject3DSW : public ShapeOwner3DSW { public: enum Type { TYPE_AREA, - TYPE_BODY + TYPE_BODY, + TYPE_SOFT_BODY, }; private: @@ -129,8 +130,8 @@ public: _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_ Transform get_transform() const { return transform; } - _FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ const Transform &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform &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 85f55ad66d..9d5448dbfa 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -31,7 +31,38 @@ #include "collision_solver_3d_sat.h" #include "core/math/geometry_3d.h" -#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.02 +#include "gjk_epa.h" + +#define fallback_collision_solver gjk_epa_calculate_penetration + +// Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders. + +/* + * Cylinder-trimesh and Cylinder-box colliders by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ struct _CollectorCallback { CollisionSolver3DSW::CallbackResult callback; @@ -43,9 +74,9 @@ struct _CollectorCallback { _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { if (swap) { - callback(p_point_B, p_point_A, userdata); + callback(p_point_B, 0, p_point_A, 0, userdata); } else { - callback(p_point_A, p_point_B, userdata); + callback(p_point_A, 0, p_point_B, 0, userdata); } } }; @@ -82,6 +113,17 @@ static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point p_callback->call(*p_points_A, closest_B); } +static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A); + + p_callback->call(*p_points_A, closest_B); +} + static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 2); @@ -128,6 +170,104 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_ p_callback->call(closest_A, closest_B); } +static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + Plane circle_plane(circle_B_pos, circle_B_normal); + + static const int max_clip = 2; + Vector3 contact_points[max_clip]; + int num_points = 0; + + // Project edge point in circle plane. + const Vector3 &edge_A_1 = p_points_A[0]; + Vector3 proj_point_1 = circle_plane.project(edge_A_1); + + Vector3 dist_vec = proj_point_1 - circle_B_pos; + real_t dist_sq = dist_vec.length_squared(); + + // Point 1 is inside disk, add as contact point. + if (dist_sq <= circle_B_radius * circle_B_radius) { + contact_points[num_points] = edge_A_1; + ++num_points; + } + + const Vector3 &edge_A_2 = p_points_A[1]; + Vector3 proj_point_2 = circle_plane.project(edge_A_2); + + Vector3 dist_vec_2 = proj_point_2 - circle_B_pos; + real_t dist_sq_2 = dist_vec_2.length_squared(); + + // Point 2 is inside disk, add as contact point. + if (dist_sq_2 <= circle_B_radius * circle_B_radius) { + contact_points[num_points] = edge_A_2; + ++num_points; + } + + if (num_points < 2) { + Vector3 line_vec = proj_point_2 - proj_point_1; + real_t line_length_sq = line_vec.length_squared(); + + // Create a quadratic formula of the form ax^2 + bx + c = 0 + real_t a, b, c; + + a = line_length_sq; + b = 2.0 * dist_vec.dot(line_vec); + c = dist_sq - circle_B_radius * circle_B_radius; + + // Solve for t. + real_t sqrtterm = b * b - 4.0 * a * c; + + // If the term we intend to square root is less than 0 then the answer won't be real, + // so the line doesn't intersect. + if (sqrtterm >= 0) { + sqrtterm = Math::sqrt(sqrtterm); + + Vector3 edge_dir = edge_A_2 - edge_A_1; + + real_t fraction_1 = (-b - sqrtterm) / (2.0 * a); + if ((fraction_1 > 0.0) && (fraction_1 < 1.0)) { + Vector3 face_point_1 = edge_A_1 + fraction_1 * edge_dir; + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = face_point_1; + ++num_points; + } + + real_t fraction_2 = (-b + sqrtterm) / (2.0 * a); + if ((fraction_2 > 0.0) && (fraction_2 < 1.0) && !Math::is_equal_approx(fraction_1, fraction_2)) { + Vector3 face_point_2 = edge_A_1 + fraction_2 * edge_dir; + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = face_point_2; + ++num_points; + } + } + } + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A < 2); @@ -217,36 +357,229 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ } } -static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 3); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + // Clip face with circle segments. + static const int circle_segments = 8; + Vector3 circle_points[circle_segments]; + + real_t angle_delta = 2.0 * Math_PI / circle_segments; + + for (int i = 0; i < circle_segments; ++i) { + Vector3 point_pos = circle_B_pos; + point_pos += circle_B_line_1 * Math::cos(i * angle_delta); + point_pos += circle_B_line_2 * Math::sin(i * angle_delta); + circle_points[i] = point_pos; + } + + _generate_contacts_face_face(p_points_A, p_point_count_A, circle_points, circle_segments, p_callback); + + // Clip face with circle plane. + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + Plane circle_plane(circle_B_pos, circle_B_normal); + + static const int max_clip = 32; + Vector3 contact_points[max_clip]; + int num_points = 0; + + for (int i = 0; i < p_point_count_A; i++) { + int i_n = (i + 1) % p_point_count_A; + + const Vector3 &edge0_A = p_points_A[i]; + const Vector3 &edge1_A = p_points_A[i_n]; + + real_t dist0 = circle_plane.distance_to(edge0_A); + real_t dist1 = circle_plane.distance_to(edge1_A); + + // First point in front of plane, generate contact point. + if (dist0 * circle_plane.d >= 0) { + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = edge0_A; + ++num_points; + } + + // Points on different sides, generate contact point. + if (dist0 * dist1 < 0) { + // calculate intersection + Vector3 rel = edge1_A - edge0_A; + real_t den = circle_plane.normal.dot(rel); + real_t dist = -(circle_plane.normal.dot(edge0_A) - circle_plane.d) / den; + Vector3 inters = edge0_A + rel * dist; + + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = inters; + ++num_points; + } + } + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + +static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 3); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_A_pos = p_points_A[0]; + Vector3 circle_A_line_1 = p_points_A[1] - circle_A_pos; + Vector3 circle_A_line_2 = p_points_A[2] - circle_A_pos; + + real_t circle_A_radius = circle_A_line_1.length(); + Vector3 circle_A_normal = circle_A_line_1.cross(circle_A_line_2).normalized(); + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + static const int max_clip = 4; + Vector3 contact_points[max_clip]; + int num_points = 0; + + Vector3 centers_diff = circle_B_pos - circle_A_pos; + Vector3 norm_proj = circle_A_normal.dot(centers_diff) * circle_A_normal; + Vector3 comp_proj = centers_diff - norm_proj; + real_t proj_dist = comp_proj.length(); + if (!Math::is_zero_approx(proj_dist)) { + comp_proj /= proj_dist; + if ((proj_dist > circle_A_radius - circle_B_radius) && (proj_dist > circle_B_radius - circle_A_radius)) { + // Circles are overlapping, use the 2 points of intersection as contacts. + real_t radius_a_sqr = circle_A_radius * circle_A_radius; + real_t radius_b_sqr = circle_B_radius * circle_B_radius; + real_t d_sqr = proj_dist * proj_dist; + real_t s = (1.0 + (radius_a_sqr - radius_b_sqr) / d_sqr) * 0.5; + real_t h = Math::sqrt(MAX(radius_a_sqr - d_sqr * s * s, 0.0)); + Vector3 midpoint = circle_A_pos + s * comp_proj * proj_dist; + Vector3 h_vec = h * circle_A_normal.cross(comp_proj); + + Vector3 point_A = midpoint + h_vec; + contact_points[num_points] = point_A; + ++num_points; + + point_A = midpoint - h_vec; + contact_points[num_points] = point_A; + ++num_points; + + // Add 2 points from circle A and B along the line between the centers. + point_A = circle_A_pos + comp_proj * circle_A_radius; + contact_points[num_points] = point_A; + ++num_points; + + point_A = circle_B_pos - comp_proj * circle_B_radius - norm_proj; + contact_points[num_points] = point_A; + ++num_points; + } // Otherwise one circle is inside the other one, use 3 arbitrary equidistant points. + } // Otherwise circles are concentric, use 3 arbitrary equidistant points. + + if (num_points == 0) { + // Generate equidistant points. + if (circle_A_radius < circle_B_radius) { + // Circle A inside circle B. + for (int i = 0; i < 3; ++i) { + Vector3 circle_A_point = circle_A_pos; + circle_A_point += circle_A_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_A_point += circle_A_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + contact_points[num_points] = circle_A_point; + ++num_points; + } + } else { + // Circle B inside circle A. + for (int i = 0; i < 3; ++i) { + Vector3 circle_B_point = circle_B_pos; + circle_B_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_B_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + Vector3 circle_A_point = circle_B_point - norm_proj; + + contact_points[num_points] = circle_A_point; + ++num_points; + } + } + } + + Plane circle_B_plane(circle_B_pos, circle_B_normal); + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_B_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_B_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + +static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, Shape3DSW::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, Shape3DSW::FeatureType p_feature_type_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A < 1); ERR_FAIL_COND(p_point_count_B < 1); #endif - static const GenerateContactsFunc generate_contacts_func_table[3][3] = { + static const GenerateContactsFunc generate_contacts_func_table[4][4] = { { _generate_contacts_point_point, _generate_contacts_point_edge, _generate_contacts_point_face, + _generate_contacts_point_circle, }, { nullptr, _generate_contacts_edge_edge, _generate_contacts_face_face, + _generate_contacts_edge_circle, }, { nullptr, nullptr, _generate_contacts_face_face, - } + _generate_contacts_face_circle, + }, + { + nullptr, + nullptr, + nullptr, + _generate_contacts_circle_circle, + }, }; int pointcount_B; int pointcount_A; const Vector3 *points_A; const Vector3 *points_B; + int version_A; + int version_B; - if (p_point_count_A > p_point_count_B) { + if (p_feature_type_A > p_feature_type_B) { //swap p_callback->swap = !p_callback->swap; p_callback->normal = -p_callback->normal; @@ -255,16 +588,17 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po pointcount_A = p_point_count_B; points_A = p_points_B; points_B = p_points_A; + version_A = p_feature_type_B; + version_B = p_feature_type_A; } else { pointcount_B = p_point_count_B; pointcount_A = p_point_count_A; points_A = p_points_A; points_B = p_points_B; + version_A = p_feature_type_A; + version_B = p_feature_type_B; } - int version_A = (pointcount_A > 3 ? 3 : pointcount_A) - 1; - int version_B = (pointcount_B > 3 ? 3 : pointcount_B) - 1; - GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; ERR_FAIL_COND(!contacts_func); contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback); @@ -292,7 +626,7 @@ public: } } - _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { + _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis, bool p_directional = false) { Vector3 axis = p_axis; if (Math::abs(axis.x) < CMP_EPSILON && @@ -328,7 +662,12 @@ public: //use the smallest depth if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0 - min_B = -min_B; + if (p_directional) { + min_B = max_B; + axis = -axis; + } else { + min_B = -min_B; + } } if (max_B < min_B) { @@ -346,6 +685,17 @@ public: return true; } + static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { + SeparatorAxisTest<ShapeA, ShapeB, withMargin> *separator = (SeparatorAxisTest<ShapeA, ShapeB, withMargin> *)p_userdata; + Vector3 axis = (p_point_B - p_point_A); + real_t depth = axis.length(); + + // Filter out bogus directions with a treshold and re-testing axis. + if (separator->best_depth - depth > 0.001) { + separator->test_axis(axis / depth); + } + } + _FORCE_INLINE_ void generate_contacts() { // nothing to do, don't generate if (best_axis == Vector3(0.0, 0.0, 0.0)) { @@ -365,7 +715,8 @@ public: Vector3 supports_A[max_supports]; int support_count_A; - shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A); + Shape3DSW::FeatureType support_type_A; + shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A); for (int i = 0; i < support_count_A; i++) { supports_A[i] = transform_A->xform(supports_A[i]); } @@ -378,7 +729,8 @@ public: Vector3 supports_B[max_supports]; int support_count_B; - shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B); + Shape3DSW::FeatureType support_type_B; + shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B); for (int i = 0; i < support_count_B; i++) { supports_B[i] = transform_B->xform(supports_B[i]); } @@ -393,7 +745,7 @@ public: if (callback->prev_axis) { *callback->prev_axis = best_axis; } - _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); + _generate_contacts_from_supports(supports_A, support_count_A, support_type_A, supports_B, support_count_B, support_type_B, callback); callback->collided = true; } @@ -498,7 +850,7 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t //capsule sphere 1, sphere - Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); + Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5); Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; @@ -529,6 +881,61 @@ 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) { + const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<SphereShape3DSW, CylinderShape3DSW, withMargin> separator(sphere_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder B end caps. + Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + if (!separator.test_axis(cylinder_B_axis)) { + return; + } + + Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; + + // Cylinder B lateral surface. + if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + // Closest point to cylinder caps. + const Vector3 &sphere_center = p_transform_a.origin; + Vector3 cyl_axis = p_transform_b.basis.get_axis(1); + Vector3 cap_axis = p_transform_b.basis.get_axis(0); + real_t height_scale = cyl_axis.length(); + real_t cap_dist = cylinder_B->get_height() * 0.5 * height_scale; + cyl_axis /= height_scale; + real_t radius_scale = cap_axis.length(); + real_t cap_radius = cylinder_B->get_radius() * radius_scale; + + for (int i = 0; i < 2; i++) { + Vector3 cap_dir = ((i == 0) ? cyl_axis : -cyl_axis); + Vector3 cap_pos = p_transform_b.origin + cap_dir * cap_dist; + + Vector3 closest_point; + + Vector3 diff = sphere_center - cap_pos; + Vector3 proj = diff - cap_dir.dot(diff) * cap_dir; + + real_t proj_len = proj.length(); + if (Math::is_zero_approx(proj_len)) { + // Point is equidistant to all circle points. + continue; + } + + closest_point = cap_pos + (cap_radius / proj_len) * proj; + + if (!separator.test_axis((closest_point - sphere_center).normalized())) { + return; + } + } + + separator.generate_contacts(); } template <bool withMargin> @@ -604,23 +1011,31 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_tran p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal, !face_B->backface_collision)) { return; } // edges and points of B for (int i = 0; i < 3; i++) { Vector3 n1 = vertex[i] - p_transform_a.origin; + if (n1.dot(normal) < 0.0) { + n1 *= -1.0; + } - if (!separator.test_axis(n1.normalized())) { + if (!separator.test_axis(n1.normalized(), !face_B->backface_collision)) { return; } Vector3 n2 = vertex[(i + 1) % 3] - vertex[i]; Vector3 axis = n1.cross(n2).cross(n2).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } - if (!separator.test_axis(axis)) { + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } @@ -739,14 +1154,14 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i); + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); if (!separator.test_axis(axis)) { return; } } - Vector3 cyl_axis = p_transform_b.basis.get_axis(2).normalized(); + Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); // edges of A, capsule cylinder @@ -791,7 +1206,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // capsule balls, edges of A for (int i = 0; i < 2; i++) { - Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); + Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5); Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -826,6 +1241,115 @@ 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) { + const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<BoxShape3DSW, CylinderShape3DSW, withMargin> separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Faces of A. + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + + // Cylinder end caps. + { + if (!separator.test_axis(cyl_axis)) { + return; + } + } + + // Edges of A, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 axis = box_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // Gather points of A. + Vector3 vertices_A[8]; + Vector3 box_extent = box_A->get_half_extents(); + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 extent = box_extent; + extent.x *= (i * 2 - 1); + extent.y *= (j * 2 - 1); + extent.z *= (k * 2 - 1); + Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k]; + point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_axis(l) * extent[l]; + } + } + } + } + + // Points of A, cylinder lateral surface. + for (int i = 0; i < 8; i++) { + const Vector3 &point = vertices_A[i]; + Vector3 axis = Plane(cyl_axis, 0).project(point).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Edges of A, cylinder end caps rim. + int edges_start_A[12] = { 0, 2, 4, 6, 0, 1, 4, 5, 0, 1, 2, 3 }; + int edges_end_A[12] = { 1, 3, 5, 7, 2, 3, 6, 7, 4, 5, 6, 7 }; + + Vector3 cap_axis = cyl_axis * (cylinder_B->get_height() * 0.5); + + for (int i = 0; i < 2; i++) { + Vector3 cap_pos = p_transform_b.origin + ((i == 0) ? cap_axis : -cap_axis); + + for (int e = 0; e < 12; e++) { + const Vector3 &edge_start = vertices_A[edges_start_A[e]]; + const Vector3 &edge_end = vertices_A[edges_end_A[e]]; + + Vector3 edge_dir = (edge_end - edge_start); + edge_dir.normalize(); + + real_t edge_dot = edge_dir.dot(cyl_axis); + if (Math::is_zero_approx(edge_dot)) { + // Edge is perpendicular to cylinder axis. + continue; + } + + // Calculate intersection between edge and circle plane. + Vector3 edge_diff = cap_pos - edge_start; + real_t diff_dot = edge_diff.dot(cyl_axis); + Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; + + // Calculate tangent that touches intersection. + Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); + + // Axis is orthogonal both to tangent and edge direction. + Vector3 axis = tangent.cross(edge_dir); + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + separator.generate_contacts(); } template <bool withMargin> @@ -956,15 +1480,20 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal, !face_B->backface_collision)) { return; } // faces of A for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } - if (!separator.test_axis(axis)) { + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } @@ -975,9 +1504,12 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo Vector3 e = vertex[i] - vertex[(i + 1) % 3]; for (int j = 0; j < 3; j++) { - Vector3 axis = p_transform_a.basis.get_axis(j); + Vector3 axis = e.cross(p_transform_a.basis.get_axis(j)).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } - if (!separator.test_axis(e.cross(axis).normalized())) { + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } @@ -997,8 +1529,11 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); Vector3 axis_ab = support_a - vertex[v]; + if (axis_ab.dot(normal) < 0.0) { + axis_ab *= -1.0; + } - if (!separator.test_axis(axis_ab.normalized())) { + if (!separator.test_axis(axis_ab.normalized(), !face_B->backface_collision)) { return; } @@ -1008,7 +1543,12 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo //a ->b Vector3 axis_a = p_transform_a.basis.get_axis(i); - if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { + Vector3 axis = axis_ab.cross(axis_a).cross(axis_a).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } @@ -1033,7 +1573,12 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo Vector3 n = (p2 - p1); - if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) { + Vector3 axis = (point - p2).cross(n).cross(n).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } @@ -1058,8 +1603,8 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_ // some values - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); - Vector3 capsule_B_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); + Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5); + Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5); Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis; Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis; @@ -1111,6 +1656,64 @@ 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) { + const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin> separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder B end caps. + Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + if (!separator.test_axis(cylinder_B_axis)) { + return; + } + + // Cylinder edge against capsule balls. + + Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1); + + Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5); + Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5); + + if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_1).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) { + return; + } + + if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_2).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) { + return; + } + + // Cylinder edge against capsule edge. + + Vector3 center_diff = p_transform_b.origin - p_transform_a.origin; + + if (!separator.test_axis(capsule_A_axis.cross(center_diff).cross(capsule_A_axis).normalized())) { + return; + } + + if (!separator.test_axis(cylinder_B_axis.cross(center_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + real_t proj = capsule_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(capsule_A_axis); + if (Math::is_zero_approx(proj)) { + // Parallel capsule and cylinder axes, handle with specific axes only. + // Note: GJKEPA with no margin can lead to degenerate cases in this situation. + separator.generate_contacts(); + return; + } + + CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { + return; + } + + separator.generate_contacts(); } template <bool withMargin> @@ -1146,7 +1749,7 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf for (int i = 0; i < edge_count; i++) { // cylinder Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]); - Vector3 axis = edge_axis.cross(p_transform_a.basis.get_axis(2)).normalized(); + Vector3 axis = edge_axis.cross(p_transform_a.basis.get_axis(1)).normalized(); if (!separator.test_axis(axis)) { return; @@ -1158,7 +1761,7 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf for (int i = 0; i < 2; i++) { // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); + Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5); Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -1190,24 +1793,35 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal, !face_B->backface_collision)) { return; } // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); + Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5); for (int i = 0; i < 3; i++) { // edge-cylinder Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; + Vector3 axis = edge_axis.cross(capsule_axis).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } - if (!separator.test_axis(axis)) { + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } - if (!separator.test_axis((p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized())) { + Vector3 dir_axis = (p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized(); + if (dir_axis.dot(normal) < 0.0) { + dir_axis *= -1.0; + } + + if (!separator.test_axis(dir_axis, !face_B->backface_collision)) { return; } @@ -1216,16 +1830,22 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis); Vector3 n1 = sphere_pos - vertex[i]; + if (n1.dot(normal) < 0.0) { + n1 *= -1.0; + } - if (!separator.test_axis(n1.normalized())) { + if (!separator.test_axis(n1.normalized(), !face_B->backface_collision)) { return; } Vector3 n2 = edge_axis; axis = n1.cross(n2).cross(n2); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } - if (!separator.test_axis(axis.normalized())) { + if (!separator.test_axis(axis.normalized(), !face_B->backface_collision)) { return; } } @@ -1236,14 +1856,178 @@ 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) { + const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1); + Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder A end caps. + if (!separator.test_axis(cylinder_A_axis.normalized())) { + return; + } + + // Cylinder B end caps. + if (!separator.test_axis(cylinder_A_axis.normalized())) { + return; + } + + Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; + + // Cylinder A lateral surface. + if (!separator.test_axis(cylinder_A_axis.cross(cylinder_diff).cross(cylinder_A_axis).normalized())) { + return; + } + + // Cylinder B lateral surface. + if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + real_t proj = cylinder_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(cylinder_A_axis); + if (Math::is_zero_approx(proj)) { + // Parallel cylinders, handle with specific axes only. + // Note: GJKEPA with no margin can lead to degenerate cases in this situation. + separator.generate_contacts(); + return; + } + + CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { + return; + } + + separator.generate_contacts(); } 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) { + const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); + const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); + + SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { + return; + } + + separator.generate_contacts(); } 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) { + const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); + const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); + + SeparatorAxisTest<CylinderShape3DSW, FaceShape3DSW, withMargin> separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + // Face B normal. + if (!separator.test_axis(normal, !face_B->backface_collision)) { + return; + } + + Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized(); + if (cyl_axis.dot(normal) < 0.0) { + cyl_axis *= -1.0; + } + + // Cylinder end caps. + if (!separator.test_axis(cyl_axis, !face_B->backface_collision)) { + return; + } + + // Edges of B, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; + Vector3 axis = edge_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis.normalized(), !face_B->backface_collision)) { + return; + } + } + + // Points of B, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + const Vector3 &point = vertex[i]; + Vector3 axis = Plane(cyl_axis, 0).project(point).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis, !face_B->backface_collision)) { + return; + } + } + + // Edges of B, cylinder end caps rim. + Vector3 cap_axis = cyl_axis * (cylinder_A->get_height() * 0.5); + + for (int i = 0; i < 2; i++) { + Vector3 cap_pos = p_transform_a.origin + ((i == 0) ? cap_axis : -cap_axis); + + for (int j = 0; j < 3; j++) { + const Vector3 &edge_start = vertex[j]; + const Vector3 &edge_end = vertex[(j + 1) % 3]; + Vector3 edge_dir = edge_end - edge_start; + edge_dir.normalize(); + + real_t edge_dot = edge_dir.dot(cyl_axis); + if (Math::is_zero_approx(edge_dot)) { + // Edge is perpendicular to cylinder axis. + continue; + } + + // Calculate intersection between edge and circle plane. + Vector3 edge_diff = cap_pos - edge_start; + real_t diff_dot = edge_diff.dot(cyl_axis); + Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; + + // Calculate tangent that touches intersection. + Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); + + // Axis is orthogonal both to tangent and edge direction. + Vector3 axis = tangent.cross(edge_dir); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis.normalized(), !face_B->backface_collision)) { + return; + } + } + } + + separator.generate_contacts(); } template <bool withMargin> @@ -1377,7 +2161,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal, !face_B->backface_collision)) { return; } @@ -1385,8 +2171,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform for (int i = 0; i < face_count; i++) { //Vector3 axis = p_transform_a.xform( faces[i].plane ).normal; Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } - if (!separator.test_axis(axis)) { + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } @@ -1399,8 +2188,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform Vector3 e2 = vertex[j] - vertex[(j + 1) % 3]; Vector3 axis = e1.cross(e2).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } - if (!separator.test_axis(axis)) { + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } @@ -1412,7 +2204,12 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform Vector3 va = p_transform_a.xform(vertices[i]); for (int j = 0; j < 3; j++) { - if (!separator.test_axis((va - vertex[j]).normalized())) { + Vector3 axis = (va - vertex[j]).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } @@ -1427,7 +2224,12 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform for (int j = 0; j < 3; j++) { Vector3 e3 = vertex[j]; - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { + Vector3 axis = (e1 - e3).cross(n).cross(n).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } @@ -1441,7 +2243,12 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform for (int j = 0; j < vertex_count; j++) { Vector3 e3 = p_transform_a.xform(vertices[j]); - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { + Vector3 axis = (e1 - e3).cross(n).cross(n).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } diff --git a/servers/physics_3d/collision_solver_3d_sat.h b/servers/physics_3d/collision_solver_3d_sat.h index 5eccfda9ac..97454c0b4a 100644 --- a/servers/physics_3d/collision_solver_3d_sat.h +++ b/servers/physics_3d/collision_solver_3d_sat.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index e2bfaf990d..f655c4626c 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -30,6 +30,7 @@ #include "collision_solver_3d_sw.h" #include "collision_solver_3d_sat.h" +#include "soft_body_3d_sw.h" #include "gjk_epa.h" @@ -46,8 +47,24 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T static const int max_supports = 16; Vector3 supports[max_supports]; int support_count; - - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); + Shape3DSW::FeatureType support_type; + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); + + if (support_type == Shape3DSW::FEATURE_CIRCLE) { + ERR_FAIL_COND_V(support_count != 3, false); + + Vector3 circle_pos = supports[0]; + Vector3 circle_axis_1 = supports[1] - circle_pos; + Vector3 circle_axis_2 = supports[2] - circle_pos; + + // Use 3 equidistant points on the circle. + for (int i = 0; i < 3; ++i) { + Vector3 vertex_pos = circle_pos; + vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); + vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); + supports[i] = vertex_pos; + } + } bool found = false; @@ -62,9 +79,9 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T if (p_result_callback) { if (p_swap_result) { - p_result_callback(supports[i], support_A, p_userdata); + p_result_callback(supports[i], 0, support_A, 0, p_userdata); } else { - p_result_callback(support_A, supports[i], p_userdata); + p_result_callback(support_A, 0, supports[i], 0, p_userdata); } } } @@ -97,14 +114,148 @@ bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform if (p_result_callback) { if (p_swap_result) { - p_result_callback(support_B, support_A, p_userdata); + p_result_callback(support_B, 0, support_A, 0, p_userdata); } else { - p_result_callback(support_A, support_B, p_userdata); + p_result_callback(support_A, 0, support_B, 0, p_userdata); } } return true; } +struct _SoftBodyContactCollisionInfo { + int node_index = 0; + CollisionSolver3DSW::CallbackResult result_callback = nullptr; + void *userdata = nullptr; + bool swap_result = false; + int contact_count = 0; +}; + +void CollisionSolver3DSW::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) { + _SoftBodyContactCollisionInfo &cinfo = *(_SoftBodyContactCollisionInfo *)(p_userdata); + + ++cinfo.contact_count; + + if (cinfo.swap_result) { + cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata); + } else { + cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, cinfo.userdata); + } +} + +struct _SoftBodyQueryInfo { + SoftBody3DSW *soft_body = nullptr; + const Shape3DSW *shape_A = nullptr; + const Shape3DSW *shape_B = nullptr; + Transform transform_A; + Transform node_transform; + _SoftBodyContactCollisionInfo contact_info; +#ifdef DEBUG_ENABLED + int node_query_count = 0; + int convex_query_count = 0; +#endif +}; + +bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) { + _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); + + Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index); + + Transform transform_B; + transform_B.origin = query_cinfo.node_transform.xform(node_position); + + query_cinfo.contact_info.node_index = p_node_index; + solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info); + +#ifdef DEBUG_ENABLED + ++query_cinfo.node_query_count; +#endif + + // Continue with the query. + return false; +} + +void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) { + _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); + + query_cinfo.shape_A = p_convex; + + // Calculate AABB for internal soft body query (in world space). + AABB shape_aabb; + for (int i = 0; i < 3; i++) { + Vector3 axis; + axis[i] = 1.0; + + real_t smin, smax; + p_convex->project_range(axis, query_cinfo.transform_A, smin, smax); + + shape_aabb.position[i] = smin; + shape_aabb.size[i] = smax - smin; + } + + shape_aabb.grow_by(query_cinfo.soft_body->get_collision_margin()); + + query_cinfo.soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo); + +#ifdef DEBUG_ENABLED + ++query_cinfo.convex_query_count; +#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) { + 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 real_t collision_margin = soft_body->get_collision_margin(); + + SphereShape3DSW sphere_shape; + sphere_shape.set_data(collision_margin); + + _SoftBodyQueryInfo query_cinfo; + query_cinfo.contact_info.result_callback = p_result_callback; + query_cinfo.contact_info.userdata = p_userdata; + query_cinfo.contact_info.swap_result = p_swap_result; + query_cinfo.soft_body = soft_body; + query_cinfo.node_transform = p_transform_B * world_to_local; + query_cinfo.shape_A = p_shape_A; + query_cinfo.transform_A = p_transform_A; + query_cinfo.shape_B = &sphere_shape; + + if (p_shape_A->is_concave()) { + // In case of concave shape, query convex shapes first. + const ConcaveShape3DSW *concave_shape_A = static_cast<const ConcaveShape3DSW *>(p_shape_A); + + AABB soft_body_aabb = soft_body->get_bounds(); + soft_body_aabb.grow_by(collision_margin); + + // Calculate AABB for internal concave shape query (in local space). + AABB local_aabb; + for (int i = 0; i < 3; i++) { + Vector3 axis(p_transform_A.basis.get_axis(i)); + real_t axis_scale = 1.0 / axis.length(); + + real_t smin = soft_body_aabb.position[i]; + real_t smax = smin + soft_body_aabb.size[i]; + + smin *= axis_scale; + smax *= axis_scale; + + local_aabb.position[i] = smin; + local_aabb.size[i] = smax - smin; + } + + concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo); + } else { + AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb()); + shape_aabb.grow_by(collision_margin); + + soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo); + } + + return (query_cinfo.contact_info.contact_count > 0); +} + struct _ConcaveCollisionInfo { const Transform *transform_A; const Shape3DSW *shape_A; @@ -199,6 +350,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo if (type_B == PhysicsServer3D::SHAPE_RAY) { return false; } + if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { + return false; + } if (swap) { return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); @@ -217,6 +371,18 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); } + } else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { + if (type_A == PhysicsServer3D::SHAPE_SOFT_BODY) { + // Soft Body / Soft Body not supported. + return false; + } + + if (swap) { + return solve_soft_body(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + } else { + return solve_soft_body(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + } + } else if (concave_B) { if (concave_A) { return false; @@ -265,8 +431,25 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const static const int max_supports = 16; Vector3 supports[max_supports]; int support_count; + Shape3DSW::FeatureType support_type; + + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); + if (support_type == Shape3DSW::FEATURE_CIRCLE) { + ERR_FAIL_COND_V(support_count != 3, false); + + Vector3 circle_pos = supports[0]; + Vector3 circle_axis_1 = supports[1] - circle_pos; + Vector3 circle_axis_2 = supports[2] - circle_pos; + + // Use 3 equidistant points on the circle. + for (int i = 0; i < 3; ++i) { + Vector3 vertex_pos = circle_pos; + vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); + vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); + supports[i] = vertex_pos; + } + } bool collided = false; Vector3 closest; diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h index 13f54ca8fb..34ac2c6d3f 100644 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -35,12 +35,16 @@ class CollisionSolver3DSW { public: - typedef void (*CallbackResult)(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); + typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); private: + static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata); + static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); + static void soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex); static void concave_callback(void *p_userdata, Shape3DSW *p_convex); static bool 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 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); diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h index 081ddb0382..2571335c43 100644 --- a/servers/physics_3d/constraint_3d_sw.h +++ b/servers/physics_3d/constraint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index d99a2532f8..1a8c7f704f 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -64,7 +64,7 @@ GJK-EPA collision solver by Nathanael Presson, 2008 /* GJK */ #define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((real_t)0.0001) +#define GJK_ACCURACY ((real_t)0.0001) #define GJK_MIN_DISTANCE ((real_t)0.0001) #define GJK_DUPLICATED_EPS ((real_t)0.0001) #define GJK_SIMPLEX2_EPS ((real_t)0.0) @@ -72,10 +72,13 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #define GJK_SIMPLEX4_EPS ((real_t)0.0) /* EPA */ -#define EPA_MAX_VERTICES 64 +#define EPA_MAX_VERTICES 128 #define EPA_MAX_FACES (EPA_MAX_VERTICES*2) #define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((real_t)0.0001) +// -- GODOT start -- +//#define EPA_ACCURACY ((real_t)0.0001) +#define EPA_ACCURACY ((real_t)0.00001) +// -- GODOT end -- #define EPA_FALLBACK (10*EPA_ACCURACY) #define EPA_PLANE_EPS ((real_t)0.00001) #define EPA_INSIDE_EPS ((real_t)0.01) @@ -102,33 +105,65 @@ typedef unsigned char U1; // MinkowskiDiff struct MinkowskiDiff { - const Shape3DSW* m_shapes[2]; Transform transform_A; Transform 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) { + m_shapes[0] = shape0; + m_shapes[1] = shape1; + transform_A = wtrs0; + transform_B = wtrs1; + margin_A = margin0; + margin_B = margin1; + + if ((margin0 > 0.0) || (margin1 > 0.0)) { + get_support = get_support_with_margin; + } else { + get_support = get_support_without_margin; + } + } + + static Vector3 get_support_without_margin(const Shape3DSW* p_shape, const Vector3& p_dir, real_t p_margin) { + return p_shape->get_support(p_dir.normalized()); + } + + static Vector3 get_support_with_margin(const Shape3DSW* p_shape, const Vector3& p_dir, real_t p_margin) { + Vector3 local_dir_norm = p_dir; + if (local_dir_norm.length_squared() < CMP_EPSILON2) { + local_dir_norm = Vector3(-1.0, -1.0, -1.0); + } + local_dir_norm.normalize(); + + return p_shape->get_support(local_dir_norm) + p_margin * local_dir_norm; + } + // i wonder how this could be sped up... if it can - _FORCE_INLINE_ Vector3 Support0 ( const Vector3& d ) const { - return transform_A.xform( m_shapes[0]->get_support( transform_A.basis.xform_inv(d).normalized() ) ); + _FORCE_INLINE_ Vector3 Support0(const Vector3& d) const { + return transform_A.xform(get_support(m_shapes[0], transform_A.basis.xform_inv(d), margin_A)); } - _FORCE_INLINE_ Vector3 Support1 ( const Vector3& d ) const { - return transform_B.xform( m_shapes[1]->get_support( transform_B.basis.xform_inv(d).normalized() ) ); + _FORCE_INLINE_ Vector3 Support1(const Vector3& d) const { + return transform_B.xform(get_support(m_shapes[1], transform_B.basis.xform_inv(d), margin_B)); } - _FORCE_INLINE_ Vector3 Support ( const Vector3& d ) const { - return ( Support0 ( d )-Support1 ( -d ) ); + _FORCE_INLINE_ Vector3 Support (const Vector3& d) const { + return (Support0(d) - Support1(-d)); } - _FORCE_INLINE_ Vector3 Support ( const Vector3& d,U index ) const - { - if ( index ) { - return ( Support1 ( d ) ); + _FORCE_INLINE_ Vector3 Support(const Vector3& d, U index) const { + if (index) { + return Support1(d); } else { - return ( Support0 ( d ) ); - -} + return Support0(d); + } } }; @@ -239,7 +274,7 @@ struct GJK /* Check for termination */ const real_t omega=vec3_dot(m_ray,w)/rl; alpha=MAX(omega,alpha); - if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) + if(((rl-alpha)-(GJK_ACCURACY*rl))<=0) {/* Return old simplex */ removevertice(m_simplices[m_current]); break; @@ -281,7 +316,6 @@ struct GJK } } if(mask==15) { m_status=eStatus::Inside; - } } else @@ -312,12 +346,10 @@ struct GJK axis[i]=1; appendvertice(*m_simplex, axis); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); appendvertice(*m_simplex,-axis); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); } @@ -335,12 +367,10 @@ struct GJK { appendvertice(*m_simplex, p); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); appendvertice(*m_simplex,-p); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); } @@ -355,12 +385,10 @@ struct GJK { appendvertice(*m_simplex,n); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); appendvertice(*m_simplex,-n); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); } @@ -372,7 +400,6 @@ struct GJK m_simplex->c[1]->w-m_simplex->c[3]->w, m_simplex->c[2]->w-m_simplex->c[3]->w))>0) { return(true); - } } break; @@ -476,7 +503,7 @@ struct GJK if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS)) { real_t mindist=-1; - real_t subw[3]; + real_t subw[3] = {0.f, 0.f, 0.f}; U subm=0; for(U i=0;i<3;++i) { @@ -522,7 +549,6 @@ struct GJK { Vector3 n; real_t d; - real_t p; sSV* c[3]; sFace* f[3]; sFace* l[2]; @@ -580,7 +606,6 @@ struct GJK face->l[0] = nullptr; face->l[1] = list.root; if(list.root) { list.root->l[0]=face; - } list.root = face; ++list.count; @@ -588,13 +613,10 @@ struct GJK static inline void remove(sList& list,sFace* face) { if(face->l[1]) { face->l[1]->l[0]=face->l[0]; - } if(face->l[0]) { face->l[0]->l[1]=face->l[1]; - } if(face==list.root) { list.root=face->l[1]; - } --list.count; } @@ -616,7 +638,6 @@ struct GJK GJK::sSimplex& simplex=*gjk.m_simplex; if((simplex.rank>1)&&gjk.EncloseOrigin()) { - /* Clean up */ while(m_hull.root) { @@ -676,9 +697,7 @@ struct GJK remove(m_hull,best); append(m_stock,best); best=findbest(); - if(best->p>=outer.p) { outer=*best; - -} + outer=*best; } else { m_status=eStatus::InvalidHull;break; } } else { m_status=eStatus::AccuraryReached;break; } } else { m_status=eStatus::OutOfVertices;break; } @@ -704,25 +723,54 @@ struct GJK } } /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const real_t nl=m_normal.length(); - if(nl>0) { - m_normal = m_normal/nl; + m_status = eStatus::FallBack; + m_normal = -guess; + const real_t nl = m_normal.length(); + if (nl > 0) { + m_normal = m_normal/nl; } else { - m_normal = Vector3(1,0,0); - -} + m_normal = Vector3(1,0,0); + } m_depth = 0; m_result.rank=1; m_result.c[0]=simplex.c[0]; m_result.p[0]=1; return(m_status); } + + bool getedgedist(sFace* face, sSV* a, sSV* b, real_t& dist) + { + const Vector3 ba = b->w - a->w; + const Vector3 n_ab = vec3_cross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const real_t a_dot_nab = vec3_dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if (a_dot_nab < 0) { + // Outside of edge a->b + const real_t ba_l2 = ba.length_squared(); + const real_t a_dot_ba = vec3_dot(a->w, ba); + const real_t b_dot_ba = vec3_dot(b->w, ba); + + if (a_dot_ba > 0) { + // Pick distance vertex a + dist = a->w.length(); + } else if (b_dot_ba < 0) { + // Pick distance vertex b + dist = b->w.length(); + } else { + // Pick distance to edge a->b + const real_t a_dot_b = vec3_dot(a->w, b->w); + dist = Math::sqrt(MAX((a->w.length_squared() * b->w.length_squared() - a_dot_b * a_dot_b) / ba_l2, 0.0)); + } + + return true; + } + + return false; + } + sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) { - if(m_stock.root) - { + if (m_stock.root) { sFace* face=m_stock.root; remove(m_stock,face); append(m_hull,face); @@ -733,25 +781,23 @@ struct GJK face->n = vec3_cross(b->w-a->w,c->w-a->w); const real_t l=face->n.length(); const bool v=l>EPA_ACCURACY; - face->p = MIN(MIN( - vec3_dot(a->w,vec3_cross(face->n,a->w-b->w)), - vec3_dot(b->w,vec3_cross(face->n,b->w-c->w))), - vec3_dot(c->w,vec3_cross(face->n,c->w-a->w))) / - (v?l:1); - face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; - if(v) - { - face->d = vec3_dot(a->w,face->n)/l; + if (v) { + if (!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = vec3_dot(a->w, face->n) / l; + } face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) - { + if (forced||(face->d>=-EPA_PLANE_EPS)) { return(face); - } else { m_status=eStatus::NonConvex; - -} - } else { m_status=eStatus::Degenerated; - -} + } else { + m_status=eStatus::NonConvex; + } + } else { + m_status=eStatus::Degenerated; + } remove(m_hull,face); append(m_stock,face); return(nullptr); @@ -766,15 +812,13 @@ struct GJK { sFace* minf=m_hull.root; real_t mind=minf->d*minf->d; - real_t maxp=minf->p; for(sFace* f=minf->l[1];f;f=f->l[1]) { const real_t sqd=f->d*f->d; - if((f->p>=maxp)&&(sqd<mind)) + if(sqd<mind) { minf=f; mind=sqd; - maxp=f->p; } } return(minf); @@ -793,7 +837,6 @@ struct GJK { bind(nf,0,f,e); if(horizon.cf) { bind(horizon.cf,1,nf,2); } else { horizon.ff=nf; - } horizon.cf=nf; ++horizon.nf; @@ -819,22 +862,17 @@ struct GJK }; // - static void Initialize( const Shape3DSW* shape0,const Transform& wtrs0, - const Shape3DSW* shape1,const Transform& wtrs1, + static void Initialize( const Shape3DSW* shape0, const Transform& wtrs0, real_t margin0, + const Shape3DSW* shape1, const Transform& wtrs1, real_t margin1, sResults& results, - tShape& shape, - bool withmargins) + tShape& shape) { /* Results */ - results.witnesses[0] = - results.witnesses[1] = Vector3(0,0,0); + results.witnesses[0] = Vector3(0,0,0); + results.witnesses[1] = Vector3(0,0,0); results.status = sResults::Separated; /* Shape */ - shape.m_shapes[0] = shape0; - shape.m_shapes[1] = shape1; - shape.transform_A = wtrs0; - shape.transform_B = wtrs1; - + shape.Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1); } @@ -848,13 +886,15 @@ struct GJK // bool Distance( const Shape3DSW* shape0, const Transform& wtrs0, - const Shape3DSW* shape1, + real_t margin0, + const Shape3DSW* shape1, const Transform& wtrs1, + real_t margin1, const Vector3& guess, sResults& results) { tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); + Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape); GJK gjk; GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); if(gjk_status==GJK::eStatus::Valid) @@ -887,14 +927,16 @@ bool Distance( const Shape3DSW* shape0, // bool Penetration( const Shape3DSW* shape0, const Transform& wtrs0, - const Shape3DSW* shape1, + real_t margin0, + const Shape3DSW* shape1, const Transform& wtrs1, - const Vector3& guess, + real_t margin1, + const Vector3& guess, sResults& results ) { tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); + Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape); GJK gjk; GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); switch(gjk_status) @@ -917,7 +959,6 @@ bool Penetration( const Shape3DSW* shape0, results.distance = -epa.m_depth; return(true); } else { results.status=sResults::EPA_Failed; - } } break; @@ -948,8 +989,6 @@ bool Penetration( const Shape3DSW* shape0, #undef EPA_FALLBACK #undef EPA_PLANE_EPS #undef EPA_INSIDE_EPS - - } // end of namespace /* clang-format on */ @@ -957,7 +996,7 @@ bool Penetration( const Shape3DSW* shape0, 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) { GjkEpa2::sResults res; - if (GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, 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)) { r_result_A = res.witnesses[0]; r_result_B = res.witnesses[1]; return true; @@ -966,15 +1005,15 @@ 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) { +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) { GjkEpa2::sResults res; - if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, 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)) { if (p_result_callback) { if (p_swap) { - p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata); + p_result_callback(res.witnesses[1], 0, res.witnesses[0], 0, p_userdata); } else { - p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata); + p_result_callback(res.witnesses[0], 0, res.witnesses[1], 0, p_userdata); } } return true; diff --git a/servers/physics_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h index dec0f269e1..a7e2e1719e 100644 --- a/servers/physics_3d/gjk_epa.h +++ b/servers/physics_3d/gjk_epa.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -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); +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); #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 789d6687a4..9c4493f4a2 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -92,9 +92,9 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans m_rbAFrame = rbAFrame; m_rbBFrame = rbBFrame; - m_swingSpan1 = Math_PI / 4.0; - m_swingSpan2 = Math_PI / 4.0; - m_twistSpan = Math_PI * 2; + m_swingSpan1 = Math_TAU / 8.0; + m_swingSpan2 = Math_TAU / 8.0; + m_twistSpan = Math_TAU; m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; 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 c713d8cf17..4e4d4e7f0c 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -102,7 +102,7 @@ public: bool m_solveSwingLimit; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_CONE_TWIST; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } virtual bool setup(real_t p_timestep); virtual void solve(real_t p_timestep); 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 fede40ca65..13b389251f 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -132,7 +132,7 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( real_t oldaccumImpulse = m_accumulatedImpulse; real_t sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; + m_accumulatedImpulse = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum); clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; @@ -201,7 +201,7 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( real_t oldNormalImpulse = m_accumulatedImpulse[limit_index]; real_t sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; + m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum); normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; Vector3 impulse_vector = axis_normal_on_a * normalImpulse; @@ -253,7 +253,6 @@ void Generic6DOFJoint3DSW::calculateAngleInfo() { /* if(m_debugDrawer) { - char buff[300]; sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", m_calculatedAxisAngleDiff[0], 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 cc1423a1cb..d61a033231 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -103,19 +103,6 @@ public: m_enableLimit = false; } - G6DOFRotationalLimitMotor3DSW(const G6DOFRotationalLimitMotor3DSW &limot) { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - m_ERP = limot.m_ERP; - m_bounce = limot.m_bounce; - m_currentLimit = limot.m_currentLimit; - m_currentLimitError = limot.m_currentLimitError; - m_enableMotor = limot.m_enableMotor; - } - //! Is limited bool isLimited() { return (m_loLimit < m_hiLimit); @@ -163,16 +150,6 @@ public: enable_limit[2] = true; } - G6DOFTranslationalLimitMotor3DSW(const G6DOFTranslationalLimitMotor3DSW &other) { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; - - m_limitSoftness = other.m_limitSoftness; - m_damping = other.m_damping; - m_restitution = other.m_restitution; - } - //! Test limit /*! - free means upper < lower, @@ -242,11 +219,8 @@ protected: //!@} - Generic6DOFJoint3DSW &operator=(Generic6DOFJoint3DSW &other) { - ERR_PRINT("pito"); - (void)other; - return *this; - } + Generic6DOFJoint3DSW(Generic6DOFJoint3DSW const &) = delete; + void operator=(Generic6DOFJoint3DSW const &) = delete; void buildLinearJacobian( JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld, @@ -260,7 +234,7 @@ protected: public: Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_6DOF; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_6DOF; } virtual bool setup(real_t p_timestep); virtual void solve(real_t p_timestep); diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index 52c7389e1f..2b9f0038b4 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -365,7 +365,6 @@ void HingeJoint3DSW::solve(real_t p_step) { void HingeJointSW::updateRHS(real_t timeStep) { (void)timeStep; - } */ diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index c5af888eca..b6117aa0bc 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -96,7 +96,7 @@ class HingeJoint3DSW : public Joint3DSW { real_t m_appliedImpulse; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_HINGE; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_HINGE; } virtual bool setup(real_t p_step); virtual void solve(real_t p_step); diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/jacobian_entry_3d_sw.h index 1737c21b3d..2829a5caf7 100644 --- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h +++ b/servers/physics_3d/joints/jacobian_entry_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index f028ad88f9..9f708ce151 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h index 0181a4455b..1875983527 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -74,7 +74,7 @@ class PinJoint3DSW : public Joint3DSW { Vector3 m_pivotInB; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_PIN; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_PIN; } virtual bool setup(real_t p_step); virtual void solve(real_t p_step); diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index 43bd49b4b5..0adc471797 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -105,7 +105,6 @@ void SliderJoint3DSW::initParams() { m_targetAngMotorVelocity = real_t(0.); m_maxAngMotorForce = real_t(0.); m_accumulatedAngMotorImpulse = real_t(0.0); - } // SliderJointSW::initParams() //----------------------------------------------------------------------------- diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index 37394a1580..f52f6ace27 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -243,7 +243,7 @@ public: bool setup(real_t p_step); void solve(real_t p_step); - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_SLIDER; } + virtual PhysicsServer3D::JointType get_type() const { 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 6a010ee771..225a71aca9 100644 --- a/servers/physics_3d/joints_3d_sw.h +++ b/servers/physics_3d/joints_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -36,10 +36,28 @@ class Joint3DSW : public Constraint3DSW { public: - virtual PhysicsServer3D::JointType get_type() const = 0; + virtual bool setup(real_t p_step) { return false; } + virtual void solve(real_t p_step) {} + + void copy_settings_from(Joint3DSW *p_joint) { + set_self(p_joint->get_self()); + set_priority(p_joint->get_priority()); + disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); + } + + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; } _FORCE_INLINE_ Joint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : Constraint3DSW(p_body_ptr, p_body_count) { } + + virtual ~Joint3DSW() { + for (int i = 0; i < get_body_count(); i++) { + Body3DSW *body = get_body_ptr()[i]; + if (body) { + body->remove_constraint(this); + } + } + } }; #endif // JOINTS_SW_H diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index 143cc9ebbd..3d0063b0fa 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -43,47 +43,63 @@ #define FLUSH_QUERY_CHECK(m_object) \ ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); -RID PhysicsServer3DSW::shape_create(ShapeType p_shape) { - Shape3DSW *shape = nullptr; - switch (p_shape) { - case SHAPE_PLANE: { - shape = memnew(PlaneShape3DSW); - } break; - case SHAPE_RAY: { - shape = memnew(RayShape3DSW); - } break; - case SHAPE_SPHERE: { - shape = memnew(SphereShape3DSW); - } break; - case SHAPE_BOX: { - shape = memnew(BoxShape3DSW); - } break; - case SHAPE_CAPSULE: { - shape = memnew(CapsuleShape3DSW); - } break; - case SHAPE_CYLINDER: { - ERR_FAIL_V_MSG(RID(), "CylinderShape3D is not supported in GodotPhysics3D. Please switch to Bullet in the Project Settings."); - } break; - case SHAPE_CONVEX_POLYGON: { - shape = memnew(ConvexPolygonShape3DSW); - } break; - case SHAPE_CONCAVE_POLYGON: { - shape = memnew(ConcavePolygonShape3DSW); - } break; - case SHAPE_HEIGHTMAP: { - shape = memnew(HeightMapShape3DSW); - } break; - case SHAPE_CUSTOM: { - ERR_FAIL_V(RID()); - - } break; - } - - RID id = shape_owner.make_rid(shape); - shape->set_self(id); - - return id; -}; +RID PhysicsServer3DSW::plane_shape_create() { + Shape3DSW *shape = memnew(PlaneShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::ray_shape_create() { + Shape3DSW *shape = memnew(RayShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::sphere_shape_create() { + Shape3DSW *shape = memnew(SphereShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::box_shape_create() { + Shape3DSW *shape = memnew(BoxShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::capsule_shape_create() { + Shape3DSW *shape = memnew(CapsuleShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::cylinder_shape_create() { + Shape3DSW *shape = memnew(CylinderShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::convex_polygon_shape_create() { + Shape3DSW *shape = memnew(ConvexPolygonShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::concave_polygon_shape_create() { + Shape3DSW *shape = memnew(ConcavePolygonShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::heightmap_shape_create() { + Shape3DSW *shape = memnew(HeightMapShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::custom_shape_create() { + ERR_FAIL_V(RID()); +} void PhysicsServer3DSW::shape_set_data(RID p_shape, const Variant &p_data) { Shape3DSW *shape = shape_owner.getornull(p_shape); @@ -174,7 +190,7 @@ real_t PhysicsServer3DSW::space_get_param(RID p_space, SpaceParameter p_param) c PhysicsDirectSpaceState3D *PhysicsServer3DSW::space_get_direct_state(RID p_space) { Space3DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, nullptr); - ERR_FAIL_COND_V_MSG(!doing_sync || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); return space->get_direct_state(); } @@ -413,13 +429,6 @@ void PhysicsServer3DSW::area_set_ray_pickable(RID p_area, bool p_enable) { area->set_ray_pickable(p_enable); } -bool PhysicsServer3DSW::area_is_ray_pickable(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, false); - - return area->is_ray_pickable(); -} - void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -429,14 +438,8 @@ void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_rec /* BODY API */ -RID PhysicsServer3DSW::body_create(BodyMode p_mode, bool p_init_sleeping) { +RID PhysicsServer3DSW::body_create() { Body3DSW *body = memnew(Body3DSW); - if (p_mode != BODY_MODE_RIGID) { - body->set_mode(p_mode); - } - if (p_init_sleeping) { - body->set_state(BODY_STATE_SLEEPING, p_init_sleeping); - } RID rid = body_owner.make_rid(body); body->set_self(rid); return rid; @@ -608,9 +611,18 @@ uint32_t PhysicsServer3DSW::body_get_collision_mask(RID p_body) const { void PhysicsServer3DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) { Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); + if (body) { + body->set_instance_id(p_id); + return; + } + + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + if (soft_body) { + soft_body->set_instance_id(p_id); + return; + } - body->set_instance_id(p_id); + ERR_FAIL_MSG("Invalid ID."); }; ObjectID PhysicsServer3DSW::body_get_object_instance_id(RID p_body) const { @@ -857,12 +869,6 @@ void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { body->set_ray_pickable(p_enable); } -bool PhysicsServer3DSW::body_is_ray_pickable(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, false); - return body->is_ray_pickable(); -} - 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) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); @@ -874,7 +880,7 @@ bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform &p_from, co 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); } -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, float p_margin) { +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) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -886,40 +892,324 @@ int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform &p_t } PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, nullptr); - ERR_FAIL_COND_V_MSG(!doing_sync || body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); direct_state->body = body; return direct_state; } +/* SOFT BODY */ + +RID PhysicsServer3DSW::soft_body_create() { + SoftBody3DSW *soft_body = memnew(SoftBody3DSW); + RID rid = soft_body_owner.make_rid(soft_body); + soft_body->set_self(rid); + return rid; +} + +void PhysicsServer3DSW::soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->update_rendering_server(p_rendering_server_handler); +} + +void PhysicsServer3DSW::soft_body_set_space(RID p_body, RID p_space) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + Space3DSW *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.getornull(p_space); + ERR_FAIL_COND(!space); + } + + if (soft_body->get_space() == space) { + return; + } + + soft_body->set_space(space); +} + +RID PhysicsServer3DSW::soft_body_get_space(RID p_body) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, RID()); + + Space3DSW *space = soft_body->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +} + +void PhysicsServer3DSW::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_collision_layer(p_layer); +} + +uint32_t PhysicsServer3DSW::soft_body_get_collision_layer(RID p_body) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, 0); + + return soft_body->get_collision_layer(); +} + +void PhysicsServer3DSW::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_collision_mask(p_mask); +} + +uint32_t PhysicsServer3DSW::soft_body_get_collision_mask(RID p_body) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, 0); + + return soft_body->get_collision_mask(); +} + +void PhysicsServer3DSW::soft_body_add_collision_exception(RID p_body, RID p_body_b) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->add_exception(p_body_b); +} + +void PhysicsServer3DSW::soft_body_remove_collision_exception(RID p_body, RID p_body_b) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->remove_exception(p_body_b); +} + +void PhysicsServer3DSW::soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + for (int i = 0; i < soft_body->get_exceptions().size(); i++) { + p_exceptions->push_back(soft_body->get_exceptions()[i]); + } +} + +void PhysicsServer3DSW::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_state(p_state, p_variant); +} + +Variant PhysicsServer3DSW::soft_body_get_state(RID p_body, BodyState p_state) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, Variant()); + + return soft_body->get_state(p_state); +} + +void PhysicsServer3DSW::soft_body_set_transform(RID p_body, const Transform &p_transform) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_state(BODY_STATE_TRANSFORM, p_transform); +} + +void PhysicsServer3DSW::soft_body_set_ray_pickable(RID p_body, bool p_enable) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_ray_pickable(p_enable); +} + +void PhysicsServer3DSW::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_iteration_count(p_simulation_precision); +} + +int PhysicsServer3DSW::soft_body_get_simulation_precision(RID p_body) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_iteration_count(); +} + +void PhysicsServer3DSW::soft_body_set_total_mass(RID p_body, real_t p_total_mass) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_total_mass(p_total_mass); +} + +real_t PhysicsServer3DSW::soft_body_get_total_mass(RID p_body) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_total_mass(); +} + +void PhysicsServer3DSW::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_linear_stiffness(p_stiffness); +} + +real_t PhysicsServer3DSW::soft_body_get_linear_stiffness(RID p_body) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_linear_stiffness(); +} + +void PhysicsServer3DSW::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_pressure_coefficient(p_pressure_coefficient); +} + +real_t PhysicsServer3DSW::soft_body_get_pressure_coefficient(RID p_body) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_pressure_coefficient(); +} + +void PhysicsServer3DSW::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_damping_coefficient(p_damping_coefficient); +} + +real_t PhysicsServer3DSW::soft_body_get_damping_coefficient(RID p_body) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_damping_coefficient(); +} + +void PhysicsServer3DSW::soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_drag_coefficient(p_drag_coefficient); +} + +real_t PhysicsServer3DSW::soft_body_get_drag_coefficient(RID p_body) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_drag_coefficient(); +} + +void PhysicsServer3DSW::soft_body_set_mesh(RID p_body, const REF &p_mesh) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_mesh(p_mesh); +} + +AABB PhysicsServer3DSW::soft_body_get_bounds(RID p_body) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, AABB()); + + return soft_body->get_bounds(); +} + +void PhysicsServer3DSW::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_vertex_position(p_point_index, p_global_position); +} + +Vector3 PhysicsServer3DSW::soft_body_get_point_global_position(RID p_body, int p_point_index) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, Vector3()); + + return soft_body->get_vertex_position(p_point_index); +} + +void PhysicsServer3DSW::soft_body_remove_all_pinned_points(RID p_body) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->unpin_all_vertices(); +} + +void PhysicsServer3DSW::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND(!soft_body); + + if (p_pin) { + soft_body->pin_vertex(p_point_index); + } else { + soft_body->unpin_vertex(p_point_index); + } +} + +bool PhysicsServer3DSW::soft_body_is_point_pinned(RID p_body, int p_point_index) const { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + ERR_FAIL_COND_V(!soft_body, false); + + return soft_body->is_vertex_pinned(p_point_index); +} + /* JOINT API */ -RID PhysicsServer3DSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { +RID PhysicsServer3DSW::joint_create() { + Joint3DSW *joint = memnew(Joint3DSW); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServer3DSW::joint_clear(RID p_joint) { + Joint3DSW *joint = joint_owner.getornull(p_joint); + if (joint->get_type() != JOINT_TYPE_MAX) { + Joint3DSW *empty_joint = memnew(Joint3DSW); + empty_joint->copy_settings_from(joint); + + joint_owner.replace(p_joint, empty_joint); + memdelete(joint); + } +} + +void PhysicsServer3DSW::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + ERR_FAIL_COND(body_A == body_B); + + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(PinJoint3DSW(body_A, p_local_A, body_B, p_local_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_param(p_param, p_value); } @@ -927,7 +1217,7 @@ void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); return pin_joint->get_param(p_param); } @@ -935,7 +1225,7 @@ real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_pos_a(p_A); } @@ -943,7 +1233,7 @@ void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); return pin_joint->get_position_a(); } @@ -951,7 +1241,7 @@ Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_pos_b(p_B); } @@ -959,55 +1249,63 @@ void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); return pin_joint->get_position_b(); } -RID PhysicsServer3DSW::joint_create_hinge(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 Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + ERR_FAIL_COND(body_A == body_B); + + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_frame_A, p_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } -RID PhysicsServer3DSW::joint_create_hinge_simple(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) { +void PhysicsServer3DSW::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) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + ERR_FAIL_COND(body_A == body_B); + + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); hinge_joint->set_param(p_param, p_value); } @@ -1015,7 +1313,7 @@ void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_par real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); return hinge_joint->get_param(p_param); } @@ -1023,7 +1321,7 @@ real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_p void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); hinge_joint->set_flag(p_flag, p_value); } @@ -1031,7 +1329,7 @@ void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); return hinge_joint->get_flag(p_flag); } @@ -1077,34 +1375,38 @@ bool PhysicsServer3DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, JOINT_PIN); + ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); return joint->get_type(); } -RID PhysicsServer3DSW::joint_create_slider(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 Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(SliderJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER); SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); slider_joint->set_param(p_param, p_value); } @@ -1112,35 +1414,39 @@ void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_p real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); return slider_joint->get_param(p_param); } -RID PhysicsServer3DSW::joint_create_cone_twist(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 Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(ConeTwistJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST); ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); cone_twist_joint->set_param(p_param, p_value); } @@ -1148,43 +1454,47 @@ void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointPa real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); return cone_twist_joint->get_param(p_param); } -RID PhysicsServer3DSW::joint_create_generic_6dof(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 Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(Generic6DOFJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); generic_6dof_joint->set_param(p_axis, p_param, p_value); } -real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) { +real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); return generic_6dof_joint->get_param(p_axis, p_param); } @@ -1192,15 +1502,15 @@ real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axi void PhysicsServer3DSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); } -bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) { +bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); return generic_6dof_joint->get_flag(p_axis, p_flag); } @@ -1237,7 +1547,13 @@ void PhysicsServer3DSW::free(RID p_rid) { body_owner.free(p_rid); memdelete(body); + } else if (soft_body_owner.owns(p_rid)) { + SoftBody3DSW *soft_body = soft_body_owner.getornull(p_rid); + + soft_body->set_space(nullptr); + soft_body_owner.free(p_rid); + memdelete(soft_body); } else if (area_owner.owns(p_rid)) { Area3DSW *area = area_owner.getornull(p_rid); @@ -1271,9 +1587,6 @@ void PhysicsServer3DSW::free(RID p_rid) { } else if (joint_owner.owns(p_rid)) { Joint3DSW *joint = joint_owner.getornull(p_rid); - for (int i = 0; i < joint->get_body_count(); i++) { - joint->get_body_ptr()[i]->remove_constraint(joint); - } joint_owner.free(p_rid); memdelete(joint); @@ -1287,7 +1600,6 @@ void PhysicsServer3DSW::set_active(bool p_active) { }; void PhysicsServer3DSW::init() { - doing_sync = true; last_step = 0.001; iterations = 8; // 8? stepper = memnew(Step3DSW); @@ -1303,8 +1615,6 @@ void PhysicsServer3DSW::step(real_t p_step) { _update_shapes(); - doing_sync = false; - last_step = p_step; PhysicsDirectBodyState3DSW::singleton->step = p_step; @@ -1320,6 +1630,10 @@ void PhysicsServer3DSW::step(real_t p_step) { #endif } +void PhysicsServer3DSW::sync() { + doing_sync = true; +}; + void PhysicsServer3DSW::flush_queries() { #ifndef _3D_DISABLED @@ -1327,8 +1641,6 @@ void PhysicsServer3DSW::flush_queries() { return; } - doing_sync = true; - flushing_queries = true; uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); @@ -1375,6 +1687,10 @@ void PhysicsServer3DSW::flush_queries() { #endif }; +void PhysicsServer3DSW::end_sync() { + doing_sync = false; +}; + void PhysicsServer3DSW::finish() { memdelete(stepper); memdelete(direct_state); @@ -1403,7 +1719,7 @@ void PhysicsServer3DSW::_update_shapes() { } } -void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { +void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { CollCbkData *cbk = (CollCbkData *)p_userdata; if (cbk->max == 0) { @@ -1436,14 +1752,15 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 & } } -PhysicsServer3DSW *PhysicsServer3DSW::singleton = nullptr; -PhysicsServer3DSW::PhysicsServer3DSW() { - singleton = this; +PhysicsServer3DSW *PhysicsServer3DSW::singletonsw = nullptr; +PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) { + singletonsw = this; BroadPhase3DSW::create_func = BroadPhaseOctree::_create; island_count = 0; active_objects = 0; collision_pairs = 0; - + using_threads = p_using_threads; active = true; flushing_queries = false; + doing_sync = false; }; diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index d9c86312cc..f92652bfad 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -31,7 +31,7 @@ #ifndef PHYSICS_SERVER_SW #define PHYSICS_SERVER_SW -#include "core/rid_owner.h" +#include "core/templates/rid_owner.h" #include "joints_3d_sw.h" #include "servers/physics_server_3d.h" #include "shape_3d_sw.h" @@ -44,13 +44,14 @@ class PhysicsServer3DSW : public PhysicsServer3D { friend class PhysicsDirectSpaceState3DSW; bool active; int iterations; - bool doing_sync; real_t last_step; int island_count; int active_objects; int collision_pairs; + bool using_threads; + bool doing_sync; bool flushing_queries; Step3DSW *stepper; @@ -58,29 +59,40 @@ class PhysicsServer3DSW : public PhysicsServer3D { PhysicsDirectBodyState3DSW *direct_state; - mutable RID_PtrOwner<Shape3DSW> shape_owner; - mutable RID_PtrOwner<Space3DSW> space_owner; - mutable RID_PtrOwner<Area3DSW> area_owner; - mutable RID_PtrOwner<Body3DSW> body_owner; - mutable RID_PtrOwner<Joint3DSW> joint_owner; + mutable RID_PtrOwner<Shape3DSW, true> shape_owner; + mutable RID_PtrOwner<Space3DSW, true> space_owner; + mutable RID_PtrOwner<Area3DSW, true> area_owner; + mutable RID_PtrOwner<Body3DSW, true> body_owner; + mutable RID_PtrOwner<SoftBody3DSW, true> soft_body_owner; + mutable RID_PtrOwner<Joint3DSW, true> joint_owner; //void _clear_query(QuerySW *p_query); friend class CollisionObject3DSW; SelfList<CollisionObject3DSW>::List pending_shape_update_list; void _update_shapes(); -public: - static PhysicsServer3DSW *singleton; + static PhysicsServer3DSW *singletonsw; +public: struct CollCbkData { int max; int amount; Vector3 *ptr; }; - static void _shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); + static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); + + virtual RID plane_shape_create() override; + virtual RID ray_shape_create() override; + virtual RID sphere_shape_create() override; + virtual RID box_shape_create() override; + virtual RID capsule_shape_create() override; + virtual RID cylinder_shape_create() override; + virtual RID convex_polygon_shape_create() override; + virtual RID concave_polygon_shape_create() override; + virtual RID heightmap_shape_create() override; + virtual RID custom_shape_create() override; - virtual RID shape_create(ShapeType p_shape) override; virtual void shape_set_data(RID p_shape, const Variant &p_data) override; virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override; @@ -141,7 +153,6 @@ public: virtual Transform area_get_transform(RID p_area) const override; virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; - virtual bool area_is_ray_pickable(RID p_area) const override; virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; @@ -154,7 +165,7 @@ public: /* BODY API */ // create a body of a given type - virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false) override; + virtual RID body_create() override; virtual void body_set_space(RID p_body, RID p_space) override; virtual RID body_get_space(RID p_body) const override; @@ -233,83 +244,75 @@ public: 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_ray_pickable(RID p_body, bool p_enable) override; - virtual bool body_is_ray_pickable(RID p_body) const 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, float p_margin = 0.001) 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; // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; /* SOFT BODY */ - virtual RID soft_body_create(bool p_init_sleeping = false) override { return RID(); } + virtual RID soft_body_create() override; - virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) override {} + virtual void soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) override; - virtual void soft_body_set_space(RID p_body, RID p_space) override {} - virtual RID soft_body_get_space(RID p_body) const override { return RID(); } + virtual void soft_body_set_space(RID p_body, RID p_space) override; + virtual RID soft_body_get_space(RID p_body) const override; - virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override {} - virtual uint32_t soft_body_get_collision_layer(RID p_body) const override { return 0; } + virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override; + virtual uint32_t soft_body_get_collision_layer(RID p_body) const override; - virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override {} - virtual uint32_t soft_body_get_collision_mask(RID p_body) const override { return 0; } + virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override; + virtual uint32_t soft_body_get_collision_mask(RID p_body) const override; - virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override {} - virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override {} - virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override {} + virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override; + virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override; + virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override; - 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 { return Variant(); } + 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 Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const override { return Vector3(); } + virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) override; - virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override {} - virtual bool soft_body_is_ray_pickable(RID p_body) const override { return false; } + virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override; - virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override {} - virtual int soft_body_get_simulation_precision(RID p_body) override { return 0; } + virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override; + virtual int soft_body_get_simulation_precision(RID p_body) const override; - virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override {} - virtual real_t soft_body_get_total_mass(RID p_body) override { return 0.; } + virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override; + virtual real_t soft_body_get_total_mass(RID p_body) const override; - virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override {} - virtual real_t soft_body_get_linear_stiffness(RID p_body) override { return 0.; } + virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override; + virtual real_t soft_body_get_linear_stiffness(RID p_body) const override; - virtual void soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness) override {} - virtual real_t soft_body_get_areaAngular_stiffness(RID p_body) override { return 0.; } + virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override; + virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override; - virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) override {} - virtual real_t soft_body_get_volume_stiffness(RID p_body) override { return 0.; } + virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override; + virtual real_t soft_body_get_damping_coefficient(RID p_body) const override; - virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override {} - virtual real_t soft_body_get_pressure_coefficient(RID p_body) override { return 0.; } + virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override; + virtual real_t soft_body_get_drag_coefficient(RID p_body) const override; - virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) override {} - virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) override { return 0.; } + virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override; - virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override {} - virtual real_t soft_body_get_damping_coefficient(RID p_body) override { return 0.; } + virtual AABB soft_body_get_bounds(RID p_body) const override; - virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override {} - virtual real_t soft_body_get_drag_coefficient(RID p_body) override { return 0.; } + virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override; + virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override; - virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override {} + virtual void soft_body_remove_all_pinned_points(RID p_body) override; + virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override; + virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override; - virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override {} - virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) override { return Vector3(); } - - virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const override { return Vector3(); } + /* JOINT API */ - virtual void soft_body_remove_all_pinned_points(RID p_body) override {} - virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override {} - virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) override { return false; } + virtual RID joint_create() override; - /* JOINT API */ + virtual void joint_clear(RID p_joint) override; //resets type - virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) override; + virtual void joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) override; virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override; virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override; @@ -320,8 +323,8 @@ 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 RID joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) override; - virtual RID joint_create_hinge_simple(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 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_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; virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const override; @@ -329,26 +332,23 @@ 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 RID joint_create_slider(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 Transform &p_local_frame_A, RID p_body_B, const Transform &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 RID joint_create_cone_twist(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 Transform &p_local_frame_A, RID p_body_B, const Transform &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 RID joint_create_generic_6dof(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 Transform &p_local_frame_A, RID p_body_B, const Transform &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) override; + virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) const override; virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) override; - virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) override; - - virtual void generic_6dof_joint_set_precision(RID p_joint, int precision) override {} - virtual int generic_6dof_joint_get_precision(RID p_joint) override { return 0; } + virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) const override; virtual JointType joint_get_type(RID p_joint) const override; @@ -365,15 +365,16 @@ public: virtual void set_active(bool p_active) override; virtual void init() override; virtual void step(real_t p_step) override; - virtual void sync() override {} + virtual void sync() override; virtual void flush_queries() override; + virtual void end_sync() override; virtual void finish() override; virtual bool is_flushing_queries() const override { return flushing_queries; } int get_process_info(ProcessInfo p_info) override; - PhysicsServer3DSW(); + PhysicsServer3DSW(bool p_using_threads = false); ~PhysicsServer3DSW() {} }; diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.cpp b/servers/physics_3d/physics_server_3d_wrap_mt.cpp new file mode 100644 index 0000000000..f73f67a756 --- /dev/null +++ b/servers/physics_3d/physics_server_3d_wrap_mt.cpp @@ -0,0 +1,140 @@ +/*************************************************************************/ +/* physics_server_3d_wrap_mt.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 "physics_server_3d_wrap_mt.h" + +#include "core/os/os.h" + +void PhysicsServer3DWrapMT::thread_exit() { + exit = true; +} + +void PhysicsServer3DWrapMT::thread_step(real_t p_delta) { + physics_3d_server->step(p_delta); + step_sem.post(); +} + +void PhysicsServer3DWrapMT::_thread_callback(void *_instance) { + PhysicsServer3DWrapMT *vsmt = reinterpret_cast<PhysicsServer3DWrapMT *>(_instance); + + vsmt->thread_loop(); +} + +void PhysicsServer3DWrapMT::thread_loop() { + server_thread = Thread::get_caller_id(); + + physics_3d_server->init(); + + exit = false; + step_thread_up = true; + while (!exit) { + // flush commands one by one, until exit is requested + command_queue.wait_and_flush_one(); + } + + command_queue.flush_all(); // flush all + + physics_3d_server->finish(); +} + +/* EVENT QUEUING */ + +void PhysicsServer3DWrapMT::step(real_t p_step) { + if (create_thread) { + command_queue.push(this, &PhysicsServer3DWrapMT::thread_step, p_step); + } else { + command_queue.flush_all(); //flush all pending from other threads + physics_3d_server->step(p_step); + } +} + +void PhysicsServer3DWrapMT::sync() { + if (create_thread) { + if (first_frame) { + first_frame = false; + } else { + step_sem.wait(); //must not wait if a step was not issued + } + } + physics_3d_server->sync(); +} + +void PhysicsServer3DWrapMT::flush_queries() { + physics_3d_server->flush_queries(); +} + +void PhysicsServer3DWrapMT::end_sync() { + physics_3d_server->end_sync(); +} + +void PhysicsServer3DWrapMT::init() { + if (create_thread) { + //OS::get_singleton()->release_rendering_thread(); + thread.start(_thread_callback, this); + while (!step_thread_up) { + OS::get_singleton()->delay_usec(1000); + } + } else { + physics_3d_server->init(); + } +} + +void PhysicsServer3DWrapMT::finish() { + if (thread.is_started()) { + command_queue.push(this, &PhysicsServer3DWrapMT::thread_exit); + thread.wait_to_finish(); + } else { + physics_3d_server->finish(); + } +} + +PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread) : + command_queue(p_create_thread) { + physics_3d_server = p_contained; + create_thread = p_create_thread; + step_pending = 0; + step_thread_up = false; + + pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc"); + + if (!p_create_thread) { + server_thread = Thread::get_caller_id(); + } else { + server_thread = 0; + } + + main_thread = Thread::get_caller_id(); + first_frame = true; +} + +PhysicsServer3DWrapMT::~PhysicsServer3DWrapMT() { + memdelete(physics_3d_server); + //finish(); +} diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h new file mode 100644 index 0000000000..49ae60db92 --- /dev/null +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -0,0 +1,413 @@ +/*************************************************************************/ +/* physics_server_3d_wrap_mt.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 PHYSICS3DSERVERWRAPMT_H +#define PHYSICS3DSERVERWRAPMT_H + +#include "core/config/project_settings.h" +#include "core/os/thread.h" +#include "core/templates/command_queue_mt.h" +#include "servers/physics_server_3d.h" + +#ifdef DEBUG_SYNC +#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__)); +#else +#define SYNC_DEBUG +#endif + +class PhysicsServer3DWrapMT : public PhysicsServer3D { + mutable PhysicsServer3D *physics_3d_server; + + mutable CommandQueueMT command_queue; + + static void _thread_callback(void *_instance); + void thread_loop(); + + Thread::ID server_thread; + Thread::ID main_thread; + volatile bool exit = false; + Thread thread; + volatile bool step_thread_up = false; + bool create_thread = false; + + Semaphore step_sem; + int step_pending; + void thread_step(real_t p_delta); + void thread_flush(); + + void thread_exit(); + + bool first_frame = true; + + Mutex alloc_mutex; + int pool_max_size = 0; + +public: +#define ServerName PhysicsServer3D +#define ServerNameWrapMT PhysicsServer3DWrapMT +#define server_name physics_3d_server +#define WRITE_ACTION + +#include "servers/server_wrap_mt_common.h" + + //FUNC1RID(shape,ShapeType); todo fix + FUNCRID(plane_shape) + FUNCRID(ray_shape) + FUNCRID(sphere_shape) + FUNCRID(box_shape) + FUNCRID(capsule_shape) + FUNCRID(cylinder_shape) + FUNCRID(convex_polygon_shape) + FUNCRID(concave_polygon_shape) + FUNCRID(heightmap_shape) + FUNCRID(custom_shape) + + FUNC2(shape_set_data, RID, const Variant &); + FUNC2(shape_set_custom_solver_bias, RID, real_t); + + FUNC2(shape_set_margin, RID, real_t) + FUNC1RC(real_t, shape_get_margin, RID) + + FUNC1RC(ShapeType, shape_get_type, RID); + FUNC1RC(Variant, shape_get_data, RID); + FUNC1RC(real_t, shape_get_custom_solver_bias, RID); +#if 0 + //these work well, but should be used from the main thread only + bool shape_collide(RID p_shape_A, const Transform &p_xform_A, const Vector3 &p_motion_A, RID p_shape_B, const Transform &p_xform_B, const Vector3 &p_motion_B, Vector3 *r_results, int p_result_max, int &r_result_count) { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_3d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count); + } +#endif + /* SPACE API */ + + FUNCRID(space); + FUNC2(space_set_active, RID, bool); + FUNC1RC(bool, space_is_active, RID); + + FUNC3(space_set_param, RID, SpaceParameter, real_t); + FUNC2RC(real_t, space_get_param, RID, SpaceParameter); + + // this function only works on physics process, errors and returns null otherwise + PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); + return physics_3d_server->space_get_direct_state(p_space); + } + + FUNC2(space_set_debug_contacts, RID, int); + virtual Vector<Vector3> space_get_contacts(RID p_space) const override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector<Vector3>()); + return physics_3d_server->space_get_contacts(p_space); + } + + virtual int space_get_contact_count(RID p_space) const override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0); + return physics_3d_server->space_get_contact_count(p_space); + } + + /* AREA API */ + + //FUNC0RID(area); + FUNCRID(area); + + FUNC2(area_set_space, RID, RID); + FUNC1RC(RID, area_get_space, RID); + + 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); + FUNC3(area_set_shape, RID, int, RID); + FUNC3(area_set_shape_transform, RID, int, const Transform &); + 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); + FUNC2(area_remove_shape, RID, int); + FUNC1(area_clear_shapes, RID); + + FUNC2(area_attach_object_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, area_get_object_instance_id, RID); + + FUNC3(area_set_param, RID, AreaParameter, const Variant &); + FUNC2(area_set_transform, RID, const Transform &); + + FUNC2RC(Variant, area_get_param, RID, AreaParameter); + FUNC1RC(Transform, area_get_transform, RID); + + FUNC2(area_set_collision_mask, RID, uint32_t); + FUNC2(area_set_collision_layer, RID, uint32_t); + + FUNC2(area_set_monitorable, RID, bool); + FUNC2(area_set_ray_pickable, RID, bool); + + FUNC3(area_set_monitor_callback, RID, Object *, const StringName &); + FUNC3(area_set_area_monitor_callback, RID, Object *, const StringName &); + + /* BODY API */ + + //FUNC2RID(body,BodyMode,bool); + FUNCRID(body) + + FUNC2(body_set_space, RID, RID); + FUNC1RC(RID, body_get_space, RID); + + FUNC2(body_set_mode, RID, BodyMode); + FUNC1RC(BodyMode, body_get_mode, RID); + + FUNC4(body_add_shape, RID, RID, const Transform &, bool); + FUNC3(body_set_shape, RID, int, RID); + FUNC3(body_set_shape_transform, RID, int, const Transform &); + + FUNC1RC(int, body_get_shape_count, RID); + FUNC2RC(Transform, body_get_shape_transform, RID, int); + FUNC2RC(RID, body_get_shape, RID, int); + + FUNC3(body_set_shape_disabled, RID, int, bool); + + FUNC2(body_remove_shape, RID, int); + FUNC1(body_clear_shapes, RID); + + FUNC2(body_attach_object_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, body_get_object_instance_id, RID); + + FUNC2(body_set_enable_continuous_collision_detection, RID, bool); + FUNC1RC(bool, body_is_continuous_collision_detection_enabled, RID); + + FUNC2(body_set_collision_layer, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_layer, RID); + + FUNC2(body_set_collision_mask, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_mask, RID); + + FUNC2(body_set_user_flags, RID, uint32_t); + FUNC1RC(uint32_t, body_get_user_flags, RID); + + 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); + + FUNC2(body_set_applied_force, RID, const Vector3 &); + FUNC1RC(Vector3, body_get_applied_force, RID); + + FUNC2(body_set_applied_torque, RID, const Vector3 &); + FUNC1RC(Vector3, body_get_applied_torque, RID); + + FUNC2(body_add_central_force, RID, const Vector3 &); + FUNC3(body_add_force, RID, const Vector3 &, const Vector3 &); + FUNC2(body_add_torque, RID, const Vector3 &); + FUNC2(body_apply_torque_impulse, RID, const Vector3 &); + FUNC2(body_apply_central_impulse, RID, const Vector3 &); + FUNC3(body_apply_impulse, RID, const Vector3 &, const Vector3 &); + FUNC2(body_set_axis_velocity, RID, const Vector3 &); + + FUNC3(body_set_axis_lock, RID, BodyAxis, bool); + FUNC2RC(bool, body_is_axis_locked, RID, BodyAxis); + + FUNC2(body_add_collision_exception, RID, RID); + FUNC2(body_remove_collision_exception, RID, RID); + FUNC2S(body_get_collision_exceptions, RID, List<RID> *); + + FUNC2(body_set_max_contacts_reported, RID, int); + FUNC1RC(int, body_get_max_contacts_reported, RID); + + FUNC2(body_set_contacts_reported_depth_threshold, RID, real_t); + FUNC1RC(real_t, body_get_contacts_reported_depth_threshold, RID); + + 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 &); + + 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 { + 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); + } + + 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 { + 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); + } + + // this function only works on physics process, errors and returns null otherwise + PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); + return physics_3d_server->body_get_direct_state(p_body); + } + + /* SOFT BODY API */ + + FUNCRID(soft_body) + + FUNC2(soft_body_update_rendering_server, RID, class RenderingServerHandler *) + + FUNC2(soft_body_set_space, RID, RID) + FUNC1RC(RID, soft_body_get_space, RID) + + FUNC2(soft_body_set_ray_pickable, RID, bool); + + FUNC2(soft_body_set_collision_layer, RID, uint32_t) + FUNC1RC(uint32_t, soft_body_get_collision_layer, RID) + + FUNC2(soft_body_set_collision_mask, RID, uint32_t) + FUNC1RC(uint32_t, soft_body_get_collision_mask, RID) + + FUNC2(soft_body_add_collision_exception, RID, RID) + FUNC2(soft_body_remove_collision_exception, RID, RID) + FUNC2S(soft_body_get_collision_exceptions, RID, List<RID> *) + + 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_simulation_precision, RID, int); + FUNC1RC(int, soft_body_get_simulation_precision, RID); + + FUNC2(soft_body_set_total_mass, RID, real_t); + FUNC1RC(real_t, soft_body_get_total_mass, RID); + + FUNC2(soft_body_set_linear_stiffness, RID, real_t); + FUNC1RC(real_t, soft_body_get_linear_stiffness, RID); + + FUNC2(soft_body_set_pressure_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_pressure_coefficient, RID); + + FUNC2(soft_body_set_damping_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_damping_coefficient, RID); + + FUNC2(soft_body_set_drag_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_drag_coefficient, RID); + + FUNC2(soft_body_set_mesh, RID, const REF &); + + FUNC1RC(AABB, soft_body_get_bounds, RID); + + FUNC3(soft_body_move_point, RID, int, const Vector3 &); + FUNC2RC(Vector3, soft_body_get_point_global_position, RID, int); + + FUNC1(soft_body_remove_all_pinned_points, RID); + FUNC3(soft_body_pin_point, RID, int, bool); + FUNC2RC(bool, soft_body_is_point_pinned, RID, int); + + /* JOINT API */ + + FUNCRID(joint) + + FUNC1(joint_clear, RID) + + FUNC5(joint_make_pin, RID, RID, const Vector3 &, RID, const Vector3 &) + + FUNC3(pin_joint_set_param, RID, PinJointParam, real_t) + FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam) + + FUNC2(pin_joint_set_local_a, RID, const Vector3 &) + FUNC1RC(Vector3, pin_joint_get_local_a, RID) + + 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 &) + 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) + FUNC2RC(real_t, hinge_joint_get_param, RID, HingeJointParam) + + 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 &) + + 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 &) + + 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 &) + + FUNC4(generic_6dof_joint_set_param, RID, Vector3::Axis, G6DOFJointAxisParam, real_t) + FUNC3RC(real_t, generic_6dof_joint_get_param, RID, Vector3::Axis, G6DOFJointAxisParam) + + FUNC4(generic_6dof_joint_set_flag, RID, Vector3::Axis, G6DOFJointAxisFlag, bool) + FUNC3RC(bool, generic_6dof_joint_get_flag, RID, Vector3::Axis, G6DOFJointAxisFlag) + + FUNC1RC(JointType, joint_get_type, RID); + + FUNC2(joint_set_solver_priority, RID, int); + FUNC1RC(int, joint_get_solver_priority, RID); + + FUNC2(joint_disable_collisions_between_bodies, RID, const bool); + FUNC1RC(bool, joint_is_disabled_collisions_between_bodies, RID); + + /* MISC */ + + FUNC1(free, RID); + FUNC1(set_active, bool); + + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void end_sync() override; + virtual void flush_queries() override; + virtual void finish() override; + + virtual bool is_flushing_queries() const override { + return physics_3d_server->is_flushing_queries(); + } + + int get_process_info(ProcessInfo p_info) override { + return physics_3d_server->get_process_info(p_info); + } + + PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread); + ~PhysicsServer3DWrapMT(); + +#undef ServerNameWrapMT +#undef ServerName +#undef server_name +#undef WRITE_ACTION +}; + +#ifdef DEBUG_SYNC +#undef DEBUG_SYNC +#endif +#undef SYNC_DEBUG + +#endif // PHYSICS3DSERVERWRAPMT_H diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index ca33241d29..bf0946a0e2 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -32,12 +32,14 @@ #include "core/math/geometry_3d.h" #include "core/math/quick_hull.h" -#include "core/sort_array.h" +#include "core/templates/sort_array.h" -#define _POINT_SNAP 0.001953125 #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 +#define _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD 0.002 +#define _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD 0.999 + void Shape3DSW::configure(const AABB &p_aabb) { aabb = p_aabb; configured = true; @@ -50,7 +52,8 @@ void Shape3DSW::configure(const AABB &p_aabb) { Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const { Vector3 res; int amnt; - get_supports(p_normal, 1, &res, amnt); + FeatureType type; + get_supports(p_normal, 1, &res, amnt, type); return res; } @@ -167,16 +170,19 @@ Vector3 RayShape3DSW::get_support(const Vector3 &p_normal) const { } } -void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = Vector3(0, 0, 0); r_supports[1] = Vector3(0, 0, length); } else if (p_normal.z > 0) { r_amount = 1; + r_type = FEATURE_POINT; *r_supports = Vector3(0, 0, length); } else { r_amount = 1; + r_type = FEATURE_POINT; *r_supports = Vector3(0, 0, 0); } } @@ -246,9 +252,10 @@ Vector3 SphereShape3DSW::get_support(const Vector3 &p_normal) const { return p_normal * radius; } -void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { *r_supports = p_normal * radius; r_amount = 1; + r_type = FEATURE_POINT; } bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { @@ -261,7 +268,7 @@ bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const { Vector3 SphereShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 p = p_point; - float l = p.length(); + real_t l = p.length(); if (l < radius) { return p_point; } @@ -312,7 +319,7 @@ Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const { return point; } -void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { static const int next[3] = { 1, 2, 0 }; static const int next2[3] = { 2, 0, 1 }; @@ -325,6 +332,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s bool neg = dot < 0; r_amount = 4; + r_type = FEATURE_FACE; Vector3 point; point[i] = half_extents[i]; @@ -333,7 +341,6 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s int i_n2 = next2[i]; static const real_t sign[4][2] = { - { -1.0, 1.0 }, { 1.0, 1.0 }, { 1.0, -1.0 }, @@ -363,6 +370,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; + r_type = FEATURE_EDGE; int i_n = next[i]; int i_n2 = next2[i]; @@ -390,6 +398,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s (p_normal.z < 0) ? -half_extents.z : half_extents.z); r_amount = 1; + r_type = FEATURE_POINT; r_supports[0] = point; } @@ -430,7 +439,7 @@ Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const { } //check segments - float min_distance = 1e20; + real_t min_distance = 1e20; Vector3 closest_vertex = half_extents * p_point.sign(); Vector3 s[2] = { closest_vertex, @@ -443,7 +452,7 @@ Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 closest_edge = Geometry3D::get_closest_point_to_segment(p_point, s); - float d = p_point.distance_to(closest_edge); + real_t d = p_point.distance_to(closest_edge); if (d < min_distance) { min_point = closest_edge; min_distance = d; @@ -482,10 +491,10 @@ BoxShape3DSW::BoxShape3DSW() { void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform &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.z > 0) ? height : -height; + real_t h = (n.y > 0) ? height : -height; n *= radius; - n.z += h * 0.5; + n.y += h * 0.5; r_max = p_normal.dot(p_transform.xform(n)); r_min = p_normal.dot(p_transform.xform(-n)); @@ -494,36 +503,38 @@ void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform &p Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const { Vector3 n = p_normal; - real_t h = (n.z > 0) ? height : -height; + real_t h = (n.y > 0) ? height : -height; n *= radius; - n.z += h * 0.5; + n.y += h * 0.5; return n; } -void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { Vector3 n = p_normal; - real_t d = n.z; + real_t d = n.y; if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { // make it flat - n.z = 0.0; + n.y = 0.0; n.normalize(); n *= radius; r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = n; - r_supports[0].z += height * 0.5; + r_supports[0].y += height * 0.5; r_supports[1] = n; - r_supports[1].z -= height * 0.5; + r_supports[1].y -= height * 0.5; } else { real_t h = (d > 0) ? height : -height; n *= radius; - n.z += h * 0.5; + n.y += h * 0.5; r_amount = 1; + r_type = FEATURE_POINT; *r_supports = n; } } @@ -540,7 +551,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & // test against cylinder and spheres :-| - collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn); + collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn, 1); if (collided) { real_t d = norm.dot(auxres); @@ -552,7 +563,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } } - collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), radius, &auxres, &auxn); + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5, 0), radius, &auxres, &auxn); if (collided) { real_t d = norm.dot(auxres); @@ -564,7 +575,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } } - collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), radius, &auxres, &auxn); + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5, 0), radius, &auxres, &auxn); if (collided) { real_t d = norm.dot(auxres); @@ -585,19 +596,19 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } bool CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const { - if (Math::abs(p_point.z) < height * 0.5) { - return Vector3(p_point.x, p_point.y, 0).length() < radius; + if (Math::abs(p_point.y) < height * 0.5) { + return Vector3(p_point.x, 0, p_point.z).length() < radius; } else { Vector3 p = p_point; - p.z = Math::abs(p.z) - height * 0.5; + p.y = Math::abs(p.y) - height * 0.5; return p.length() < radius; } } Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 s[2] = { - Vector3(0, 0, -height * 0.5), - Vector3(0, 0, height * 0.5), + Vector3(0, -height * 0.5, 0), + Vector3(0, height * 0.5, 0), }; Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); @@ -616,13 +627,13 @@ 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) { height = p_height; radius = p_radius; - configure(AABB(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0))); + configure(AABB(Vector3(-radius, -height * 0.5 - radius, -radius), Vector3(radius * 2, height + radius * 2.0, radius * 2))); } void CapsuleShape3DSW::set_data(const Variant &p_data) { @@ -643,6 +654,186 @@ CapsuleShape3DSW::CapsuleShape3DSW() { height = radius = 0; } +/********** CYLINDER *************/ + +void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform &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); + + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + real_t scale = local_normal.length(); + real_t scaled_radius = radius * scale; + real_t scaled_height = height * scale; + + real_t length; + if (Math::abs(axis_dot) > 1.0) { + length = scaled_height * 0.5; + } else { + length = Math::abs(axis_dot * scaled_height * 0.5) + scaled_radius * Math::sqrt(1.0 - axis_dot * axis_dot); + } + + real_t distance = p_normal.dot(p_transform.origin); + + r_min = distance - length; + r_max = distance + length; +} + +Vector3 CylinderShape3DSW::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + real_t h = (n.y > 0) ? height : -height; + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = radius; + n.y = h * 0.5; + n.z = 0.0; + } else { + real_t d = radius / s; + n.x = n.x * d; + n.y = h * 0.5; + n.z = n.z * d; + } + + return n; +} + +void CylinderShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + real_t d = p_normal.y; + if (Math::abs(d) > _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD) { + real_t h = (d > 0) ? height : -height; + + Vector3 n = p_normal; + n.x = 0.0; + n.z = 0.0; + n.y = h * 0.5; + + r_amount = 3; + r_type = FEATURE_CIRCLE; + r_supports[0] = n; + r_supports[1] = n; + r_supports[1].x += radius; + r_supports[2] = n; + r_supports[2].z += radius; + } else if (Math::abs(d) < _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD) { + // make it flat + Vector3 n = p_normal; + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = n; + r_supports[0].y += height * 0.5; + r_supports[1] = n; + r_supports[1].y -= height * 0.5; + } else { + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = get_support(p_normal); + return; + + Vector3 n = p_normal; + real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius); + if (Math::abs(h) > 1.0) { + // Top or bottom surface. + n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5; + } else { + // Lateral surface. + n.y = height * 0.5 * h; + } + + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = 0.0; + n.z = 0.0; + } else { + real_t scaled_radius = radius / s; + n.x = n.x * scaled_radius; + n.z = n.z * scaled_radius; + } + + r_supports[0] = n; + } +} + +bool CylinderShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1); +} + +bool CylinderShape3DSW::intersect_point(const Vector3 &p_point) const { + if (Math::abs(p_point.y) < height * 0.5) { + return Vector3(p_point.x, 0, p_point.z).length() < radius; + } + return false; +} + +Vector3 CylinderShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + if (Math::absf(p_point.y) > height * 0.5) { + // Project point to top disk. + real_t dir = p_point.y > 0.0 ? 1.0 : -1.0; + Vector3 circle_pos(0.0, dir * height * 0.5, 0.0); + Plane circle_plane(circle_pos, Vector3(0.0, dir, 0.0)); + Vector3 proj_point = circle_plane.project(p_point); + + // Clip position. + Vector3 delta_point_1 = proj_point - circle_pos; + real_t dist_point_1 = delta_point_1.length_squared(); + if (!Math::is_zero_approx(dist_point_1)) { + dist_point_1 = Math::sqrt(dist_point_1); + proj_point = circle_pos + delta_point_1 * MIN(dist_point_1, radius) / dist_point_1; + } + + return proj_point; + } else { + Vector3 s[2] = { + Vector3(0, -height * 0.5, 0), + Vector3(0, height * 0.5, 0), + }; + + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) { + return p_point; + } + + return p + (p_point - p).normalized() * radius; + } +} + +Vector3 CylinderShape3DSW::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + 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.x * extents.x + extents.y * extents.y)); +} + +void CylinderShape3DSW::_setup(real_t p_height, real_t p_radius) { + height = p_height; + radius = p_radius; + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0))); +} + +void CylinderShape3DSW::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("radius")); + ERR_FAIL_COND(!d.has("height")); + _setup(d["height"], d["radius"]); +} + +Variant CylinderShape3DSW::get_data() const { + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +CylinderShape3DSW::CylinderShape3DSW() { + height = radius = 0; +} + /********** CONVEX POLYGON *************/ void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { @@ -690,7 +881,7 @@ Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const { return vrts[vert_support_idx]; } -void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); @@ -735,6 +926,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve r_supports[j] = vertices[ind[j]]; } r_amount = m; + r_type = FEATURE_FACE; return; } } @@ -744,6 +936,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = vertices[edges[i].a]; r_supports[1] = vertices[edges[i].b]; return; @@ -752,6 +945,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve r_supports[0] = vertices[vtx]; r_amount = 1; + r_type = FEATURE_POINT; } bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { @@ -840,7 +1034,7 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con return p_point; } - float min_distance = 1e20; + real_t min_distance = 1e20; Vector3 min_point; //check edges @@ -853,7 +1047,7 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con }; Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s); - float d = closest.distance_to(p_point); + real_t d = closest.distance_to(p_point); if (d < min_distance) { min_distance = d; min_point = closest; @@ -870,7 +1064,7 @@ 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) { @@ -936,12 +1130,13 @@ Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const { return vertex[vert_support_idx]; } -void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { Vector3 n = p_normal; /** TEST FACE AS SUPPORT **/ - if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + if (Math::abs(normal.dot(n)) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 3; + r_type = FEATURE_FACE; for (int i = 0; i < 3; i++) { r_supports[i] = vertex[i]; } @@ -975,6 +1170,7 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = vertex[i]; r_supports[1] = vertex[nx]; return; @@ -982,6 +1178,7 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ } r_amount = 1; + r_type = FEATURE_POINT; r_supports[0] = vertex[vert_support_idx]; } @@ -990,7 +1187,11 @@ bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_e if (c) { r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; if (r_normal.dot(p_end - p_begin) > 0) { - r_normal = -r_normal; + if (backface_collision) { + r_normal = -r_normal; + } else { + c = false; + } } } @@ -1088,30 +1289,24 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par } if (bvh->face_index >= 0) { - Vector3 res; - Vector3 vertices[3] = { - p_params->vertices[p_params->faces[bvh->face_index].indices[0]], - p_params->vertices[p_params->faces[bvh->face_index].indices[1]], - p_params->vertices[p_params->faces[bvh->face_index].indices[2]] - }; + const Face *f = &p_params->faces[bvh->face_index]; + FaceShape3DSW *face = p_params->face; + face->normal = f->normal; + face->vertex[0] = p_params->vertices[f->indices[0]]; + face->vertex[1] = p_params->vertices[f->indices[1]]; + face->vertex[2] = p_params->vertices[f->indices[2]]; - if (Geometry3D::segment_intersects_triangle( - p_params->from, - p_params->to, - vertices[0], - vertices[1], - vertices[2], - &res)) { + Vector3 res; + Vector3 normal; + if (face->intersect_segment(p_params->from, p_params->to, res, normal)) { real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); - //TODO, seems segmen/triangle intersection is broken :( - if (d > 0 && d < p_params->min_d) { + if ((d > 0) && (d < p_params->min_d)) { p_params->min_d = d; p_params->result = res; - p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).normal; + p_params->normal = normal; p_params->collisions++; } } - } else { if (bvh->left >= 0) { _cull_segment(bvh->left, p_params); @@ -1132,17 +1327,20 @@ bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Ve const Vector3 *vr = vertices.ptr(); const BVH *br = bvh.ptr(); + FaceShape3DSW face; + face.backface_collision = backface_collision; + _SegmentCullParams params; params.from = p_begin; params.to = p_end; - params.collisions = 0; params.dir = (p_end - p_begin).normalized(); params.faces = fr; params.vertices = vr; params.bvh = br; - params.min_d = 1e20; + params.face = &face; + // cull _cull_segment(0, ¶ms); @@ -1204,6 +1402,7 @@ void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback const BVH *br = bvh.ptr(); FaceShape3DSW face; // use this to send in the callback + face.backface_collision = backface_collision; _CullParams params; params.aabb = local_aabb; @@ -1225,7 +1424,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 { @@ -1335,7 +1534,7 @@ void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_ar memdelete(p_bvh_tree); } -void ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) { +void ConcavePolygonShape3DSW::_setup(const Vector<Vector3> &p_faces, bool p_backface_collision) { int src_face_count = p_faces.size(); if (src_face_count == 0) { configure(AABB()); @@ -1390,15 +1589,24 @@ void ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) { int idx = 0; _fill_bvh(bvh_tree, bvh_arrayw2, idx); + backface_collision = p_backface_collision; + configure(_aabb); // this type of shape has no margin } void ConcavePolygonShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("faces")); + + _setup(d["faces"], d["backface_collision"]); } Variant ConcavePolygonShape3DSW::get_data() const { - return get_faces(); + Dictionary d; + d["faces"] = get_faces(); + d["backface_collision"] = backface_collision; + + return d; } ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() { @@ -1454,7 +1662,7 @@ 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) { diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index 2a2cd42255..988e76c699 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -67,8 +67,11 @@ protected: void configure(const AABB &p_aabb); public: - enum { - MAX_SUPPORTS = 8 + enum FeatureType { + FEATURE_POINT, + FEATURE_EDGE, + FEATURE_FACE, + FEATURE_CIRCLE, }; virtual real_t get_area() const { return aabb.get_area(); } @@ -85,7 +88,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &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) const = 0; + 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; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0; virtual bool intersect_point(const Vector3 &p_point) const = 0; @@ -110,7 +113,7 @@ class ConcaveShape3DSW : public Shape3DSW { public: virtual bool is_concave() const { return true; } typedef void (*Callback)(void *p_userdata, Shape3DSW *p_convex); - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; @@ -129,7 +132,7 @@ public: 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 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) const { r_amount = 0; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; @@ -156,7 +159,7 @@ public: 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 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) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; @@ -184,7 +187,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &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) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -209,7 +212,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &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) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -238,7 +241,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &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) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -251,6 +254,35 @@ public: CapsuleShape3DSW(); }; +class CylinderShape3DSW : public Shape3DSW { + real_t height; + real_t radius; + + void _setup(real_t p_height, real_t p_radius); + +public: + _FORCE_INLINE_ real_t get_height() const { return height; } + _FORCE_INLINE_ real_t get_radius() const { return radius; } + + virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + + virtual 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 Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + + virtual void set_data(const Variant &p_data); + virtual Variant get_data() const; + + CylinderShape3DSW(); +}; + struct ConvexPolygonShape3DSW : public Shape3DSW { Geometry3D::MeshData mesh; @@ -263,7 +295,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &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) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -302,34 +334,37 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { struct _CullParams { AABB aabb; - Callback callback; - void *userdata; - const Face *faces; - const Vector3 *vertices; - const BVH *bvh; - FaceShape3DSW *face; + Callback callback = nullptr; + void *userdata = nullptr; + const Face *faces = nullptr; + const Vector3 *vertices = nullptr; + const BVH *bvh = nullptr; + FaceShape3DSW *face = nullptr; }; struct _SegmentCullParams { Vector3 from; Vector3 to; - const Face *faces; - const Vector3 *vertices; - const BVH *bvh; Vector3 dir; + const Face *faces = nullptr; + const Vector3 *vertices = nullptr; + const BVH *bvh = nullptr; + FaceShape3DSW *face = nullptr; Vector3 result; Vector3 normal; - real_t min_d; - int collisions; + real_t min_d = 1e20; + int collisions = 0; }; + bool backface_collision = false; + void _cull_segment(int p_idx, _SegmentCullParams *p_params) const; void _cull(int p_idx, _CullParams *p_params) const; void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx); - void _setup(Vector<Vector3> p_faces); + void _setup(const Vector<Vector3> &p_faces, bool p_backface_collision); public: Vector<Vector3> get_faces() const; @@ -392,6 +427,7 @@ public: struct FaceShape3DSW : public Shape3DSW { Vector3 normal; //cache Vector3 vertex[3]; + bool backface_collision = false; virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } @@ -399,7 +435,7 @@ struct FaceShape3DSW : public Shape3DSW { void project_range(const Vector3 &p_normal, const Transform &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) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -437,7 +473,7 @@ struct MotionShape3DSW : public Shape3DSW { } return support; } - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; } virtual bool intersect_point(const Vector3 &p_point) const { return false; } virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; } diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp new file mode 100644 index 0000000000..f63a470cbe --- /dev/null +++ b/servers/physics_3d/soft_body_3d_sw.cpp @@ -0,0 +1,1221 @@ +/*************************************************************************/ +/* soft_body_3d_sw.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 "soft_body_3d_sw.h" +#include "space_3d_sw.h" + +#include "core/math/geometry_3d.h" +#include "core/templates/map.h" + +// Based on Bullet soft body. + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ +///btSoftBody implementation by Nathanael Presson + +SoftBody3DSW::SoftBody3DSW() : + CollisionObject3DSW(TYPE_SOFT_BODY), + active_list(this) { + _set_static(false); +} + +void SoftBody3DSW::_shapes_changed() { +} + +void SoftBody3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + _set_transform(p_variant); + _set_inv_transform(get_transform().inverse()); + + apply_nodes_transform(get_transform()); + + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + // Not supported. + ERR_FAIL_MSG("Linear velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_MSG("Angular velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); + } break; + } +} + +Variant SoftBody3DSW::get_state(PhysicsServer3D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + return get_transform(); + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + ERR_FAIL_V_MSG(Vector3(), "Linear velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_V_MSG(Vector3(), "Angular velocity is not supported for Soft bodies."); + return Vector3(); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); + } break; + } + + return Variant(); +} + +void SoftBody3DSW::set_space(Space3DSW *p_space) { + if (get_space()) { + get_space()->soft_body_remove_from_active_list(&active_list); + + deinitialize_shape(); + } + + _set_space(p_space); + + if (get_space()) { + get_space()->soft_body_add_to_active_list(&active_list); + + if (bounds != AABB()) { + initialize_shape(true); + } + } +} + +void SoftBody3DSW::set_mesh(const Ref<Mesh> &p_mesh) { + destroy(); + + soft_mesh = p_mesh; + + if (soft_mesh.is_null()) { + return; + } + + Array arrays = soft_mesh->surface_get_arrays(0); + ERR_FAIL_COND(!(soft_mesh->surface_get_format(0) & RS::ARRAY_FORMAT_INDEX)); + + bool success = create_from_trimesh(arrays[RS::ARRAY_INDEX], arrays[RS::ARRAY_VERTEX]); + if (!success) { + destroy(); + soft_mesh = Ref<Mesh>(); + } +} + +void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) { + if (soft_mesh.is_null()) { + return; + } + + const uint32_t vertex_count = map_visual_to_physics.size(); + for (uint32_t i = 0; i < vertex_count; ++i) { + const uint32_t node_index = map_visual_to_physics[i]; + const Node &node = nodes[node_index]; + const Vector3 &vertex_position = node.x; + const Vector3 &vertex_normal = node.n; + + p_rendering_server_handler->set_vertex(i, &vertex_position); + p_rendering_server_handler->set_normal(i, &vertex_normal); + } + + p_rendering_server_handler->set_aabb(bounds); +} + +void SoftBody3DSW::update_normals() { + uint32_t i, ni; + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + nodes[i].n = Vector3(); + } + + for (i = 0, ni = faces.size(); i < ni; ++i) { + Face &face = faces[i]; + const Vector3 n = vec3_cross(face.n[0]->x - face.n[2]->x, face.n[0]->x - face.n[1]->x); + face.n[0]->n += n; + face.n[1]->n += n; + face.n[2]->n += n; + face.normal = n; + face.normal.normalize(); + } + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + real_t len = node.n.length(); + if (len > CMP_EPSILON) { + node.n /= len; + } + } +} + +void SoftBody3DSW::update_bounds() { + AABB prev_bounds = bounds; + prev_bounds.grow_by(collision_margin); + + bounds = AABB(); + + const uint32_t nodes_count = nodes.size(); + if (nodes_count == 0) { + deinitialize_shape(); + return; + } + + bool first = true; + bool moved = false; + for (uint32_t node_index = 0; node_index < nodes_count; ++node_index) { + const Node &node = nodes[node_index]; + if (!prev_bounds.has_point(node.x)) { + moved = true; + } + if (first) { + bounds.position = node.x; + first = false; + } else { + bounds.expand_to(node.x); + } + } + + if (get_space()) { + initialize_shape(moved); + } +} + +void SoftBody3DSW::update_constants() { + reset_link_rest_lengths(); + update_link_constants(); + update_area(); +} + +void SoftBody3DSW::update_area() { + int i, ni; + + // Face area. + for (i = 0, ni = faces.size(); i < ni; ++i) { + Face &face = faces[i]; + + const Vector3 &x0 = face.n[0]->x; + const Vector3 &x1 = face.n[1]->x; + const Vector3 &x2 = face.n[2]->x; + + const Vector3 a = x1 - x0; + const Vector3 b = x2 - x0; + const Vector3 cr = vec3_cross(a, b); + face.ra = cr.length(); + } + + // Node area. + LocalVector<int> counts; + counts.resize(nodes.size()); + memset(counts.ptr(), 0, counts.size() * sizeof(int)); + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + nodes[i].area = 0.0; + } + + for (i = 0, ni = faces.size(); i < ni; ++i) { + const Face &face = faces[i]; + for (int j = 0; j < 3; ++j) { + const int index = (int)(face.n[j] - &nodes[0]); + counts[index]++; + face.n[j]->area += Math::abs(face.ra); + } + } + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + if (counts[i] > 0) { + nodes[i].area /= (real_t)counts[i]; + } else { + nodes[i].area = 0.0; + } + } +} + +void SoftBody3DSW::reset_link_rest_lengths() { + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.rl = (link.n[0]->x - link.n[1]->x).length(); + link.c1 = link.rl * link.rl; + } +} + +void SoftBody3DSW::update_link_constants() { + real_t inv_linear_stiffness = 1.0 / linear_stiffness; + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.c0 = (link.n[0]->im + link.n[1]->im) * inv_linear_stiffness; + } +} + +void SoftBody3DSW::apply_nodes_transform(const Transform &p_transform) { + if (soft_mesh.is_null()) { + return; + } + + uint32_t node_count = nodes.size(); + Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; + for (uint32_t node_index = 0; node_index < node_count; ++node_index) { + Node &node = nodes[node_index]; + + node.x = p_transform.xform(node.x); + node.q = node.x; + node.v = Vector3(); + node.bv = Vector3(); + + AABB node_aabb(node.x, leaf_size); + node_tree.update(node.leaf, node_aabb); + } + + face_tree.clear(); + + update_normals(); + update_bounds(); + update_constants(); +} + +Vector3 SoftBody3DSW::get_vertex_position(int p_index) const { + if (soft_mesh.is_null()) { + return Vector3(); + } + + ERR_FAIL_INDEX_V(p_index, (int)map_visual_to_physics.size(), Vector3()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND_V(node_index >= nodes.size(), Vector3()); + return nodes[node_index].x; +} + +void SoftBody3DSW::set_vertex_position(int p_index, const Vector3 &p_position) { + if (soft_mesh.is_null()) { + return; + } + + ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.q = node.x; + node.x = p_position; +} + +void SoftBody3DSW::pin_vertex(int p_index) { + if (is_vertex_pinned(p_index)) { + return; + } + + pinned_vertices.push_back(p_index); + + if (!soft_mesh.is_null()) { + ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.im = 0.0; + } +} + +void SoftBody3DSW::unpin_vertex(int p_index) { + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + if (p_index == pinned_vertices[i]) { + pinned_vertices.remove(i); + + if (!soft_mesh.is_null()) { + ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + real_t inv_node_mass = nodes.size() * inv_total_mass; + + Node &node = nodes[node_index]; + node.im = inv_node_mass; + } + + return; + } + } +} + +void SoftBody3DSW::unpin_all_vertices() { + if (!soft_mesh.is_null()) { + real_t inv_node_mass = nodes.size() * inv_total_mass; + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + uint32_t vertex_index = pinned_vertices[i]; + + ERR_CONTINUE(vertex_index >= map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[vertex_index]; + + ERR_CONTINUE(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.im = inv_node_mass; + } + } + + pinned_vertices.clear(); +} + +bool SoftBody3DSW::is_vertex_pinned(int p_index) const { + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + if (p_index == pinned_vertices[i]) { + return true; + } + } + + return false; +} + +uint32_t SoftBody3DSW::get_node_count() const { + return nodes.size(); +} + +real_t SoftBody3DSW::get_node_inv_mass(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), 0.0); + return nodes[p_node_index].im; +} + +Vector3 SoftBody3DSW::get_node_position(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].x; +} + +Vector3 SoftBody3DSW::get_node_velocity(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].v; +} + +Vector3 SoftBody3DSW::get_node_biased_velocity(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].bv; +} + +void SoftBody3DSW::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { + ERR_FAIL_COND(p_node_index >= nodes.size()); + Node &node = nodes[p_node_index]; + node.v += p_impulse * node.im; +} + +void SoftBody3DSW::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { + ERR_FAIL_COND(p_node_index >= nodes.size()); + Node &node = nodes[p_node_index]; + node.bv += p_impulse * node.im; +} + +uint32_t SoftBody3DSW::get_face_count() const { + return faces.size(); +} + +void SoftBody3DSW::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const { + ERR_FAIL_COND(p_face_index >= faces.size()); + const Face &face = faces[p_face_index]; + r_point_1 = face.n[0]->x; + r_point_2 = face.n[1]->x; + r_point_3 = face.n[2]->x; +} + +Vector3 SoftBody3DSW::get_face_normal(uint32_t p_face_index) const { + ERR_FAIL_COND_V(p_face_index >= faces.size(), Vector3()); + return faces[p_face_index].normal; +} + +bool SoftBody3DSW::create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices) { + uint32_t node_count = 0; + LocalVector<Vector3> vertices; + const int visual_vertex_count(p_vertices.size()); + + LocalVector<int> triangles; + const uint32_t triangle_count(p_indices.size() / 3); + triangles.resize(triangle_count * 3); + + // Merge all overlapping vertices and create a map of physical vertices to visual vertices. + { + // Process vertices. + { + uint32_t vertex_count = 0; + Map<Vector3, uint32_t> unique_vertices; + + vertices.resize(visual_vertex_count); + map_visual_to_physics.resize(visual_vertex_count); + + for (int visual_vertex_index = 0; visual_vertex_index < visual_vertex_count; ++visual_vertex_index) { + const Vector3 &vertex = p_vertices[visual_vertex_index]; + + Map<Vector3, uint32_t>::Element *e = unique_vertices.find(vertex); + uint32_t vertex_id; + if (e) { + // Already existing. + vertex_id = e->value(); + } else { + // Create new one. + vertex_id = vertex_count++; + unique_vertices[vertex] = vertex_id; + vertices[vertex_id] = vertex; + } + + map_visual_to_physics[visual_vertex_index] = vertex_id; + } + + vertices.resize(vertex_count); + } + + // Process triangles. + { + for (uint32_t triangle_index = 0; triangle_index < triangle_count; ++triangle_index) { + for (int i = 0; i < 3; ++i) { + int visual_index = 3 * triangle_index + i; + int physics_index = map_visual_to_physics[p_indices[visual_index]]; + triangles[visual_index] = physics_index; + node_count = MAX((int)node_count, physics_index); + } + } + } + } + + ++node_count; + + // Create nodes from vertices. + nodes.resize(node_count); + real_t inv_node_mass = node_count * inv_total_mass; + Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; + for (uint32_t i = 0; i < node_count; ++i) { + Node &node = nodes[i]; + node.s = vertices[i]; + node.x = node.s; + node.q = node.s; + node.im = inv_node_mass; + + AABB node_aabb(node.x, leaf_size); + node.leaf = node_tree.insert(node_aabb, &node); + + node.index = i; + } + + // Create links and faces from triangles. + LocalVector<bool> chks; + chks.resize(node_count * node_count); + memset(chks.ptr(), 0, chks.size() * sizeof(bool)); + + for (uint32_t i = 0; i < triangle_count * 3; i += 3) { + const int idx[] = { triangles[i], triangles[i + 1], triangles[i + 2] }; + + for (int j = 2, k = 0; k < 3; j = k++) { + int chk = idx[k] * node_count + idx[j]; + if (!chks[chk]) { + chks[chk] = true; + int inv_chk = idx[j] * node_count + idx[k]; + chks[inv_chk] = true; + + append_link(idx[j], idx[k]); + } + } + + append_face(idx[0], idx[1], idx[2]); + } + + // Set pinned nodes. + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + int pinned_vertex = pinned_vertices[i]; + + ERR_CONTINUE(pinned_vertex >= visual_vertex_count); + uint32_t node_index = map_visual_to_physics[pinned_vertex]; + + ERR_CONTINUE(node_index >= node_count); + Node &node = nodes[node_index]; + node.im = 0.0; + } + + generate_bending_constraints(2); + reoptimize_link_order(); + + update_constants(); + update_normals(); + update_bounds(); + + return true; +} + +void SoftBody3DSW::generate_bending_constraints(int p_distance) { + uint32_t i, j; + + if (p_distance > 1) { + // Build graph. + const uint32_t n = nodes.size(); + const unsigned inf = (~(unsigned)0) >> 1; + const uint32_t adj_size = n * n; + unsigned *adj = memnew_arr(unsigned, adj_size); + +#define IDX(_x_, _y_) ((_y_)*n + (_x_)) + for (j = 0; j < n; ++j) { + for (i = 0; i < n; ++i) { + int idx_ij = j * n + i; + int idx_ji = i * n + j; + if (i != j) { + adj[idx_ij] = adj[idx_ji] = inf; + } else { + adj[idx_ij] = adj[idx_ji] = 0; + } + } + } + for (i = 0; i < links.size(); ++i) { + const int ia = (int)(links[i].n[0] - &nodes[0]); + const int ib = (int)(links[i].n[1] - &nodes[0]); + int idx = ib * n + ia; + int idx_inv = ia * n + ib; + adj[idx] = 1; + adj[idx_inv] = 1; + } + + // Special optimized case for distance == 2. + if (p_distance == 2) { + LocalVector<LocalVector<int>> node_links; + + // Build node links. + node_links.resize(nodes.size()); + + for (i = 0; i < links.size(); ++i) { + const int ia = (int)(links[i].n[0] - &nodes[0]); + const int ib = (int)(links[i].n[1] - &nodes[0]); + if (node_links[ia].find(ib) == -1) { + node_links[ia].push_back(ib); + } + + if (node_links[ib].find(ia) == -1) { + node_links[ib].push_back(ia); + } + } + for (uint32_t ii = 0; ii < node_links.size(); ii++) { + for (uint32_t jj = 0; jj < node_links[ii].size(); jj++) { + int k = node_links[ii][jj]; + for (uint32_t kk = 0; kk < node_links[k].size(); kk++) { + int l = node_links[k][kk]; + if ((int)ii != l) { + int idx_ik = k * n + ii; + int idx_kj = l * n + k; + const unsigned sum = adj[idx_ik] + adj[idx_kj]; + ERR_FAIL_COND(sum != 2); + int idx_ij = l * n + ii; + if (adj[idx_ij] > sum) { + int idx_ji = l * n + ii; + adj[idx_ij] = adj[idx_ji] = sum; + } + } + } + } + } + } else { + // Generic Floyd's algorithm. + for (uint32_t k = 0; k < n; ++k) { + for (j = 0; j < n; ++j) { + for (i = j + 1; i < n; ++i) { + int idx_ik = k * n + i; + int idx_kj = j * n + k; + const unsigned sum = adj[idx_ik] + adj[idx_kj]; + int idx_ij = j * n + i; + if (adj[idx_ij] > sum) { + int idx_ji = j * n + i; + adj[idx_ij] = adj[idx_ji] = sum; + } + } + } + } + } + + // Build links. + for (j = 0; j < n; ++j) { + for (i = j + 1; i < n; ++i) { + int idx_ij = j * n + i; + if (adj[idx_ij] == (unsigned)p_distance) { + append_link(i, j); + } + } + } + memdelete_arr(adj); + } +} + +//=================================================================== +// +// +// 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 +// be exploited by out-of-order instruction processors with large but +// (inevitably) finite instruction windows. +// +//=================================================================== + +// A small structure to track lists of dependent link calculations. +class LinkDeps { +public: + int value; // A link calculation that is dependent on this one + // Positive values = "input A" while negative values = "input B" + LinkDeps *next; // Next dependence in the list +}; +typedef LinkDeps *LinkDepsPtr; + +void SoftBody3DSW::reoptimize_link_order() { + const int reop_not_dependent = -1; + const int reop_node_complete = -2; + + uint32_t i, link_count = links.size(), node_count = nodes.size(); + Link *lr; + int ar, br; + Node *node0 = &(nodes[0]); + Node *node1 = &(nodes[1]); + LinkDepsPtr link_dep; + int ready_list_head, ready_list_tail, link_num, link_dep_frees, dep_link; + + // Allocate temporary buffers. + int *node_written_at = memnew_arr(int, node_count + 1); // What link calculation produced this node's current values? + int *link_dep_A = memnew_arr(int, link_count); // Link calculation input is dependent upon prior calculation #N + int *link_dep_B = memnew_arr(int, link_count); + int *ready_list = memnew_arr(int, link_count); // List of ready-to-process link calculations (# of links, maximum) + LinkDeps *link_dep_free_list = memnew_arr(LinkDeps, 2 * link_count); // Dependent-on-me list elements (2x# of links, maximum) + LinkDepsPtr *link_dep_list_starts = memnew_arr(LinkDepsPtr, link_count); // Start nodes of dependent-on-me lists, one for each link + + // Copy the original, unsorted links to a side buffer. + Link *link_buffer = memnew_arr(Link, link_count); + memcpy(link_buffer, &(links[0]), sizeof(Link) * link_count); + + // Clear out the node setup and ready list. + for (i = 0; i < node_count + 1; i++) { + node_written_at[i] = reop_not_dependent; + } + for (i = 0; i < link_count; i++) { + link_dep_list_starts[i] = nullptr; + } + ready_list_head = ready_list_tail = link_dep_frees = 0; + + // Initial link analysis to set up data structures. + for (i = 0; i < link_count; i++) { + // Note which prior link calculations we are dependent upon & build up dependence lists. + lr = &(links[i]); + ar = (lr->n[0] - node0) / (node1 - node0); + br = (lr->n[1] - node0) / (node1 - node0); + if (node_written_at[ar] > reop_not_dependent) { + link_dep_A[i] = node_written_at[ar]; + link_dep = &link_dep_free_list[link_dep_frees++]; + link_dep->value = i; + link_dep->next = link_dep_list_starts[node_written_at[ar]]; + link_dep_list_starts[node_written_at[ar]] = link_dep; + } else { + link_dep_A[i] = reop_not_dependent; + } + if (node_written_at[br] > reop_not_dependent) { + link_dep_B[i] = node_written_at[br]; + link_dep = &link_dep_free_list[link_dep_frees++]; + link_dep->value = -(int)(i + 1); + link_dep->next = link_dep_list_starts[node_written_at[br]]; + link_dep_list_starts[node_written_at[br]] = link_dep; + } else { + link_dep_B[i] = reop_not_dependent; + } + + // Add this link to the initial ready list, if it is not dependent on any other links. + if ((link_dep_A[i] == reop_not_dependent) && (link_dep_B[i] == reop_not_dependent)) { + ready_list[ready_list_tail++] = i; + link_dep_A[i] = link_dep_B[i] = reop_node_complete; // Probably not needed now. + } + + // Update the nodes to mark which ones are calculated by this link. + node_written_at[ar] = node_written_at[br] = i; + } + + // Process the ready list and create the sorted list of links: + // -- By treating the ready list as a queue, we maximize the distance between any + // inter-dependent node calculations. + // -- All other (non-related) nodes in the ready list will automatically be inserted + // in between each set of inter-dependent link calculations by this loop. + i = 0; + while (ready_list_head != ready_list_tail) { + // Use ready list to select the next link to process. + link_num = ready_list[ready_list_head++]; + // Copy the next-to-calculate link back into the original link array. + links[i++] = link_buffer[link_num]; + + // Free up any link inputs that are dependent on this one. + link_dep = link_dep_list_starts[link_num]; + while (link_dep) { + dep_link = link_dep->value; + if (dep_link >= 0) { + link_dep_A[dep_link] = reop_not_dependent; + } else { + dep_link = -dep_link - 1; + link_dep_B[dep_link] = reop_not_dependent; + } + // Add this dependent link calculation to the ready list if *both* inputs are clear. + if ((link_dep_A[dep_link] == reop_not_dependent) && (link_dep_B[dep_link] == reop_not_dependent)) { + ready_list[ready_list_tail++] = dep_link; + link_dep_A[dep_link] = link_dep_B[dep_link] = reop_node_complete; // Probably not needed now. + } + link_dep = link_dep->next; + } + } + + // Delete the temporary buffers. + memdelete_arr(node_written_at); + memdelete_arr(link_dep_A); + memdelete_arr(link_dep_B); + memdelete_arr(ready_list); + memdelete_arr(link_dep_free_list); + memdelete_arr(link_dep_list_starts); + memdelete_arr(link_buffer); +} + +void SoftBody3DSW::append_link(uint32_t p_node1, uint32_t p_node2) { + if (p_node1 == p_node2) { + return; + } + + Node *node1 = &nodes[p_node1]; + Node *node2 = &nodes[p_node2]; + + Link link; + link.n[0] = node1; + link.n[1] = node2; + link.rl = (node1->x - node2->x).length(); + + links.push_back(link); +} + +void SoftBody3DSW::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) { + if (p_node1 == p_node2) { + return; + } + if (p_node1 == p_node3) { + return; + } + if (p_node2 == p_node3) { + return; + } + + Node *node1 = &nodes[p_node1]; + Node *node2 = &nodes[p_node2]; + Node *node3 = &nodes[p_node3]; + + Face face; + face.n[0] = node1; + face.n[1] = node2; + face.n[2] = node3; + + face.index = faces.size(); + + faces.push_back(face); +} + +void SoftBody3DSW::set_iteration_count(int p_val) { + iteration_count = p_val; +} + +void SoftBody3DSW::set_total_mass(real_t p_val) { + ERR_FAIL_COND(p_val < 0.0); + + inv_total_mass = 1.0 / p_val; + real_t mass_factor = total_mass * inv_total_mass; + total_mass = p_val; + + uint32_t node_count = nodes.size(); + for (uint32_t node_index = 0; node_index < node_count; ++node_index) { + Node &node = nodes[node_index]; + node.im *= mass_factor; + } + + update_constants(); +} + +void SoftBody3DSW::set_collision_margin(real_t p_val) { + collision_margin = p_val; +} + +void SoftBody3DSW::set_linear_stiffness(real_t p_val) { + linear_stiffness = p_val; +} + +void SoftBody3DSW::set_pressure_coefficient(real_t p_val) { + pressure_coefficient = p_val; +} + +void SoftBody3DSW::set_damping_coefficient(real_t p_val) { + damping_coefficient = p_val; +} + +void SoftBody3DSW::set_drag_coefficient(real_t p_val) { + drag_coefficient = p_val; +} + +void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) { + for (uint32_t i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + if (node.im > 0) { + node.v += p_velocity; + } + } +} + +void SoftBody3DSW::apply_forces() { + if (pressure_coefficient < CMP_EPSILON) { + return; + } + + if (nodes.is_empty()) { + return; + } + + uint32_t i, ni; + + // Calculate volume. + real_t volume = 0.0; + const Vector3 &org = nodes[0].x; + for (i = 0, ni = faces.size(); i < ni; ++i) { + const Face &face = faces[i]; + volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org)); + } + volume /= 6.0; + + // Apply per node forces. + real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + if (node.im > 0) { + node.f += node.n * (node.area * ivolumetp); + } + } +} + +void SoftBody3DSW::predict_motion(real_t p_delta) { + const real_t inv_delta = 1.0 / p_delta; + + ERR_FAIL_COND(!get_space()); + + Area3DSW *def_area = get_space()->get_default_area(); + ERR_FAIL_COND(!def_area); + + // Apply forces. + Vector3 gravity = def_area->get_gravity_vector() * def_area->get_gravity(); + add_velocity(gravity * p_delta); + apply_forces(); + + // Avoid soft body from 'exploding' so use some upper threshold of maximum motion + // that a node can travel per frame. + const real_t max_displacement = 1000.0; + real_t clamp_delta_v = max_displacement * inv_delta; + + // Integrate. + uint32_t i, ni; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + node.q = node.x; + Vector3 delta_v = node.f * node.im * p_delta; + for (int c = 0; c < 3; c++) { + delta_v[c] = CLAMP(delta_v[c], -clamp_delta_v, clamp_delta_v); + } + node.v += delta_v; + node.x += node.v * p_delta; + node.f = Vector3(); + } + + // Bounds and tree update. + update_bounds(); + + // Node tree update. + for (i = 0, ni = nodes.size(); i < ni; ++i) { + const Node &node = nodes[i]; + + AABB node_aabb(node.x, Vector3()); + node_aabb.expand_to(node.x + node.v * p_delta); + node_aabb.grow_by(collision_margin); + + node_tree.update(node.leaf, node_aabb); + } + + // Face tree update. + if (!face_tree.is_empty()) { + update_face_tree(p_delta); + } + + // Optimize node tree. + node_tree.optimize_incremental(1); + face_tree.optimize_incremental(1); +} + +void SoftBody3DSW::solve_constraints(real_t p_delta) { + const real_t inv_delta = 1.0 / p_delta; + + uint32_t i, ni; + + for (i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.c3 = link.n[1]->q - link.n[0]->q; + link.c2 = 1 / (link.c3.length_squared() * link.c0); + } + + // Solve velocities. + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + node.x = node.q + node.v * p_delta; + } + + // Solve positions. + for (int isolve = 0; isolve < iteration_count; ++isolve) { + const real_t ti = isolve / (real_t)iteration_count; + solve_links(1.0, ti); + } + const real_t vc = (1.0 - damping_coefficient) * inv_delta; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + + node.x += node.bv * p_delta; + node.bv = Vector3(); + + node.v = (node.x - node.q) * vc; + + node.q = node.x; + } + + update_normals(); +} + +void SoftBody3DSW::solve_links(real_t kst, real_t ti) { + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + if (link.c0 > 0) { + Node &node_a = *link.n[0]; + Node &node_b = *link.n[1]; + const Vector3 del = node_b.x - node_a.x; + const real_t len = del.length_squared(); + if (link.c1 + len > CMP_EPSILON) { + const real_t k = ((link.c1 - len) / (link.c0 * (link.c1 + len))) * kst; + node_a.x -= del * (k * node_a.im); + node_b.x += del * (k * node_b.im); + } + } + } +} + +struct AABBQueryResult { + const SoftBody3DSW *soft_body = nullptr; + void *userdata = nullptr; + SoftBody3DSW::QueryResultCallback result_callback = nullptr; + + _FORCE_INLINE_ bool operator()(void *p_data) { + return result_callback(soft_body->get_node_index(p_data), userdata); + }; +}; + +void SoftBody3DSW::query_aabb(const AABB &p_aabb, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) { + AABBQueryResult query_result; + query_result.soft_body = this; + query_result.result_callback = p_result_callback; + query_result.userdata = p_userdata; + + node_tree.aabb_query(p_aabb, query_result); +} + +struct RayQueryResult { + const SoftBody3DSW *soft_body = nullptr; + void *userdata = nullptr; + SoftBody3DSW::QueryResultCallback result_callback = nullptr; + + _FORCE_INLINE_ bool operator()(void *p_data) { + return result_callback(soft_body->get_face_index(p_data), userdata); + }; +}; + +void SoftBody3DSW::query_ray(const Vector3 &p_from, const Vector3 &p_to, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) { + if (face_tree.is_empty()) { + initialize_face_tree(); + } + + RayQueryResult query_result; + query_result.soft_body = this; + query_result.result_callback = p_result_callback; + query_result.userdata = p_userdata; + + face_tree.ray_query(p_from, p_to, query_result); +} + +void SoftBody3DSW::initialize_face_tree() { + face_tree.clear(); + for (uint32_t i = 0; i < faces.size(); ++i) { + Face &face = faces[i]; + + AABB face_aabb; + + face_aabb.position = face.n[0]->x; + face_aabb.expand_to(face.n[1]->x); + face_aabb.expand_to(face.n[2]->x); + + face_aabb.grow_by(collision_margin); + + face.leaf = face_tree.insert(face_aabb, &face); + } +} + +void SoftBody3DSW::update_face_tree(real_t p_delta) { + for (uint32_t i = 0; i < faces.size(); ++i) { + const Face &face = faces[i]; + + AABB face_aabb; + + const Node *node0 = face.n[0]; + face_aabb.position = node0->x; + face_aabb.expand_to(node0->x + node0->v * p_delta); + + const Node *node1 = face.n[1]; + face_aabb.expand_to(node1->x); + face_aabb.expand_to(node1->x + node1->v * p_delta); + + const Node *node2 = face.n[2]; + face_aabb.expand_to(node2->x); + face_aabb.expand_to(node2->x + node2->v * p_delta); + + face_aabb.grow_by(collision_margin); + + face_tree.update(face.leaf, face_aabb); + } +} + +void SoftBody3DSW::initialize_shape(bool p_force_move) { + if (get_shape_count() == 0) { + SoftBodyShape3DSW *soft_body_shape = memnew(SoftBodyShape3DSW(this)); + add_shape(soft_body_shape); + } else if (p_force_move) { + SoftBodyShape3DSW *soft_body_shape = static_cast<SoftBodyShape3DSW *>(get_shape(0)); + soft_body_shape->update_bounds(); + } +} + +void SoftBody3DSW::deinitialize_shape() { + if (get_shape_count() > 0) { + Shape3DSW *shape = get_shape(0); + remove_shape(shape); + memdelete(shape); + } +} + +void SoftBody3DSW::destroy() { + map_visual_to_physics.clear(); + + node_tree.clear(); + face_tree.clear(); + + nodes.clear(); + links.clear(); + faces.clear(); + + bounds = AABB(); + deinitialize_shape(); +} + +void SoftBodyShape3DSW::update_bounds() { + ERR_FAIL_COND(!soft_body); + + AABB collision_aabb = soft_body->get_bounds(); + collision_aabb.grow_by(soft_body->get_collision_margin()); + configure(collision_aabb); +} + +SoftBodyShape3DSW::SoftBodyShape3DSW(SoftBody3DSW *p_soft_body) { + soft_body = p_soft_body; + update_bounds(); +} + +struct _SoftBodyIntersectSegmentInfo { + const SoftBody3DSW *soft_body = nullptr; + Vector3 from; + Vector3 dir; + Vector3 hit_position; + uint32_t hit_face_index = -1; + real_t hit_dist_sq = Math_INF; + + static bool process_hit(uint32_t p_face_index, void *p_userdata) { + _SoftBodyIntersectSegmentInfo &query_info = *(_SoftBodyIntersectSegmentInfo *)(p_userdata); + + Vector3 points[3]; + query_info.soft_body->get_face_points(p_face_index, points[0], points[1], points[2]); + + Vector3 result; + if (Geometry3D::ray_intersects_triangle(query_info.from, query_info.dir, points[0], points[1], points[2], &result)) { + real_t dist_sq = query_info.from.distance_squared_to(result); + if (dist_sq < query_info.hit_dist_sq) { + query_info.hit_dist_sq = dist_sq; + query_info.hit_position = result; + query_info.hit_face_index = p_face_index; + } + } + + // Continue with the query. + return false; + } +}; + +bool SoftBodyShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + _SoftBodyIntersectSegmentInfo query_info; + query_info.soft_body = soft_body; + query_info.from = p_begin; + query_info.dir = (p_end - p_begin).normalized(); + + soft_body->query_ray(p_begin, p_end, _SoftBodyIntersectSegmentInfo::process_hit, &query_info); + + if (query_info.hit_dist_sq != Math_INF) { + r_result = query_info.hit_position; + r_normal = soft_body->get_face_normal(query_info.hit_face_index); + return true; + } + + return false; +} + +bool SoftBodyShape3DSW::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 SoftBodyShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + ERR_FAIL_V_MSG(Vector3(), "Get closest point is not supported for soft bodies."); +} diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h new file mode 100644 index 0000000000..6c0451ff45 --- /dev/null +++ b/servers/physics_3d/soft_body_3d_sw.h @@ -0,0 +1,247 @@ +/*************************************************************************/ +/* soft_body_3d_sw.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 SOFT_BODY_3D_SW_H +#define SOFT_BODY_3D_SW_H + +#include "collision_object_3d_sw.h" + +#include "core/math/aabb.h" +#include "core/math/dynamic_bvh.h" +#include "core/math/vector3.h" +#include "core/templates/local_vector.h" +#include "core/templates/set.h" +#include "core/templates/vset.h" +#include "scene/resources/mesh.h" + +class Constraint3DSW; + +class SoftBody3DSW : public CollisionObject3DSW { + Ref<Mesh> soft_mesh; + + struct Node { + Vector3 s; // Source position + Vector3 x; // Position + Vector3 q; // Previous step position/Test position + Vector3 f; // Force accumulator + Vector3 v; // Velocity + Vector3 bv; // Biased Velocity + Vector3 n; // Normal + real_t area = 0.0; // Area + real_t im = 0.0; // 1/mass + DynamicBVH::ID leaf; // Leaf data + uint32_t index = 0; + }; + + struct Link { + Vector3 c3; // gradient + Node *n[2] = { nullptr, nullptr }; // Node pointers + real_t rl = 0.0; // Rest length + real_t c0 = 0.0; // (ima+imb)*kLST + real_t c1 = 0.0; // rl^2 + real_t c2 = 0.0; // |gradient|^2/c0 + }; + + struct Face { + Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers + Vector3 normal; // Normal + real_t ra = 0.0; // Rest area + DynamicBVH::ID leaf; // Leaf data + uint32_t index = 0; + }; + + LocalVector<Node> nodes; + LocalVector<Link> links; + LocalVector<Face> faces; + + DynamicBVH node_tree; + DynamicBVH face_tree; + + LocalVector<uint32_t> map_visual_to_physics; + + AABB bounds; + + real_t collision_margin = 0.05; + + real_t total_mass = 1.0; + real_t inv_total_mass = 1.0; + + int iteration_count = 5; + real_t linear_stiffness = 0.5; // [0,1] + real_t pressure_coefficient = 0.0; // [-inf,+inf] + real_t damping_coefficient = 0.01; // [0,1] + real_t drag_coefficient = 0.0; // [0,1] + LocalVector<int> pinned_vertices; + + SelfList<SoftBody3DSW> active_list; + + Set<Constraint3DSW *> constraints; + + VSet<RID> exceptions; + +public: + SoftBody3DSW(); + + const AABB &get_bounds() const { return bounds; } + + void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); + Variant get_state(PhysicsServer3D::BodyState p_state) const; + + _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const Set<Constraint3DSW *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } + + _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } + _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } + _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; } + + virtual void set_space(Space3DSW *p_space); + + void set_mesh(const Ref<Mesh> &p_mesh); + + void update_rendering_server(RenderingServerHandler *p_rendering_server_handler); + + Vector3 get_vertex_position(int p_index) const; + void set_vertex_position(int p_index, const Vector3 &p_position); + + void pin_vertex(int p_index); + void unpin_vertex(int p_index); + void unpin_all_vertices(); + bool is_vertex_pinned(int p_index) const; + + uint32_t get_node_count() const; + real_t get_node_inv_mass(uint32_t p_node_index) const; + Vector3 get_node_position(uint32_t p_node_index) const; + Vector3 get_node_velocity(uint32_t p_node_index) const; + Vector3 get_node_biased_velocity(uint32_t p_node_index) const; + void apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse); + void apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse); + + uint32_t get_face_count() const; + void get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const; + Vector3 get_face_normal(uint32_t p_face_index) const; + + void set_iteration_count(int p_val); + _FORCE_INLINE_ real_t get_iteration_count() const { return iteration_count; } + + void set_total_mass(real_t p_val); + _FORCE_INLINE_ real_t get_total_mass() const { return total_mass; } + _FORCE_INLINE_ real_t get_total_inv_mass() const { return inv_total_mass; } + + void set_collision_margin(real_t p_val); + _FORCE_INLINE_ real_t get_collision_margin() const { return collision_margin; } + + void set_linear_stiffness(real_t p_val); + _FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; } + + void set_pressure_coefficient(real_t p_val); + _FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; } + + void set_damping_coefficient(real_t p_val); + _FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; } + + void set_drag_coefficient(real_t p_val); + _FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; } + + void predict_motion(real_t p_delta); + void solve_constraints(real_t p_delta); + + _FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return ((Node *)p_node)->index; } + _FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return ((Face *)p_face)->index; } + + // Return true to stop the query. + // p_index is the node index for AABB query, face index for Ray query. + typedef bool (*QueryResultCallback)(uint32_t p_index, void *p_userdata); + + void query_aabb(const AABB &p_aabb, QueryResultCallback p_result_callback, void *p_userdata); + void query_ray(const Vector3 &p_from, const Vector3 &p_to, QueryResultCallback p_result_callback, void *p_userdata); + +protected: + virtual void _shapes_changed(); + +private: + void update_normals(); + void update_bounds(); + void update_constants(); + void update_area(); + void reset_link_rest_lengths(); + void update_link_constants(); + + void apply_nodes_transform(const Transform &p_transform); + + void add_velocity(const Vector3 &p_velocity); + + void apply_forces(); + + bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices); + void generate_bending_constraints(int p_distance); + void reoptimize_link_order(); + void append_link(uint32_t p_node1, uint32_t p_node2); + void append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3); + + void solve_links(real_t kst, real_t ti); + + void initialize_face_tree(); + void update_face_tree(real_t p_delta); + + void initialize_shape(bool p_force_move = true); + void deinitialize_shape(); + + void destroy(); +}; + +class SoftBodyShape3DSW : public Shape3DSW { + SoftBody3DSW *soft_body = nullptr; + +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 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; } + + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); } + + virtual void set_data(const Variant &p_data) {} + virtual Variant get_data() const { return Variant(); } + + void update_bounds(); + + SoftBodyShape3DSW(SoftBody3DSW *p_soft_body); + ~SoftBodyShape3DSW() {} +}; + +#endif // SOFT_BODY_3D_SW_H diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 48f250ba35..2df824b320 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -31,7 +31,7 @@ #include "space_3d_sw.h" #include "collision_solver_3d_sw.h" -#include "core/project_settings.h" +#include "core/config/project_settings.h" #include "physics_server_3d_sw.h" _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { @@ -47,6 +47,10 @@ _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint return false; } + if (p_object->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY && !p_collide_with_bodies) { + return false; + } + return true; } @@ -181,7 +185,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans return 0; } - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_xform.xform(shape->get_aabb()); @@ -210,6 +214,10 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; + if (col_obj->is_shape_set_as_disabled(shape_idx)) { + continue; + } + if (!CollisionSolver3DSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) { continue; } @@ -232,7 +240,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans } 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) { - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, false); AABB aabb = p_xform.xform(shape->get_aabb()); @@ -265,6 +273,10 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; + if (col_obj->is_shape_set_as_disabled(shape_idx)) { + continue; + } + Vector3 point_A, point_B; Vector3 sep_axis = p_motion.normalized(); @@ -274,11 +286,11 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor continue; } - //test initial overlap + //test initial overlap, ignore objects it's inside of. sep_axis = p_motion.normalized(); if (!CollisionSolver3DSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { - return false; + continue; } //just do kinematic solving @@ -324,7 +336,8 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor best_first = false; if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) { const Body3DSW *body = static_cast<const Body3DSW *>(col_obj); - r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B); + Vector3 rel_vec = closest_B - (body->get_transform().origin + body->get_center_of_mass()); + r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); } } } @@ -340,7 +353,7 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ return false; } - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_shape_xform.xform(shape->get_aabb()); @@ -365,12 +378,17 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ } const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; if (p_exclude.has(col_obj->get_self())) { continue; } + int shape_idx = space->intersection_query_subindex_results[i]; + + if (col_obj->is_shape_set_as_disabled(shape_idx)) { + continue; + } + if (CollisionSolver3DSW::solve_static(shape, p_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 = true; } @@ -384,6 +402,8 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ struct _RestCallbackData { const CollisionObject3DSW *object; const CollisionObject3DSW *best_object; + int local_shape; + int best_local_shape; int shape; int best_shape; Vector3 best_contact; @@ -392,7 +412,7 @@ struct _RestCallbackData { real_t min_allowed_depth; }; -static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { +static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { _RestCallbackData *rd = (_RestCallbackData *)p_userdata; Vector3 contact_rel = p_point_B - p_point_A; @@ -409,10 +429,11 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, rd->best_normal = contact_rel / len; rd->best_object = rd->object; rd->best_shape = rd->shape; + 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) { - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_shape_xform.xform(shape->get_aabb()); @@ -432,12 +453,17 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shap } const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; if (p_exclude.has(col_obj->get_self())) { continue; } + int shape_idx = space->intersection_query_subindex_results[i]; + + if (col_obj->is_shape_set_as_disabled(shape_idx)) { + continue; + } + rcd.object = col_obj; rcd.shape = shape_idx; bool sc = CollisionSolver3DSW::solve_static(shape, p_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); @@ -457,8 +483,8 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shap r_info->rid = rcd.best_object->get_self(); if (rcd.best_object->get_type() == CollisionObject3DSW::TYPE_BODY) { const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_object); - r_info->linear_velocity = body->get_linear_velocity() + - (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos); + Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass()); + r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); } else { r_info->linear_velocity = Vector3(); @@ -468,15 +494,15 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shap } Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { - CollisionObject3DSW *obj = PhysicsServer3DSW::singleton->area_owner.getornull(p_object); + CollisionObject3DSW *obj = PhysicsServer3DSW::singletonsw->area_owner.getornull(p_object); if (!obj) { - obj = PhysicsServer3DSW::singleton->body_owner.getornull(p_object); + obj = PhysicsServer3DSW::singletonsw->body_owner.getornull(p_object); } ERR_FAIL_COND_V(!obj, Vector3()); ERR_FAIL_COND_V(obj->get_space() != space, Vector3()); - float min_distance = 1e20; + real_t min_distance = 1e20; Vector3 min_point; bool shapes_found = false; @@ -492,7 +518,7 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); point = shape_xform.xform(point); - float dist = point.distance_to(p_point); + real_t dist = point.distance_to(p_point); if (dist < min_distance) { min_distance = dist; min_point = point; @@ -523,6 +549,8 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) { keep = false; } else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_AREA) { keep = false; + } else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY) { + keep = false; } else if ((static_cast<Body3DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) { keep = false; } else if (static_cast<Body3DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { @@ -649,9 +677,9 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra Vector3 a = sr[k * 2 + 0]; Vector3 b = sr[k * 2 + 1]; - recover_motion += (b - a) * 0.4; + recover_motion += (b - a) / cbk.amount; - float depth = a.distance_to(b); + real_t depth = a.distance_to(b); if (depth > result.collision_depth) { result.collision_depth = depth; result.collision_point = b; @@ -663,10 +691,8 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra //result.collider_metadata = col_obj->get_shape_metadata(shape_idx); if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) { Body3DSW *body = (Body3DSW *)col_obj; - - Vector3 rel_vec = b - body->get_transform().get_origin(); - //result.collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rel_vec); // * mPos); + Vector3 rel_vec = b - (body->get_transform().origin + body->get_center_of_mass()); + result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); } } } @@ -739,8 +765,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb)); body_aabb = body_aabb.grow(p_margin); + real_t motion_length = p_motion.length(); + Vector3 motion_normal = p_motion / motion_length; + Transform body_transform = p_from; + bool recovered = false; + { //STEP 1, FREE BODY IF STUCK @@ -791,7 +822,17 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons for (int i = 0; i < cbk.amount; i++) { Vector3 a = sr[i * 2 + 0]; Vector3 b = sr[i * 2 + 1]; - recover_motion += (b - a) * 0.4; + + // Compute plane on b towards a. + Vector3 n = (a - b).normalized(); + real_t d = n.dot(b); + + // Compute depth on recovered motion. + real_t depth = n.dot(a + recover_motion) - d; + if (depth > 0.0) { + // Only recover if there is penetration. + recover_motion -= n * depth * 0.4; + } } if (recover_motion == Vector3()) { @@ -799,6 +840,8 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons break; } + recovered = true; + body_transform.origin += recover_motion; body_aabb.position += recover_motion; @@ -848,14 +891,14 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons //test initial overlap, does it collide if going all the way? Vector3 point_A, point_B; - Vector3 sep_axis = p_motion.normalized(); + Vector3 sep_axis = motion_normal; Transform 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; } - sep_axis = p_motion.normalized(); + sep_axis = motion_normal; if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { stuck = true; @@ -865,13 +908,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons //just do kinematic solving real_t low = 0; real_t hi = 1; - Vector3 mnormal = p_motion.normalized(); for (int k = 0; k < 8; k++) { //steps should be customizable.. real_t ofs = (low + hi) * 0.5; - Vector3 sep = mnormal; //important optimization for this to work fast enough + Vector3 sep = motion_normal; //important optimization for this to work fast enough mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs); @@ -912,16 +954,11 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } bool collided = false; - if (safe >= 1) { - //not collided - collided = false; - if (r_result) { - r_result->motion = p_motion; - r_result->remainder = Vector3(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + if (recovered || (safe < 1)) { + if (safe >= 1) { + best_shape = -1; //no best shape with cast, reset to -1 } - } else { //it collided, let's get the rest info in unsafe advance Transform ugt = body_transform; ugt.origin += p_motion * unsafe; @@ -930,25 +967,40 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons rcd.best_len = 0; rcd.best_object = nullptr; rcd.best_shape = 0; - rcd.min_allowed_depth = test_motion_min_contact_depth; - Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape); - Shape3DSW *body_shape = p_body->get_shape(best_shape); + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth); - body_aabb.position += p_motion * unsafe; + int from_shape = best_shape != -1 ? best_shape : 0; + int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); - int amount = _cull_aabb_for_body(p_body, body_aabb); + for (int j = from_shape; j < to_shape; j++) { + if (p_body->is_shape_set_as_disabled(j)) { + continue; + } - for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; + Transform body_shape_xform = ugt * p_body->get_shape_transform(j); + Shape3DSW *body_shape = p_body->get_shape(j); - 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); - if (!sc) { + if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { continue; } + + body_aabb.position += p_motion * unsafe; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int i = 0; i < amount; i++) { + const CollisionObject3DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + 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); + if (!sc) { + continue; + } + } } if (rcd.best_len != 0) { @@ -956,15 +1008,15 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons r_result->collider = rcd.best_object->get_self(); r_result->collider_id = rcd.best_object->get_instance_id(); r_result->collider_shape = rcd.best_shape; - r_result->collision_local_shape = best_shape; + r_result->collision_local_shape = rcd.best_local_shape; r_result->collision_normal = rcd.best_normal; r_result->collision_point = rcd.best_contact; //r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_object); - //Vector3 rel_vec = r_result->collision_point - body->get_transform().get_origin(); - // r_result->collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos); + + Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass()); + r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); r_result->motion = safe * p_motion; r_result->remainder = p_motion - safe * p_motion; @@ -972,17 +1024,15 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } collided = true; - } else { - if (r_result) { - r_result->motion = p_motion; - r_result->remainder = Vector3(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); - } - - collided = false; } } + if (!collided && r_result) { + r_result->motion = p_motion; + r_result->remainder = Vector3(); + r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + } + return collided; } @@ -1009,14 +1059,23 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll Area3DSW *area_b = static_cast<Area3DSW *>(B); Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A)); return area2_pair; + } else if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) { + // Area/Soft Body, not supported. } else { Body3DSW *body = static_cast<Body3DSW *>(B); AreaPair3DSW *area_pair = memnew(AreaPair3DSW(body, p_subindex_B, area, p_subindex_A)); return area_pair; } + } else if (type_A == CollisionObject3DSW::TYPE_BODY) { + if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) { + BodySoftBodyPair3DSW *soft_pair = memnew(BodySoftBodyPair3DSW((Body3DSW *)A, p_subindex_A, (SoftBody3DSW *)B)); + return soft_pair; + } else { + BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B)); + return b; + } } else { - BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B)); - return b; + // Soft Body/Soft Body, not supported. } return nullptr; @@ -1099,6 +1158,18 @@ const SelfList<Area3DSW>::List &Space3DSW::get_moved_area_list() const { return area_moved_list; } +const SelfList<SoftBody3DSW>::List &Space3DSW::get_active_soft_body_list() const { + return active_soft_body_list; +} + +void Space3DSW::soft_body_add_to_active_list(SelfList<SoftBody3DSW> *p_soft_body) { + active_soft_body_list.add(p_soft_body); +} + +void Space3DSW::soft_body_remove_from_active_list(SelfList<SoftBody3DSW> *p_soft_body) { + active_soft_body_list.remove(p_soft_body); +} + void Space3DSW::call_queries() { while (state_query_list.first()) { Body3DSW *b = state_query_list.first()->self(); @@ -1211,7 +1282,7 @@ Space3DSW::Space3DSW() { constraint_bias = 0.01; body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); - body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI)); + body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0)); body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); body_angular_velocity_damp_ratio = 10; diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index 4aba80c8f3..3a8f452e54 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -37,9 +37,10 @@ #include "body_pair_3d_sw.h" #include "broad_phase_3d_sw.h" #include "collision_object_3d_sw.h" -#include "core/hash_map.h" -#include "core/project_settings.h" +#include "core/config/project_settings.h" +#include "core/templates/hash_map.h" #include "core/typedefs.h" +#include "soft_body_3d_sw.h" class PhysicsDirectSpaceState3DSW : public PhysicsDirectSpaceState3D { GDCLASS(PhysicsDirectSpaceState3DSW, PhysicsDirectSpaceState3D); @@ -82,6 +83,7 @@ private: SelfList<Body3DSW>::List state_query_list; SelfList<Area3DSW>::List monitor_query_list; SelfList<Area3DSW>::List area_moved_list; + SelfList<SoftBody3DSW>::List active_soft_body_list; static void *_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self); static void _broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self); @@ -97,7 +99,6 @@ private: real_t test_motion_min_contact_depth; enum { - INTERSECTION_QUERY_MAX = 2048 }; @@ -146,6 +147,10 @@ public: void area_remove_from_moved_list(SelfList<Area3DSW> *p_area); const SelfList<Area3DSW>::List &get_moved_area_list() const; + const SelfList<SoftBody3DSW>::List &get_active_soft_body_list() const; + void soft_body_add_to_active_list(SelfList<SoftBody3DSW> *p_soft_body); + void soft_body_remove_from_active_list(SelfList<SoftBody3DSW> *p_soft_body); + BroadPhase3DSW *get_broadphase(); void add_object(CollisionObject3DSW *p_object); @@ -183,7 +188,7 @@ public: PhysicsDirectSpaceState3DSW *get_direct_state(); void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } - _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); } + _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } _FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) { if (contact_debug_count < contact_debug.size()) { contact_debug.write[contact_debug_count++] = p_contact; diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index 9a2a0073a1..9a73e11562 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ @@ -146,6 +146,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { 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(); + /* INTEGRATE FORCES */ uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); @@ -160,6 +162,15 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { active_count++; } + /* UPDATE SOFT BODY MOTION */ + + const SelfList<SoftBody3DSW> *sb = soft_body_list->first(); + while (sb) { + sb->self()->predict_motion(p_delta); + sb = sb->next(); + active_count++; + } + p_space->set_active_objects(active_count); { //profile @@ -214,6 +225,20 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here } + 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; + } + sb = sb->next(); + } + { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); @@ -272,6 +297,14 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { } } + /* UPDATE SOFT BODY CONSTRAINTS */ + + sb = soft_body_list->first(); + while (sb) { + sb->self()->solve_constraints(p_delta); + sb = sb->next(); + } + { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h index 9dbb61308f..55c48ec0eb 100644 --- a/servers/physics_3d/step_3d_sw.h +++ b/servers/physics_3d/step_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* 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 */ |