extends RefCounted
class_name CurveMath
## THE CURVE, EVALUATED (#120 F5, docs/node-tree-and-stacks.md §6). Pure and static: one
## function set used by follow-curve, cables and flanges, so they all walk a curve the
## same way.
##
## A curve is points {p, h_in, h_out, tilt, radius, n}: p a position, h_in / h_out the
## handles as OFFSETS from p (so moving a point carries its handles), tilt in degrees
## about the curve, radius a scale for whatever is swept along it, and n the SURFACE
## NORMAL where the point was placed (stored, so no peer re-raycasts; it seeds the first
## frame, and a cable's end leaves the surface along it — Cablerator's normal handles).
##
## Segment i is the cubic Bézier p_i, p_i + h_out_i, p_{i+1} + h_in_{i+1}, p_{i+1}; a
## closed curve adds the segment back to the first point.
##
## FRAMES ARE ROTATION-MINIMISING (parallel transport): each normal is the previous one
## turned by the smallest rotation taking the previous tangent onto this one, so a
## swept profile does not twist on its own the way Frenet frames do at an inflection.
## Then each point's tilt (interpolated by length) turns it about the tangent.
## Subdivisions per segment for the arc-length table.
const TABLE_STEPS := 32
static func point(p: Vector3, n: Vector3 = Vector3.UP) -> Dictionary:
return {"p": p, "h_in": Vector3.ZERO, "h_out": Vector3.ZERO, "tilt": 0.0, "radius": 1.0, "n": n}
static func segment_count(pts: Array, closed: bool) -> int:
if pts.size() < 2:
return 0
return pts.size() if closed else pts.size() - 1
## The four control points of segment i.
static func controls(pts: Array, closed: bool, i: int) -> Array:
var a: Dictionary = pts[i]
var b: Dictionary = pts[(i + 1) % pts.size()]
return [a["p"], (a["p"] as Vector3) + (a["h_out"] as Vector3), (b["p"] as Vector3) + (b["h_in"] as Vector3), b["p"]]
static func bezier(c: Array, t: float) -> Vector3:
var u := 1.0 - t
return (c[0] as Vector3) * (u * u * u) + (c[1] as Vector3) * (3.0 * u * u * t) \
+ (c[2] as Vector3) * (3.0 * u * t * t) + (c[3] as Vector3) * (t * t * t)
static func bezier_d(c: Array, t: float) -> Vector3:
var u := 1.0 - t
return ((c[1] as Vector3) - (c[0] as Vector3)) * (3.0 * u * u) \
+ ((c[2] as Vector3) - (c[1] as Vector3)) * (6.0 * u * t) \
+ ((c[3] as Vector3) - (c[2] as Vector3)) * (3.0 * t * t)
## HANDLE TYPES (docs/node-tree-and-stacks.md §6.1, Blender's): a point's `handle`; a point
## without one is auto, so curves recorded before types fold unchanged.
const HANDLE_TYPES := ["auto", "aligned", "free", "vector"]
static func handle_type(q: Dictionary) -> String:
var t := String(q.get("handle", "auto"))
return t if HANDLE_TYPES.has(t) else "auto"
## AUTO HANDLES (as points are placed): Catmull-Rom tangents, a third of the way to the
## neighbours; an open curve's ends leave along their surface normal (a cable rises off
## the wall it is fixed to), scaled to a third of the first / last span.
## By TYPE (§6.1): auto as above; vector = a third of the way to each neighbour (an open
## end mirrors its one side); aligned / free keep their stored handles.
static func auto_handles(pts: Array, closed: bool) -> Array:
var out: Array = []
var n := pts.size()
for i in n:
var q: Dictionary = (pts[i] as Dictionary).duplicate(true)
var p: Vector3 = q["p"]
var kind := handle_type(q)
if kind == "aligned" or kind == "free":
out.append(q)
continue
if kind == "vector":
var has_prev := closed or i > 0
var has_next := closed or i < n - 1
var to_prev: Vector3 = ((pts[(i - 1 + n) % n] as Dictionary)["p"] as Vector3) - p if has_prev else Vector3.ZERO
var to_next: Vector3 = ((pts[(i + 1) % n] as Dictionary)["p"] as Vector3) - p if has_next else Vector3.ZERO
q["h_in"] = to_prev / 3.0 if has_prev else -to_next / 3.0
q["h_out"] = to_next / 3.0 if has_next else -to_prev / 3.0
out.append(q)
continue
var prev: Vector3
var next: Vector3
if closed:
prev = (pts[(i - 1 + n) % n] as Dictionary)["p"]
next = (pts[(i + 1) % n] as Dictionary)["p"]
else:
prev = (pts[maxi(i - 1, 0)] as Dictionary)["p"]
next = (pts[mini(i + 1, n - 1)] as Dictionary)["p"]
var tan := (next - prev) * 0.5
q["h_out"] = tan / 3.0
q["h_in"] = -tan / 3.0
# An open curve's ends leave / arrive along their surface normal, a third of the
# span out: the first point's out-handle and the last point's in-handle point off
# the surface they were placed on.
var nn: Vector3 = (q["n"] as Vector3).normalized() if q.get("n") is Vector3 else Vector3.ZERO
if not closed and n >= 2 and nn.length() > 0.5 and (i == 0 or i == n - 1):
var other: Vector3 = (pts[1] as Dictionary)["p"] if i == 0 else (pts[n - 2] as Dictionary)["p"]
var off := nn * other.distance_to(p) / 3.0
if i == 0:
q["h_out"] = off
q["h_in"] = -off
else:
q["h_in"] = off
q["h_out"] = -off
out.append(q)
return out
## Cumulative arc length: {s: PackedFloat64Array, seg: PackedInt32Array, t: PackedFloat64Array}
## over TABLE_STEPS samples per segment, starting at s = 0.
static func table(pts: Array, closed: bool) -> Dictionary:
var s := PackedFloat64Array([0.0])
var seg := PackedInt32Array([0])
var tt := PackedFloat64Array([0.0])
var total := 0.0
for i in segment_count(pts, closed):
var c := controls(pts, closed, i)
var prev := bezier(c, 0.0)
for k in range(1, TABLE_STEPS + 1):
var t := float(k) / TABLE_STEPS
var q := bezier(c, t)
total += prev.distance_to(q)
prev = q
s.append(total)
seg.append(i)
tt.append(t)
return {"s": s, "seg": seg, "t": tt}
static func length(pts: Array, closed: bool) -> float:
var tb := table(pts, closed)
var s: PackedFloat64Array = tb["s"]
return s[s.size() - 1] if not s.is_empty() else 0.0
## (segment, t) at arc length `d`, by the table (linear between its entries).
static func locate(tb: Dictionary, d: float) -> Array:
var s: PackedFloat64Array = tb["s"]
var lo := 0
var hi := s.size() - 1
if d <= 0.0:
return [int((tb["seg"] as PackedInt32Array)[0]), 0.0]
if d >= s[hi]:
return [int((tb["seg"] as PackedInt32Array)[hi]), 1.0]
while hi - lo > 1:
var mid := (lo + hi) / 2
if s[mid] < d:
lo = mid
else:
hi = mid
var f := (d - s[lo]) / maxf(s[hi] - s[lo], 1e-12)
var seg_hi := int((tb["seg"] as PackedInt32Array)[hi])
var t_lo: float = (tb["t"] as PackedFloat64Array)[lo]
if int((tb["seg"] as PackedInt32Array)[lo]) != seg_hi:
t_lo = 0.0 # the table entry before a segment's first step
return [seg_hi, lerpf(t_lo, (tb["t"] as PackedFloat64Array)[hi], f)]
## `count` samples evenly spaced by length (both ends included on an open curve; a closed
## one does not repeat its start): [{p, t (tangent), n (normal), b (binormal), radius, s}].
static func sample(pts: Array, closed: bool, count: int) -> Array:
var out: Array = []
if segment_count(pts, closed) == 0 or count < 2:
return out
var tb := table(pts, closed)
var total: float = (tb["s"] as PackedFloat64Array)[(tb["s"] as PackedFloat64Array).size() - 1]
var steps := count if closed else count - 1
var prev_t := Vector3.ZERO
var nrm := Vector3.ZERO
for k in count:
var d := total * float(k) / float(steps)
var loc := locate(tb, d)
var c := controls(pts, closed, int(loc[0]))
var p := bezier(c, float(loc[1]))
var tan := bezier_d(c, float(loc[1]))
if tan.length() < 1e-9:
tan = prev_t if prev_t != Vector3.ZERO else Vector3.FORWARD
tan = tan.normalized()
if k == 0:
# Seeded by the first point's surface normal, made perpendicular to the tangent.
var sd: Vector3 = ((pts[0] as Dictionary).get("n", Vector3.UP) as Vector3)
nrm = sd - tan * sd.dot(tan)
if nrm.length() < 1e-6:
nrm = Vector3.UP - tan * tan.y
if nrm.length() < 1e-6:
nrm = Vector3.RIGHT - tan * tan.x
nrm = nrm.normalized()
else:
# Parallel transport: turn the normal by the rotation taking prev_t onto tan.
var axis := prev_t.cross(tan)
var sn := axis.length()
if sn > 1e-9:
var ang := atan2(sn, prev_t.dot(tan))
nrm = nrm.rotated(axis / sn, ang)
nrm = (nrm - tan * nrm.dot(tan)).normalized()
prev_t = tan
var tilt := _along(pts, closed, tb, d, "tilt")
var rad := _along(pts, closed, tb, d, "radius")
var n2 := nrm.rotated(tan, deg_to_rad(tilt)) if absf(tilt) > 1e-9 else nrm
out.append({"p": p, "t": tan, "n": n2, "b": tan.cross(n2), "radius": rad, "s": d})
return out
## Samples per segment for a profile outline: a `vector` corner lands on a sample exactly.
const PROFILE_STEPS := 4
## A profile outline has at most this many vertices: a cable piece is the hull of two
## rings, and Jolt refused a hull of 63 planes (§11.4).
const PROFILE_MAX := 24
## A CUSTOM CABLE PROFILE (§14.2): a CLOSED curve `c` ({points, closed}) as a convex 2D
## outline, normalised so its farthest vertex is 1 (a cable's `radius` stays half its width).
## Its plane: the samples' Newell normal through their centroid, u = towards the first
## sample, v = normal x u — derived from the points alone, so every peer agrees. A concave
## outline becomes its convex hull. Empty when the curve is open, flat to a line, or under
## 3 distinct points.
static func profile_outline(c: Dictionary) -> PackedVector2Array:
var pts: Array = c.get("points", [])
if not bool(c.get("closed", false)) or pts.size() < 3:
return PackedVector2Array()
var smp := PackedVector3Array()
for i in segment_count(pts, true):
var ctl := controls(pts, true, i)
for k in PROFILE_STEPS:
smp.append(bezier(ctl, float(k) / PROFILE_STEPS))
var o := Vector3.ZERO
for q: Vector3 in smp:
o += q
o /= float(smp.size())
var nrm := Vector3.ZERO
for i in smp.size():
var a := smp[i] - o
var b := smp[(i + 1) % smp.size()] - o
nrm += a.cross(b)
if nrm.length() < 1.0e-9:
return PackedVector2Array()
nrm = nrm.normalized()
var u := Vector3.ZERO
for q: Vector3 in smp:
var w := (q - o) - nrm * (q - o).dot(nrm)
if w.length() > 1.0e-6:
u = w.normalized()
break
if u == Vector3.ZERO:
return PackedVector2Array()
var v := nrm.cross(u)
var flat := PackedVector2Array()
for q: Vector3 in smp:
flat.append(Vector2((q - o).dot(u), (q - o).dot(v)))
var hull := Geometry2D.convex_hull(flat)
if hull.size() > 1 and hull[0].is_equal_approx(hull[hull.size() - 1]):
hull.remove_at(hull.size() - 1) # convex_hull closes the loop
# Collinear vertices add planes and no shape.
var out := PackedVector2Array()
for i in hull.size():
var p0 := hull[(i - 1 + hull.size()) % hull.size()]
var p1 := hull[i]
var p2 := hull[(i + 1) % hull.size()]
if absf((p1 - p0).cross(p2 - p1)) > 1.0e-9:
out.append(p1)
while out.size() > PROFILE_MAX:
var half := PackedVector2Array()
for i in range(0, out.size(), 2):
half.append(out[i])
out = half
if out.size() < 3:
return PackedVector2Array()
var far := 0.0
for q2: Vector2 in out:
far = maxf(far, q2.length())
if far < 1.0e-9:
return PackedVector2Array()
for i in out.size():
out[i] = out[i] / far
return out
## A per-point value (tilt, radius) interpolated by arc length between the two points
## of the segment `d` lies on.
static func _along(pts: Array, closed: bool, tb: Dictionary, d: float, key: String) -> float:
var loc := locate(tb, d)
var i := int(loc[0])
var a := float((pts[i] as Dictionary).get(key, 0.0 if key == "tilt" else 1.0))
var b := float((pts[(i + 1) % pts.size()] as Dictionary).get(key, 0.0 if key == "tilt" else 1.0))
return lerpf(a, b, float(loc[1]))