diff --git a/doc/classes/SpringBoneCollision3D.xml b/doc/classes/SpringBoneCollision3D.xml index 897efa62df28..3d7cedf251cf 100644 --- a/doc/classes/SpringBoneCollision3D.xml +++ b/doc/classes/SpringBoneCollision3D.xml @@ -34,4 +34,15 @@ The offset of the rotation from [Skeleton3D]'s [member bone] pose rotation. + + + Collision is only with spheres around the joints. + + + Collision acts to trap the spheres around the joints within the collision. + + + Collision is against the joint spheres and the tapered cones between them. + + diff --git a/doc/classes/SpringBoneCollisionCapsule3D.xml b/doc/classes/SpringBoneCollisionCapsule3D.xml index b45049e58cd5..b1bc6b4e7188 100644 --- a/doc/classes/SpringBoneCollisionCapsule3D.xml +++ b/doc/classes/SpringBoneCollisionCapsule3D.xml @@ -9,6 +9,9 @@ + + Sets whether the collision capcule collides or traps the joint spheres, or interacts with the tapered cones between them. + The capsule's full height, including the hemispheres. [b]Note:[/b] The [member height] of a capsule must be at least twice its [member radius]. Otherwise, the capsule becomes a sphere. If the [member height] is less than twice the [member radius], the properties adjust to a valid value. diff --git a/doc/classes/SpringBoneCollisionSphere3D.xml b/doc/classes/SpringBoneCollisionSphere3D.xml index 52f5e3a53601..28677d40f12f 100644 --- a/doc/classes/SpringBoneCollisionSphere3D.xml +++ b/doc/classes/SpringBoneCollisionSphere3D.xml @@ -9,8 +9,8 @@ - - If [code]true[/code], the collision acts to trap the joint within the collision. + + Sets whether the collision sphere collides or traps the joint spheres, or interacts with the tapered cones between them. The sphere's radius. diff --git a/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp b/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp index e1fd5446df8c..b8becdd2d084 100644 --- a/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp +++ b/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp @@ -111,8 +111,10 @@ void SpringBoneSimulator3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { p_gizmo->add_mesh(mesh, Ref(), skel_tr, skeleton->register_skin(skeleton->create_skin_from_rest_transforms())); } +bool draw_as_chain = true; Ref SpringBoneSimulator3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_skeleton, SpringBoneSimulator3D *p_simulator, bool p_is_selected) { Color bone_color = EDITOR_GET("editors/3d_gizmos/gizmo_colors/spring_bone_joint"); + Color cone_color = Color(1, 0, 0, 1); Ref surface_tool; surface_tool.instantiate(); @@ -147,7 +149,13 @@ Ref SpringBoneSimulator3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_s Transform3D parent_global_pose = p_skeleton->get_bone_global_rest(prev_bone); Vector3 bone_vector = p_simulator->get_bone_vector(i, j - 1); Vector3 center = parent_global_pose.translated_local(bone_vector).origin; - draw_line(surface_tool, parent_global_pose.origin, center, bone_color); + + if (draw_as_chain) { + draw_sphere_tangent_cone(surface_tool, parent_global_pose.origin, p_simulator->get_joint_radius(i, MAX(0, j - 2)), center, p_simulator->get_joint_radius(i, j - 1), cone_color); + } else { + draw_line(surface_tool, parent_global_pose.origin, center, bone_color); + } + draw_sphere(surface_tool, global_pose.basis, center, p_simulator->get_joint_radius(i, j - 1), bone_color); // Draw rotation axis vector if not ROTATION_AXIS_ALL. @@ -172,7 +180,11 @@ Ref SpringBoneSimulator3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_s surface_tool->set_bones(Vector(bones)); surface_tool->set_weights(Vector(weights)); Vector3 center = global_pose.translated_local(bone_vector).origin; - draw_line(surface_tool, global_pose.origin, center, bone_color); + if (draw_as_chain) { + draw_sphere_tangent_cone(surface_tool, global_pose.origin, p_simulator->get_joint_radius(i, j - 1), center, p_simulator->get_joint_radius(i, j), cone_color); + } else { + draw_line(surface_tool, global_pose.origin, center, bone_color); + } draw_sphere(surface_tool, global_pose.basis, center, p_simulator->get_joint_radius(i, j), bone_color); } else { bones[0] = current_bone; @@ -222,6 +234,64 @@ void SpringBoneSimulator3DGizmoPlugin::draw_sphere(Ref &p_surface_t } } +void SpringBoneSimulator3DGizmoPlugin::draw_sphere_tangent_cone(Ref &p_surface_tool, const Vector3 &p_prev_center, float p_prev_radius, const Vector3 &p_center, float p_radius, const Color &p_color) { + static constexpr int STEP = 16; + static constexpr float SPPI = Math::TAU / (float)STEP; + + Vector3 axis_vec = p_center - p_prev_center; + float axis_length = axis_vec.length(); + if (Math::is_zero_approx(axis_length)) { + return; + } + Vector3 cone_axis = axis_vec / axis_length; + float cone_gradient = (p_prev_radius - p_radius) / axis_length; + if ((cone_gradient <= -1.0) || (cone_gradient >= 1.0)) { + return; + } + + Vector3 prev_cone_center = p_prev_center + cone_axis * (p_prev_radius * cone_gradient); + Vector3 cone_center = p_center + cone_axis * (p_radius * cone_gradient); + + float cone_rad_fac = sqrt(1 - cone_gradient * cone_gradient); + Vector3 cone_side_axis = cone_axis.cross(fabs(axis_vec.x) < 0.5 ? Vector3(1, 0, 0) : Vector3(0, 0, 1)).normalized(); + Vector3 prev_cone_side_vec = cone_side_axis * (p_prev_radius * cone_rad_fac); + Vector3 cone_side_vec = cone_side_axis * (p_radius * cone_rad_fac); + + for (int i = 1; i <= STEP; i++) { + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(prev_cone_center + prev_cone_side_vec.rotated(cone_axis, SPPI * ((i - 1) % STEP))); + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(prev_cone_center + prev_cone_side_vec.rotated(cone_axis, SPPI * (i % STEP))); + } + + for (int i = 1; i <= STEP; i++) { + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(cone_center + cone_side_vec.rotated(cone_axis, SPPI * ((i - 1) % STEP))); + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(cone_center + cone_side_vec.rotated(cone_axis, SPPI * (i % STEP))); + } + + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(prev_cone_center + prev_cone_side_vec); + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(cone_center + cone_side_vec); + + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(prev_cone_center + prev_cone_side_vec.rotated(cone_axis, Math::PI * 0.5)); + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(cone_center + cone_side_vec.rotated(cone_axis, Math::PI * 0.5)); + + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(prev_cone_center + prev_cone_side_vec.rotated(cone_axis, Math::PI)); + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(cone_center + cone_side_vec.rotated(cone_axis, Math::PI)); + + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(prev_cone_center + prev_cone_side_vec.rotated(cone_axis, Math::PI * 1.5)); + p_surface_tool->set_color(p_color); + p_surface_tool->add_vertex(cone_center + cone_side_vec.rotated(cone_axis, Math::PI * 1.5)); +} + void SpringBoneSimulator3DGizmoPlugin::draw_line(Ref &p_surface_tool, const Vector3 &p_begin_pos, const Vector3 &p_end_pos, const Color &p_color) { p_surface_tool->set_color(p_color); p_surface_tool->add_vertex(p_begin_pos); @@ -309,13 +379,13 @@ Ref SpringBoneCollision3DGizmoPlugin::get_collision_mesh(SpringBoneCo SpringBoneCollisionSphere3D *sphere = Object::cast_to(p_collision); if (sphere) { - draw_sphere(surface_tool, sphere->get_radius(), sphere->is_inside() ? inside_collision_color : collision_color); + draw_sphere(surface_tool, sphere->get_radius(), sphere->get_collide_mode() == SpringBoneCollision3D::COLLIDE_MODE_INSIDE ? inside_collision_color : collision_color); return surface_tool->commit(); } SpringBoneCollisionCapsule3D *capsule = Object::cast_to(p_collision); if (capsule) { - draw_capsule(surface_tool, capsule->get_radius(), capsule->get_height(), capsule->is_inside() ? inside_collision_color : collision_color); + draw_capsule(surface_tool, capsule->get_radius(), capsule->get_height(), capsule->get_collide_mode() == SpringBoneCollision3D::COLLIDE_MODE_INSIDE ? inside_collision_color : collision_color); return surface_tool->commit(); } diff --git a/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.h b/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.h index 69c519092e94..fc6946050083 100644 --- a/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.h +++ b/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.h @@ -48,6 +48,7 @@ class SpringBoneSimulator3DGizmoPlugin : public EditorNode3DGizmoPlugin { public: static Ref get_joints_mesh(Skeleton3D *p_skeleton, SpringBoneSimulator3D *p_simulator, bool p_is_selected); static void draw_sphere(Ref &p_surface_tool, const Basis &p_basis, const Vector3 &p_center, float p_radius, const Color &p_color); + static void draw_sphere_tangent_cone(Ref &p_surface_tool, const Vector3 &p_prev_center, float p__prev_radius, const Vector3 &p_center, float p_radius, const Color &p_color); static void draw_line(Ref &p_surface_tool, const Vector3 &p_begin_pos, const Vector3 &p_end_pos, const Color &p_color); bool has_gizmo(Node3D *p_spatial) override; diff --git a/scene/3d/spring_bone_collision_3d.cpp b/scene/3d/spring_bone_collision_3d.cpp index f602d877d0aa..be78a2474cbe 100644 --- a/scene/3d/spring_bone_collision_3d.cpp +++ b/scene/3d/spring_bone_collision_3d.cpp @@ -99,7 +99,7 @@ void SpringBoneCollision3D::set_bone(int p_bone) { Skeleton3D *sk = get_skeleton(); if (sk) { - if (bone <= -1 || bone >= sk->get_bone_count()) { + if (bone < -1 || bone >= sk->get_bone_count()) { WARN_PRINT("Bone index '" + itos(p_bone) + "' is out of range! Cannot connect BoneAttachment to node!"); bone = -1; } else { @@ -187,6 +187,10 @@ void SpringBoneCollision3D::_bind_methods() { ADD_GROUP("Offset", ""); ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "position_offset"), "set_position_offset", "get_position_offset"); ADD_PROPERTY(PropertyInfo(Variant::QUATERNION, "rotation_offset"), "set_rotation_offset", "get_rotation_offset"); + + BIND_ENUM_CONSTANT(COLLIDE_MODE_JOINT); + BIND_ENUM_CONSTANT(COLLIDE_MODE_INSIDE); + BIND_ENUM_CONSTANT(COLLIDE_MODE_CHAIN); } void SpringBoneCollision3D::_notification(int p_what) { @@ -198,11 +202,113 @@ void SpringBoneCollision3D::_notification(int p_what) { } } -Vector3 SpringBoneCollision3D::collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current) const { - return _collide(p_center, p_bone_radius, p_bone_length, p_current); +int SpringBoneCollision3D::Dsegmentindexbeingcalculated = 0; + +// static +Vector3 SpringBoneCollision3D::_collide_sphere(const Vector3 &p_origin, float p_radius, bool p_inside, float p_bone_radius, const Vector3 &p_current) { + Vector3 diff = p_current - p_origin; + float diff_length = diff.length(); + float r = p_inside ? p_radius - p_bone_radius : p_bone_radius + p_radius; + float distance = p_inside ? r - diff_length : diff_length - r; + if (distance > 0) { + return p_current; + } + return p_origin + diff.normalized() * r; +} + +// static +Vector3 SpringBoneCollision3D::_collide_sphere_taper(const Vector3 &p_origin, float p_radius, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) { + // (p_origin, p_radius) defines the external collider + // The bone capsule is from (p_current_origin, p_bone_origin_radius) to (p_current, p_bone_radius) + // where p_current is to be displaced + + float taper_fore = (p_bone_origin_radius - p_bone_radius) / p_bone_length; + + // send the short taper case into old implementation + if (Math::abs(taper_fore) >= 1.0) { + return _collide_sphere(p_origin, p_radius, false, p_bone_radius, p_current); + } + + Vector3 diff = p_current - p_origin; + Vector3 bone_axis = p_current - p_current_origin; + DEV_ASSERT(Math::is_equal_approx(bone_axis.length(), p_bone_length)); + float taper_side = Math::sqrt(1.0 - taper_fore * taper_fore); + float bone_axis_sq = bone_axis.dot(bone_axis); + float lam = 1.0 - bone_axis.dot(diff) / bone_axis_sq; // calculated from the tail end + Vector3 vecside = p_origin - (p_current_origin + bone_axis * lam); + // printf(" zz=%f ", vecside.dot(bone_axis)); // should be zero + float radial_distance = vecside.length(); + if (radial_distance > MAX(p_bone_origin_radius, p_bone_radius) + p_radius) { + return p_current; + } + float bone_axis_length = Math::sqrt(bone_axis_sq); + + // limit contact with the cone close to the root where it gets twitchy + float gapdistance = p_bone_radius * 0.5 + p_bone_origin_radius * 0.5 + p_radius * Math::sqrt(0.5); + float lamconemin = gapdistance / bone_axis_length * 0.5; + + // case of collide sphere being very large. + if (lamconemin > 1.0) { // apply this case before the beyond origin end to avoid twitchiness + return _collide_sphere(p_origin, p_radius, false, p_bone_radius, p_current); + } + + float lamd = radial_distance * taper_fore / taper_side / bone_axis_length; + float lamcone = lam - lamd; + if (lamcone <= 0.0) { // beyond origin end + return p_current; + } + if (lamcone >= 1.0) { // beyond tail end + return _collide_sphere(p_origin, p_radius, false, p_bone_radius, p_current); + } + + // prove numerically this is the closest approach to the cone + /*float lam1 = lamcone; + float m1 = (p_current_origin + bone_axis * lam1 - p_origin).length() - (p_bone_origin_radius + (p_bone_radius - p_bone_origin_radius) * lam1); + float lam0 = lamcone - 0.01; + float m0 = (p_current_origin + bone_axis * lam0 - p_origin).length() - (p_bone_origin_radius + (p_bone_radius - p_bone_origin_radius) * lam0); + float lam2 = lamcone + 0.01; + float m2 = (p_current_origin + bone_axis * lam2 - p_origin).length() - (p_bone_origin_radius + (p_bone_radius - p_bone_origin_radius) * lam2); + printf(" check %f>0 ", std::min(m0, m2) - m1); + */ + + if (lamcone < lamconemin) { + lamcone = lamconemin; + } + + // Check collision with this cone + Vector3 coneaxispoint = p_current_origin + bone_axis * lamcone; + Vector3 conepointdiff = coneaxispoint - p_origin; + float coneaxisradius = p_bone_origin_radius + (p_bone_radius - p_bone_origin_radius) * lamcone; + + float r = coneaxisradius + p_radius; + float conepointdifflength = conepointdiff.length(); + float distance = conepointdifflength - r; + if (distance > 0.0) { + return p_current; + } + //printf(" hh=%f; ", distance); + + // We could model a rotation of the bone_axis about p_current_origin to move the (coneaxispoint, coneaxisradius) sphere + // away from its intersection with (p_origin, p_radius) [not quite accurate since as it rotates the lamcone position + // of the virtual sphere inside the cone changes]. + // But instead we will just project it as a simple lever and rely on limit_length() and the iteration to settle it into the correct place. + + // position virtual sphere of contact in the cone is pushed to + Vector3 p_coneaxispointnew = p_origin + conepointdiff.normalized() * r; + + // projection out to the end point of the cone as though it were a lever + // (this isn't necessary since the change it makes is masked by the limit_length() function) + // also limit the size of the multiplier to avoid extreme movement when near the joint + Vector3 p_current_new = p_current_origin + (p_coneaxispointnew - p_current_origin) / MAX(0.1f, lamcone); + + return p_current_new; +} + +Vector3 SpringBoneCollision3D::collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) const { + return _collide(p_center, p_bone_radius, p_bone_length, p_current_origin, p_bone_origin_radius, p_current); } -Vector3 SpringBoneCollision3D::_collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current) const { +Vector3 SpringBoneCollision3D::_collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) const { return Vector3(0, 0, 0); } diff --git a/scene/3d/spring_bone_collision_3d.h b/scene/3d/spring_bone_collision_3d.h index c09189c9b76c..7550a4657c3c 100644 --- a/scene/3d/spring_bone_collision_3d.h +++ b/scene/3d/spring_bone_collision_3d.h @@ -50,9 +50,18 @@ class SpringBoneCollision3D : public Node3D { void _notification(int p_what); static void _bind_methods(); - virtual Vector3 _collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current) const; + static Vector3 _collide_sphere(const Vector3 &p_origin, float p_radius, bool p_inside, float p_bone_radius, const Vector3 &p_current); + static Vector3 _collide_sphere_taper(const Vector3 &p_origin, float p_radius, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current); + + virtual Vector3 _collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) const; public: + enum CollideMode { + COLLIDE_MODE_JOINT, + COLLIDE_MODE_INSIDE, + COLLIDE_MODE_CHAIN, + }; + Skeleton3D *get_skeleton() const; void set_bone_name(const String &p_name); @@ -68,7 +77,10 @@ class SpringBoneCollision3D : public Node3D { void sync_pose(); Transform3D get_transform_from_skeleton(const Transform3D &p_center) const; - Vector3 collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current) const; + Vector3 collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) const; + static int Dsegmentindexbeingcalculated; SpringBoneCollision3D(); }; + +VARIANT_ENUM_CAST(SpringBoneCollision3D::CollideMode); diff --git a/scene/3d/spring_bone_collision_capsule_3d.cpp b/scene/3d/spring_bone_collision_capsule_3d.cpp index fec5b4efaa1f..69ad8d3ac24c 100644 --- a/scene/3d/spring_bone_collision_capsule_3d.cpp +++ b/scene/3d/spring_bone_collision_capsule_3d.cpp @@ -73,17 +73,17 @@ real_t SpringBoneCollisionCapsule3D::get_mid_height() const { return height - radius * 2.0f; } -void SpringBoneCollisionCapsule3D::set_inside(bool p_enabled) { - inside = p_enabled; +SpringBoneCollision3D::CollideMode SpringBoneCollisionCapsule3D::get_collide_mode() const { + return collide_mode; +} + +void SpringBoneCollisionCapsule3D::set_collide_mode(CollideMode p_collide_mode) { + collide_mode = p_collide_mode; #ifdef TOOLS_ENABLED update_gizmos(); #endif // TOOLS_ENABLED } -bool SpringBoneCollisionCapsule3D::is_inside() const { - return inside; -} - Pair SpringBoneCollisionCapsule3D::get_head_and_tail(const Transform3D &p_center) const { Transform3D tr = get_transform_from_skeleton(p_center); return Pair(tr.origin + tr.basis.xform(Vector3::UP * (height * 0.5 - radius)), tr.origin + tr.basis.xform(Vector3::DOWN * (height * 0.5 - radius))); @@ -96,31 +96,277 @@ void SpringBoneCollisionCapsule3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_height"), &SpringBoneCollisionCapsule3D::get_height); ClassDB::bind_method(D_METHOD("set_mid_height", "mid_height"), &SpringBoneCollisionCapsule3D::set_mid_height); ClassDB::bind_method(D_METHOD("get_mid_height"), &SpringBoneCollisionCapsule3D::get_mid_height); - ClassDB::bind_method(D_METHOD("set_inside", "enabled"), &SpringBoneCollisionCapsule3D::set_inside); - ClassDB::bind_method(D_METHOD("is_inside"), &SpringBoneCollisionCapsule3D::is_inside); + ClassDB::bind_method(D_METHOD("set_collide_mode", "collide_mode"), &SpringBoneCollisionCapsule3D::set_collide_mode); + ClassDB::bind_method(D_METHOD("get_collide_mode"), &SpringBoneCollisionCapsule3D::get_collide_mode); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"), "set_radius", "get_radius"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "height", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"), "set_height", "get_height"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "mid_height", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m", PROPERTY_USAGE_NONE), "set_mid_height", "get_mid_height"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "inside"), "set_inside", "is_inside"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "collide_mode", PROPERTY_HINT_ENUM, "Joint,Inside,Chain"), "set_collide_mode", "get_collide_mode"); +} + +// The SpringBoneCollisionCapsule3D::_collide() function is to find the deepest point of +// collision between two capsule shaped components, a conical springbone and a cylindrical capsule collider. +// The first step is to select the sphere within the capsule collider that has the deepest ingress into the conical springbone. +// Then we call _collide_sphere_taper() to collide this shape, which is a subset of the collider, into the conical springbone. + +// In other words, we need to find a mu between 0 and 1 that minimizes verify_distance_within_taper(lerp(head,tail,mu)). + +// Very local calculation verification feature +#define VERIFY_SPRINGBONECAPSULE_CALCULATIONS 1 +#if VERIFY_SPRINGBONECAPSULE_CALCULATIONS +#define SB_DEV_ASSERT(m_cond) \ + if ((!(m_cond))) { \ + printf("SB_DEV_ASSERT %s %s %d %s\n", FUNCTION_STR, __FILE__, __LINE__, _STR(m_cond)); \ + } else \ + ((void)0) +#else +#define SB_DEV_ASSERT(m_cond) +#endif + +// function to verify calculations +real_t verify_distance_within_taper(const Vector3 &p_origin, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) { + // (p_origin) defines the external point we are measuring the distance to (on the axis of the collider) + // The bone capsule is from (p_current_origin, p_bone_origin_radius) to (p_current, p_bone_radius) + real_t taper_fore = (p_bone_origin_radius - p_bone_radius) / p_bone_length; + Vector3 diff = p_current - p_origin; + Vector3 bone_axis = p_current - p_current_origin; // should be length p_bone_radius due to calls to limit_length() + DEV_ASSERT(Math::is_equal_approx(bone_axis.length(), p_bone_length)); + real_t taper_side = Math::sqrt(1.0 - taper_fore * taper_fore); + real_t lam = 1.0 - bone_axis.dot(diff) / (p_bone_length * p_bone_length); + Vector3 vecside = p_origin - (p_current_origin + bone_axis * lam); + real_t radial_distance = vecside.length(); + real_t bone_axis_length = p_bone_length; + + real_t lamd = radial_distance * taper_fore / taper_side / bone_axis_length; + real_t lamcone = lam - lamd; + real_t clamcone = MIN(MAX(lamcone, 0.0), 1.0); + Vector3 closest_cone_axis_point = p_current_origin + bone_axis * clamcone; + real_t cone_sphere_rad = p_bone_origin_radius + clamcone * (p_bone_radius - p_bone_origin_radius); + return (p_origin - closest_cone_axis_point).length() - cone_sphere_rad; +} + +static Vector3 _closest_capsule_sphere(const Vector3 &head, const Vector3 &tail, const Vector3 &bone_sphere_center) { + Vector3 p = tail - head; + Vector3 q = bone_sphere_center - head; + real_t dot = p.dot(q); + if (dot <= 0) { + return head; + } + real_t pls = p.length_squared(); + if ((pls <= dot) || Math::is_zero_approx(pls)) { + return tail; + } + return head + p * (dot / pls); +} + +static real_t _closest_capsule_sphere_to_taper(const Vector3 &head, const Vector3 &tail, float radius, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) { + // The collision capsule is (head, radius) to (tail, radius) parametrized by mu + // The bone capsule is from (p_current_origin, p_bone_origin_radius) to (p_current, p_bone_radius) parametrized by lam + + Vector3 bone_axis = p_current - p_current_origin; + DEV_ASSERT(Math::is_equal_approx(bone_axis.length(), p_bone_length)); // enforced by limit_length() + Vector3 p = tail - head; + + // The bone_axis and p (the capsule axis) are skew lines, + // so the cross-product vector is the shortest distance between them. + Vector3 perp = bone_axis.cross(p); + real_t perp_sq = perp.dot(perp); + real_t perp_len = sqrt(perp_sq); + if (Math::is_zero_approx(perp_len)) { // This case also removes zero length bones and capsules. + return 0.5; // Axes are parallel, so should actually pick point in overlap, but this is a very rare case. + } + real_t perp_bone = perp.dot(p_current_origin); + real_t perp_capsule = perp.dot(head); + real_t perp_dist = (perp_capsule - perp_bone) / perp_len; + if (Math::abs(perp_dist) > radius + MAX(p_bone_origin_radius, p_bone_radius) + CMP_EPSILON) { + return -1.0; // Geometry too distant for to interactions. + } + + // Calculate the points of closest approach between these two skew lines + // by solving: p_current_origin + bone_axis * lam + perp = head + p * mu + + Vector3 hh = p_current_origin - head; + // dot bone_axis: hh.dot(bone_axis) + bone_axis.dot(bone_axis) * lam = p.dot(bone_axis) * mu + // dot p: hh.dot(p) + bone_axis.dot(p) * lam = p.dot(p) * mu + real_t badp = bone_axis.dot(p); + real_t badba = bone_axis.dot(bone_axis); + real_t pdp = p.dot(p); + real_t hhdba = hh.dot(bone_axis); + real_t hhdp = hh.dot(p); + // hhdba = -badba * lam + badp * mu + // hhdp = -badp * lam + pdp * mu + // ( -badba badp ) ( lam ) ( hhdba ) + // ( -badp pdp ) * ( mu ) = ( hhdp ) + + // If T is the angle between ba and p, then the determinant of this matrix is: + // -badba * pdp + badp * badp = -ba^2*p^2 + ba^2*p^2*cosT^2 = -ba^2*p^2*sinT^2 = -(ba x p)^2 + real_t det = -perp_sq; + // ( pdp -badp ) ( hhdba ) ( lam ) + // ( badp -badba ) * ( hhdp ) = ( mu ) * det + real_t lam = (pdp * hhdba - badp * hhdp) / det; + real_t mu = (badp * hhdba - badba * hhdp) / det; + +#ifdef VERIFY_SPRINGBONECAPSULE_CALCULATIONS + SB_DEV_ASSERT(Math::is_equal_approx(det, -badba * pdp + badp * badp)); + real_t Dhhdba = -badba * lam + badp * mu; + real_t Dhhdp = -badp * lam + pdp * mu; + Vector3 Dperpvec = perp * (perp_dist / perp_len); + SB_DEV_ASSERT(Math::is_equal_approx(Dhhdba, hhdba)); + SB_DEV_ASSERT(Math::is_equal_approx(Dhhdp, hhdp)); + Vector3 Dlammuvec = (p_current_origin + bone_axis * lam + Dperpvec) - (head + p * mu); + SB_DEV_ASSERT(Math::is_zero_approx(Dlammuvec.length())); +#endif + + // Handle cylindrical springbone case. + // The bone capsule (cylinder) is from (p_current_origin, p_bone_origin_radius) to (p_current, p_bone_radius) + if (p_bone_radius == p_bone_origin_radius) { + // clamp the collision sphere center point to the bone and recalculate it for the capsule + if ((lam < 0.0) || (lam > 1.0)) { + Vector3 bone_sphere_end = (lam < 0.0 ? p_current_origin : p_current); + mu = (bone_sphere_end - head).dot(p) / pdp; + lam = (lam < 0.0 ? 0.0 : 1.0); + } + + mu = (mu < 0.0 ? 0.0 : (mu > 1.0 ? 1.0 : mu)); // clamp(0,1) + +#ifdef VERIFY_SPRINGBONECAPSULE_CALCULATIONS + Vector3 Dcapsule_sphere_center = head * (1.0 - mu) + tail * mu; + Vector3 Dbone_sphere_center = p_current_origin * (1.0 - lam) + p_current * lam; + real_t Dvpdist = (Dcapsule_sphere_center - Dbone_sphere_center).length(); + real_t Dvdist = verify_distance_within_taper(Dcapsule_sphere_center, p_bone_radius, p_bone_length, p_current_origin, p_bone_origin_radius, p_current); + if (fabs((Dvpdist - p_bone_radius) - Dvdist) > 0.01) { + printf(" %d disag %.3f %.3f mu %.3f lam %.3f\n", SpringBoneCollision3D::Dsegmentindexbeingcalculated, Dvpdist, Dvdist, mu, lam); + } +#endif + + return mu; + } + + // Now consider the plane C perpendicular to perp containing the head-tail vector of the capsule + // Set its origin to be at (head + p * mu) with y-vector along the normalized_bone_axis + // and x-vector perpendicular to bone_axis and perp. + // We can apply the radius of the collusion capsule to the radii of the cone and replace the collision capsule with a line. + // The intersection of this plane with the bone cone will be a hyperbola (conic section). + + // If the capsule intrudes by a distance of intrude_radius into the bone cone, then the cone + // from (p_current_origin, p_bone_origin_radiusP + radius - intrude_radius) to + // to (p_current, p_bone_radiusP + radius - intrude_radius) + // will define a hyperbola that is tangential to the capsule axis. + + // Therefore we need to calculate intrude_radius which makes the hyperbola tangential. + + // But first we need to calculate p_bone_origin_radiusP and p_bone_radiusP which are the + // radii of the cone in the plane across its axis -- whereas the given definition is + // in terms of a cone tangential to the spheres around the endpoints of the axis. + + // If cone_side_perp is the unit vector in the cone axis (x component) and perpendicular to the cone axis (y component) + real_t cone_side_perp_x = (p_bone_origin_radius - p_bone_radius) / p_bone_length; + real_t cone_side_perp_y = sqrt(1 - cone_side_perp_x * cone_side_perp_x); + real_t p_bone_origin_radiusP = p_bone_origin_radius / cone_side_perp_y; + real_t p_bone_radiusP = p_bone_radius / cone_side_perp_y; + + //Vector3 C_plane_origin = head + p * mu; + Vector3 C_plane_z = perp * (1 / perp_len); + Vector3 C_plane_y = bone_axis * (1 / p_bone_length); + Vector3 C_plane_x = C_plane_z.cross(C_plane_y); + SB_DEV_ASSERT(Math::is_equal_approx(C_plane_z.length(), 1)); + SB_DEV_ASSERT(Math::is_equal_approx(C_plane_y.length(), 1)); + SB_DEV_ASSERT(Math::is_equal_approx(C_plane_x.length(), 1)); + + real_t p_length = sqrt(pdp); + Vector3 C_capsule_vec = p * (1 / p_length); + SB_DEV_ASSERT(Math::is_equal_approx(C_capsule_vec.length(), 1)); + Vector3 C_capsule_vec_inplane = Vector3(C_capsule_vec.dot(C_plane_x), C_capsule_vec.dot(C_plane_y), C_capsule_vec.dot(C_plane_z)); + SB_DEV_ASSERT(Math::is_zero_approx(C_capsule_vec_inplane.z)); + real_t capsule_vec_slope = C_capsule_vec_inplane.y / C_capsule_vec_inplane.x; + + // The apex of the cone relative to plane C frame is (0, ya, perp_dist) + // where ya will vary to make different intersections with the C plane as a hyperbola + // to find the value where it is tangential to C_capsule_vec_inplane. + + real_t cone_slope = (p_bone_radiusP - p_bone_origin_radiusP) / p_bone_length; + // perp_dist^2 + x^2 = ((y - ya)*cone_slope)^2, + // Diff by x: 2x = 2(y - ya)cone_slope * dy/dx + + // Simultaneous tangential equations to solve are: + // y = x * capsule_vec_slope + // perp_dist^2 + x^2 = ((y - ya)*cone_slope)^2 + // dy/dx = capsule_vec_slope + // x = (y - ya) * dy/dx * cone_slope^2 + + // x = (y - ya)*cone_slope * capsule_vec_slope * cone_slope + // x/(capsule_vec_slope * cone_slope) = (y - ya)*cone_slope + // perp_dist^2 = x^2*((1/(capsule_vec_slope * cone_slope))^2 - 1) + // x^2 = perp_dist^2 / ((1/(capsule_vec_slope * cone_slope))^2 - 1) + + // Protect division by zero which occurs with alignment of the capsule beyond the asymtote + real_t cc = capsule_vec_slope * cone_slope; + real_t perp_dist_sq = perp_dist * perp_dist; + real_t xsq_num = perp_dist_sq * cc * cc; + real_t xsq_den = 1 - cc * cc; + if (xsq_den <= xsq_num * 0.0000001 + CMP_EPSILON) { + // this needs to pick the best endpoint of the capsule that will hit the cone + return -1.0; + } + real_t xsq = xsq_num / xsq_den; + + real_t x = sqrt(xsq); + if ((capsule_vec_slope > 0) == (p_bone_origin_radius > p_bone_radius)) { + x = -x; + } + real_t y = x * capsule_vec_slope; + //real_t ya = y - x / (capsule_vec_slope * cone_slope * cone_slope); + + real_t emu = y / (C_capsule_vec_inplane.y * p_length); + real_t mu0 = mu + emu; + + // If the sphere in the bone cone is off and endpoint, then pick a mu in the collision capsule + // that is closest to that end point + real_t cone_axis_rad = sqrt(perp_dist_sq + xsq); + real_t lam_cone_sphere = lam + (y + cone_axis_rad * cone_slope) / p_bone_length; + if ((lam_cone_sphere < 0) || (lam_cone_sphere > 1)) { + mu0 = p.dot((lam_cone_sphere < 0 ? p_current_origin : p_current) - head) / pdp; + } + return (mu0 < 0.0 ? 0.0 : (mu0 > 1.0 ? 1.0 : mu0)); // clamp(0,1) } -Vector3 SpringBoneCollisionCapsule3D::_collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current) const { +Vector3 SpringBoneCollisionCapsule3D::_collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) const { + // The tapered bone capsule is from (p_current_origin, p_bone_origin_radius) to (p_current, p_bone_radius). Pair head_tail = get_head_and_tail(p_center); Vector3 head = head_tail.first; Vector3 tail = head_tail.second; - Vector3 p = tail - head; - Vector3 q = p_current - head; - float dot = p.dot(q); - if (dot <= 0) { - return SpringBoneCollisionSphere3D::_collide_sphere(head, radius, inside, p_bone_radius, p_bone_length, p_current); + + // dispose of the non-capsule bone chains (the capsule collider just hits each bone node). + if (collide_mode != COLLIDE_MODE_CHAIN) { + // Pick sphere in collider capsule that best collides with the bone end point (the joint). + Vector3 capsule_sphere_center = _closest_capsule_sphere(head, tail, p_current); + return _collide_sphere(capsule_sphere_center, radius, (collide_mode == COLLIDE_MODE_INSIDE), p_bone_radius, p_current); } - float pls = p.length_squared(); - if (Math::is_zero_approx(pls)) { + + real_t capsule_mu = _closest_capsule_sphere_to_taper(head, tail, radius, p_bone_radius, p_bone_length, p_current_origin, p_bone_origin_radius, p_current); + if (capsule_mu == -1.0) { return p_current; } - if (pls <= dot) { - return SpringBoneCollisionSphere3D::_collide_sphere(head + p, radius, inside, p_bone_radius, p_bone_length, p_current); + + Vector3 capsule_sphere_center = head * (1.0 - capsule_mu) + tail * capsule_mu; + + // Numerically test the claim that we have found the sphere in the collision capsule that enters the bone cone the deepest. +#ifdef VERIFY_SPRINGBONECAPSULE_CALCULATIONS + real_t Dvdist = verify_distance_within_taper(capsule_sphere_center, p_bone_radius, p_bone_length, p_current_origin, p_bone_origin_radius, p_current); + real_t mulo = MAX(capsule_mu - 0.1, 0.0); + Vector3 caplo = head * (1.0 - mulo) + tail * mulo; + real_t muhi = MIN(capsule_mu + 0.1, 1.0); + Vector3 caphi = head * (1.0 - muhi) + tail * muhi; + real_t Dvdistlo = verify_distance_within_taper(caplo, p_bone_radius, p_bone_length, p_current_origin, p_bone_origin_radius, p_current); + real_t Dvdisthi = verify_distance_within_taper(caphi, p_bone_radius, p_bone_length, p_current_origin, p_bone_origin_radius, p_current); + if (Dvdistlo < Dvdist - 0.001) { + printf("%d Non-minimal mu=%.2f %.3f < %.3f lo\n", SpringBoneCollision3D::Dsegmentindexbeingcalculated, capsule_mu, Dvdistlo, Dvdist); + } + if (Dvdisthi < Dvdist - 0.001) { + printf("%d Non-minimal mu=%.2f %.3f < %.3f hi\n", SpringBoneCollision3D::Dsegmentindexbeingcalculated, capsule_mu, Dvdisthi, Dvdist); } - return SpringBoneCollisionSphere3D::_collide_sphere(head + p * (dot / pls), radius, inside, p_bone_radius, p_bone_length, p_current); +#endif + + return _collide_sphere_taper(capsule_sphere_center, radius, p_bone_radius, p_bone_length, p_current_origin, p_bone_origin_radius, p_current); } diff --git a/scene/3d/spring_bone_collision_capsule_3d.h b/scene/3d/spring_bone_collision_capsule_3d.h index 5ea7bd9b5571..9aeec8d5d77c 100644 --- a/scene/3d/spring_bone_collision_capsule_3d.h +++ b/scene/3d/spring_bone_collision_capsule_3d.h @@ -37,12 +37,12 @@ class SpringBoneCollisionCapsule3D : public SpringBoneCollision3D { float radius = 0.1; float height = 0.5; - bool inside = false; + CollideMode collide_mode = COLLIDE_MODE_JOINT; protected: static void _bind_methods(); - virtual Vector3 _collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current) const override; + virtual Vector3 _collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) const override; public: void set_radius(float p_radius); @@ -51,8 +51,8 @@ class SpringBoneCollisionCapsule3D : public SpringBoneCollision3D { float get_height() const; void set_mid_height(real_t p_mid_height); real_t get_mid_height() const; - void set_inside(bool p_enabled); - bool is_inside() const; + void set_collide_mode(CollideMode p_collide_mode); + CollideMode get_collide_mode() const; // Helper. Pair get_head_and_tail(const Transform3D &p_center) const; diff --git a/scene/3d/spring_bone_collision_plane_3d.cpp b/scene/3d/spring_bone_collision_plane_3d.cpp index e70b9b297198..e785305760c5 100644 --- a/scene/3d/spring_bone_collision_plane_3d.cpp +++ b/scene/3d/spring_bone_collision_plane_3d.cpp @@ -30,7 +30,7 @@ #include "spring_bone_collision_plane_3d.h" -Vector3 SpringBoneCollisionPlane3D::_collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current) const { +Vector3 SpringBoneCollisionPlane3D::_collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) const { Transform3D tr = get_transform_from_skeleton(p_center); Vector3 pos = tr.origin; Vector3 normal = tr.basis.get_rotation_quaternion().xform(Vector3::UP); diff --git a/scene/3d/spring_bone_collision_plane_3d.h b/scene/3d/spring_bone_collision_plane_3d.h index 0d6a44e055dc..bf1b61a538bd 100644 --- a/scene/3d/spring_bone_collision_plane_3d.h +++ b/scene/3d/spring_bone_collision_plane_3d.h @@ -36,5 +36,5 @@ class SpringBoneCollisionPlane3D : public SpringBoneCollision3D { GDCLASS(SpringBoneCollisionPlane3D, SpringBoneCollision3D); protected: - virtual Vector3 _collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current) const override; + virtual Vector3 _collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) const override; }; diff --git a/scene/3d/spring_bone_collision_sphere_3d.cpp b/scene/3d/spring_bone_collision_sphere_3d.cpp index c49657d66fe8..b27f8a6aa6ed 100644 --- a/scene/3d/spring_bone_collision_sphere_3d.cpp +++ b/scene/3d/spring_bone_collision_sphere_3d.cpp @@ -43,38 +43,31 @@ float SpringBoneCollisionSphere3D::get_radius() const { return radius; } -void SpringBoneCollisionSphere3D::set_inside(bool p_enabled) { - inside = p_enabled; +SpringBoneCollision3D::CollideMode SpringBoneCollisionSphere3D::get_collide_mode() const { + return collide_mode; +} + +void SpringBoneCollisionSphere3D::set_collide_mode(CollideMode p_collide_mode) { + collide_mode = p_collide_mode; #ifdef TOOLS_ENABLED update_gizmos(); #endif // TOOLS_ENABLED } -bool SpringBoneCollisionSphere3D::is_inside() const { - return inside; -} - void SpringBoneCollisionSphere3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_radius", "radius"), &SpringBoneCollisionSphere3D::set_radius); ClassDB::bind_method(D_METHOD("get_radius"), &SpringBoneCollisionSphere3D::get_radius); - ClassDB::bind_method(D_METHOD("set_inside", "enabled"), &SpringBoneCollisionSphere3D::set_inside); - ClassDB::bind_method(D_METHOD("is_inside"), &SpringBoneCollisionSphere3D::is_inside); + ClassDB::bind_method(D_METHOD("set_collide_mode", "collide_mode"), &SpringBoneCollisionSphere3D::set_collide_mode); + ClassDB::bind_method(D_METHOD("get_collide_mode"), &SpringBoneCollisionSphere3D::get_collide_mode); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"), "set_radius", "get_radius"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "inside"), "set_inside", "is_inside"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "collide_mode", PROPERTY_HINT_ENUM, "Joint,Inside,Chain"), "set_collide_mode", "get_collide_mode"); } -Vector3 SpringBoneCollisionSphere3D::_collide_sphere(const Vector3 &p_origin, float p_radius, bool p_inside, float p_bone_radius, float p_bone_length, const Vector3 &p_current) { - Vector3 diff = p_current - p_origin; - float length = diff.length(); - float r = p_inside ? p_radius - p_bone_radius : p_bone_radius + p_radius; - float distance = p_inside ? r - length : length - r; - if (distance > 0) { - return p_current; +Vector3 SpringBoneCollisionSphere3D::_collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) const { + Vector3 origin = get_transform_from_skeleton(p_center).origin; + if (collide_mode == COLLIDE_MODE_CHAIN) { + return _collide_sphere_taper(origin, radius, p_bone_radius, p_bone_length, p_current_origin, p_bone_origin_radius, p_current); } - return p_origin + diff.normalized() * r; -} - -Vector3 SpringBoneCollisionSphere3D::_collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current) const { - return _collide_sphere(get_transform_from_skeleton(p_center).origin, radius, inside, p_bone_radius, p_bone_length, p_current); + return _collide_sphere(origin, radius, (collide_mode == COLLIDE_MODE_INSIDE), p_bone_radius, p_current); } diff --git a/scene/3d/spring_bone_collision_sphere_3d.h b/scene/3d/spring_bone_collision_sphere_3d.h index 2f109544108a..5ebd0d926a3a 100644 --- a/scene/3d/spring_bone_collision_sphere_3d.h +++ b/scene/3d/spring_bone_collision_sphere_3d.h @@ -40,17 +40,16 @@ class SpringBoneCollisionSphere3D : public SpringBoneCollision3D { friend class SpringBoneCollisionCapsule3D; float radius = 0.1; - bool inside = false; + CollideMode collide_mode = COLLIDE_MODE_JOINT; protected: static void _bind_methods(); - static Vector3 _collide_sphere(const Vector3 &p_origin, float p_radius, bool p_inside, float p_bone_radius, float p_bone_length, const Vector3 &p_current); - virtual Vector3 _collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current) const override; + virtual Vector3 _collide(const Transform3D &p_center, float p_bone_radius, float p_bone_length, const Vector3 &p_current_origin, float p_bone_origin_radius, const Vector3 &p_current) const override; public: void set_radius(float p_radius); float get_radius() const; - void set_inside(bool p_enabled); - bool is_inside() const; + void set_collide_mode(CollideMode p_collide_mode); + CollideMode get_collide_mode() const; }; diff --git a/scene/3d/spring_bone_simulator_3d.cpp b/scene/3d/spring_bone_simulator_3d.cpp index f8780f4f65f5..9ded5bc81ce1 100644 --- a/scene/3d/spring_bone_simulator_3d.cpp +++ b/scene/3d/spring_bone_simulator_3d.cpp @@ -1849,7 +1849,9 @@ void SpringBoneSimulator3D::_process_joints(double p_delta, Skeleton3D *p_skelet SpringBoneCollision3D *col = Object::cast_to(obj); if (col) { // Collider movement should separate from the effect of the center. - next_tail = col->collide(p_center_transform, p_joints[i]->radius, verlet->length, next_tail); + float origin_radius = p_joints[(i > 0 ? i - 1 : i)]->radius; + SpringBoneCollision3D::Dsegmentindexbeingcalculated = i; + next_tail = col->collide(p_center_transform, p_joints[i]->radius, verlet->length, current_origin, origin_radius, next_tail); // Snap to plane if axis locked. if (p_joints[i]->rotation_axis != ROTATION_AXIS_ALL) { next_tail = current_world_pose.origin + current_world_pose.basis.get_rotation_quaternion().xform(snap_vector_to_plane(p_joints[i]->get_rotation_axis_vector(), current_world_pose.basis.get_rotation_quaternion().xform_inv(next_tail - current_world_pose.origin)));