SpringBoneSimulator3D to implement COLLIDE_MODE_CHAIN to detect between the joints

doc/classes/SpringBoneCollision3D.xml

@@ -34,4 +34,15 @@
 			The offset of the rotation from [Skeleton3D]'s [member bone] pose rotation.
 		</member>
 	</members>
+	<constants>
+		<constant name="COLLIDE_MODE_JOINT" value="0" enum="CollideMode">
+			Collision is only with spheres around the joints.
+		</constant>
+		<constant name="COLLIDE_MODE_INSIDE" value="1" enum="CollideMode">
+			Collision acts to trap the spheres around the joints within the collision.
+		</constant>
+		<constant name="COLLIDE_MODE_CHAIN" value="2" enum="CollideMode">
+			Collision is against the joint spheres and the tapered cones between them.
+		</constant>
+	</constants>
 </class>

doc/classes/SpringBoneCollisionCapsule3D.xml

@@ -9,6 +9,9 @@
 	<tutorials>
 	</tutorials>
 	<members>
+		<member name="collide_mode" type="int" enum="SpringBoneCollision3D.CollideMode" setter="set_collide_mode" getter="get_collide_mode" default="SpringBoneCollision3D.COLLIDE_MODE_JOINT">
+			Sets whether the collision capcule collides or traps the joint spheres, or interacts with the tapered cones between them.
+		</member>
 		<member name="height" type="float" setter="set_height" getter="get_height" default="0.5">
 			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.

doc/classes/SpringBoneCollisionSphere3D.xml

@@ -9,8 +9,8 @@
 	<tutorials>
 	</tutorials>
 	<members>
-		<member name="inside" type="bool" setter="set_inside" getter="is_inside" default="false">
-			If [code]true[/code], the collision acts to trap the joint within the collision.
+		<member name="collide_mode" type="int" enum="SpringBoneCollision3D.CollideMode" setter="set_collide_mode" getter="get_collide_mode" default="SpringBoneCollision3D.COLLIDE_MODE_JOINT">
+			Sets whether the collision sphere collides or traps the joint spheres, or interacts with the tapered cones between them.
 		</member>
 		<member name="radius" type="float" setter="set_radius" getter="get_radius" default="0.1">
 			The sphere's radius.

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<Material>(), skel_tr, skeleton->register_skin(skeleton->create_skin_from_rest_transforms()));
 }
 
+bool draw_as_chain = true;
 Ref<ArrayMesh> 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<SurfaceTool> surface_tool;
 	surface_tool.instantiate();
@@ -147,7 +149,13 @@ Ref<ArrayMesh> 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<ArrayMesh> SpringBoneSimulator3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_s
 				surface_tool->set_bones(Vector<int>(bones));
 				surface_tool->set_weights(Vector<float>(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<SurfaceTool> &p_surface_t
 	}
 }
 
+void SpringBoneSimulator3DGizmoPlugin::draw_sphere_tangent_cone(Ref<SurfaceTool> &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<SurfaceTool> &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<ArrayMesh> SpringBoneCollision3DGizmoPlugin::get_collision_mesh(SpringBoneCo
 
 	SpringBoneCollisionSphere3D *sphere = Object::cast_to<SpringBoneCollisionSphere3D>(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<SpringBoneCollisionCapsule3D>(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();
 	}
 

editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.h

@@ -48,6 +48,7 @@ class SpringBoneSimulator3DGizmoPlugin : public EditorNode3DGizmoPlugin {
 public:
 	static Ref<ArrayMesh> get_joints_mesh(Skeleton3D *p_skeleton, SpringBoneSimulator3D *p_simulator, bool p_is_selected);
 	static void draw_sphere(Ref<SurfaceTool> &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<SurfaceTool> &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<SurfaceTool> &p_surface_tool, const Vector3 &p_begin_pos, const Vector3 &p_end_pos, const Color &p_color);
 
 	bool has_gizmo(Node3D *p_spatial) override;

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);
 }
 

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);