diff options
Diffstat (limited to 'servers/physics_3d')
22 files changed, 2784 insertions, 276 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index b6c5b3003c..bb4e0ed752 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -215,7 +215,9 @@ void Area3DSW::call_queries() { for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) { if (E->get().state == 0) { // Nothing happened - E = E->next(); + Map<BodyKey, BodyState>::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; continue; } @@ -250,7 +252,9 @@ void Area3DSW::call_queries() { for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) { if (E->get().state == 0) { // Nothing happened - E = E->next(); + Map<BodyKey, BodyState>::Element *next = E->next(); + monitored_areas.erase(E); + E = next; continue; } diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index 82356e77ef..64ba0cb09d 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -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 diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index 8e21003a5f..e87ff2364b 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -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); diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index 6012ff1522..36114c0c91 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -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. @@ -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 4d049eafdc..74dddfa6aa 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -28,32 +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) @@ -68,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); @@ -92,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/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index 3847b81381..85221b7746 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_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 f507cacdc3..9d5448dbfa 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -74,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); } } }; @@ -626,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 && @@ -662,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) { @@ -680,7 +685,7 @@ public: return true; } - static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { + 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(); @@ -845,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; @@ -1006,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; } } @@ -1148,7 +1161,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran } } - 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 @@ -1193,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); @@ -1467,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; } } @@ -1486,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; } } @@ -1508,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; } @@ -1519,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; } } @@ -1544,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; } } @@ -1569,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; @@ -1627,10 +1661,55 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p 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)) { + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { return; } @@ -1670,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; @@ -1682,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); @@ -1714,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; } @@ -1740,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; } } @@ -1805,7 +1901,7 @@ static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform & 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)) { + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { return; } @@ -1822,7 +1918,7 @@ static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Trans 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)) { + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { return; } @@ -1846,18 +1942,21 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr 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((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { + 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)) { - return; - } + if (!separator.test_axis(cyl_axis, !face_B->backface_collision)) { + return; } // Edges of B, cylinder lateral surface. @@ -1868,7 +1967,11 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr continue; } - if (!separator.test_axis(axis.normalized())) { + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis.normalized(), !face_B->backface_collision)) { return; } } @@ -1877,8 +1980,11 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr 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)) { + if (!separator.test_axis(axis, !face_B->backface_collision)) { return; } } @@ -1911,8 +2017,11 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr // 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())) { + if (!separator.test_axis(axis.normalized(), !face_B->backface_collision)) { return; } } @@ -2052,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; } @@ -2060,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; } } @@ -2074,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; } } @@ -2087,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; } } @@ -2102,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; } } @@ -2116,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_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index fd9ea00d92..f655c4626c 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -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" @@ -78,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); } } } @@ -113,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; @@ -215,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); @@ -233,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; diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h index 81d87e9773..34ac2c6d3f 100644 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -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/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index aa7c11eec5..1a8c7f704f 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -110,26 +110,60 @@ struct MinkowskiDiff { 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); + } } }; @@ -828,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); } @@ -857,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) @@ -896,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) @@ -963,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; @@ -972,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 be3ba4e664..a7e2e1719e 100644 --- a/servers/physics_3d/gjk_epa.h +++ b/servers/physics_3d/gjk_epa.h @@ -34,7 +34,7 @@ #include "collision_solver_3d_sw.h" #include "shape_3d_sw.h" -bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false); +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_3d_sw.h b/servers/physics_3d/joints_3d_sw.h index 1fe573c69e..225a71aca9 100644 --- a/servers/physics_3d/joints_3d_sw.h +++ b/servers/physics_3d/joints_3d_sw.h @@ -49,6 +49,15 @@ public: _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 735e9094d2..3d0063b0fa 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -611,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 { @@ -893,6 +902,266 @@ PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_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() { @@ -1278,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); @@ -1312,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); @@ -1447,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) { diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index afda161fa8..f92652bfad 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -63,6 +63,7 @@ class PhysicsServer3DSW : public PhysicsServer3D { 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); @@ -79,7 +80,7 @@ public: 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; @@ -252,68 +253,58 @@ public: /* SOFT BODY */ - virtual RID soft_body_create() 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 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) const 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) const 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) const 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_angular_stiffness(RID p_body, real_t p_stiffness) override {} - virtual real_t soft_body_get_angular_stiffness(RID p_body) const 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) const 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) const 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) const 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) const 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) const 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_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 { return Vector3(); } - - virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const override { return Vector3(); } - - 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 { return false; } + 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; /* JOINT API */ diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h index f60e1332d5..49ae60db92 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -273,7 +273,7 @@ public: FUNCRID(soft_body) - FUNC2(soft_body_update_rendering_server, RID, class SoftBodyRenderingServerHandler *) + FUNC2(soft_body_update_rendering_server, RID, class RenderingServerHandler *) FUNC2(soft_body_set_space, RID, RID) FUNC1RC(RID, soft_body_get_space, RID) @@ -294,7 +294,6 @@ public: FUNC2RC(Variant, soft_body_get_state, RID, BodyState); FUNC2(soft_body_set_transform, RID, const Transform &); - FUNC2RC(Vector3, soft_body_get_vertex_position, RID, int); FUNC2(soft_body_set_simulation_precision, RID, int); FUNC1RC(int, soft_body_get_simulation_precision, RID); @@ -305,18 +304,9 @@ public: FUNC2(soft_body_set_linear_stiffness, RID, real_t); FUNC1RC(real_t, soft_body_get_linear_stiffness, RID); - FUNC2(soft_body_set_angular_stiffness, RID, real_t); - FUNC1RC(real_t, soft_body_get_angular_stiffness, RID); - - FUNC2(soft_body_set_volume_stiffness, RID, real_t); - FUNC1RC(real_t, soft_body_get_volume_stiffness, RID); - FUNC2(soft_body_set_pressure_coefficient, RID, real_t); FUNC1RC(real_t, soft_body_get_pressure_coefficient, RID); - FUNC2(soft_body_set_pose_matching_coefficient, RID, real_t); - FUNC1RC(real_t, soft_body_get_pose_matching_coefficient, RID); - FUNC2(soft_body_set_damping_coefficient, RID, real_t); FUNC1RC(real_t, soft_body_get_damping_coefficient, RID); @@ -325,9 +315,10 @@ public: 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); - FUNC2RC(Vector3, soft_body_get_point_offset, RID, int); FUNC1(soft_body_remove_all_pinned_points, RID); FUNC3(soft_body_pin_point, RID, int, bool); diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index 5bac4f19b9..bf0946a0e2 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -491,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)); @@ -503,36 +503,36 @@ 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, 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; @@ -551,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); @@ -563,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); @@ -575,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); @@ -596,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); @@ -627,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) { @@ -807,7 +807,7 @@ Vector3 CylinderShape3DSW::get_moment_of_inertia(real_t p_mass) const { return Vector3( (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); } void CylinderShape3DSW::_setup(real_t p_height, real_t p_radius) { @@ -1064,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) { @@ -1134,7 +1134,7 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ 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++) { @@ -1187,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; + } } } @@ -1285,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); @@ -1329,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); @@ -1401,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; @@ -1422,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 { @@ -1532,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()); @@ -1587,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() { @@ -1651,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 cafe978abb..988e76c699 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -334,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; @@ -424,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; } 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 dd5754b9ac..2df824b320 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -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; } @@ -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; } @@ -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(); @@ -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); } } } @@ -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; } @@ -394,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; @@ -435,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); @@ -460,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(); @@ -526,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())) { @@ -666,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); } } } @@ -991,9 +1014,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons //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; @@ -1036,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; @@ -1126,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(); diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index eed3d86a72..3a8f452e54 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -40,6 +40,7 @@ #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); @@ -145,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); diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index d9370de6a3..9a73e11562 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -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); |