Luce packages
LuceLuce BaseLuciaOS

dymokomi/luce-cad

Source of release 0.9.4

0.9.4 / src / geometry / inversion.lucb

## Inverting a B-spline surface: the normalized parameters of a point on
## it (Newton from a seed, then a lattice and the control points' Greville
## parameters as further starts), and its point at normalized parameters.
## Charts of B-spline faces are [0, 1] x [0, 1] over the knot domain.

## Surface `surface` at normalized parameters `uv`.
pub func evaluate(surface: Surface, uv: Vector3) -> Vector3!:
    var u: Knot
    var v: Knot
    try u.at(surface.ku, surface.nu, surface.du, uv.x)
    try v.at(surface.kv, surface.nv, surface.dv, uv.y)
    return try point_of(surface, &u, &v)

## One normalized parameter's knot value and nonzero basis. Newton steps
## reuse them: finite differences share one parameter with the current point,
## and an accepted line-search candidate is the next iteration's point.
struct Knot:
    var first: usize
    var values: f64[12]

    ## The clamp and domain check of Surface.at, then the basis.
    func at(knots: const f64[], count: i64, degree: i64, fraction: f64) -> !:
        let low = knots[(usize)degree]
        let high = knots[(usize)count]
        let t = math.clamp(low + (high - low) * fraction, low, high)
        if not math.is_finite(t) or t < low or t > high:
            error(invalid, "surface parameter outside knot domain")
        self.first = nurbs_basis(knots, count, degree, t, &self.values)

func point_of(surface: Surface, u: Knot*, v: Knot*) -> Vector3!:
    return try nurbs_combine(surface.points, surface.weights, surface.nv, surface.du, surface.dv, u.first, &u.values, v.first, &v.values)

## Distance as Vector3.length computes it.
inline func separation(a: Vector3, b: Vector3) -> f64:
    return math.hypot(math.hypot(a.x - b.x, a.y - b.y), a.z - b.z)

## The normalized parameters of `surface`'s point nearest `target` (on it
## or off it), from `seed`: Newton on the distance, the nearest local
## answer; with `global`, the best of a lattice of starts first.
pub func nearest_parameters(surface: Surface, target: Vector3, seed: Vector3, global: bool) -> Vector3!:
    var start = seed
    if global:
        var best: f64 = 1e300
        for u in 0..<9:
            for v in 0..<9:
                let candidate = Vector3((f64)u / 8.0, (f64)v / 8.0, 0.0)
                let distance = (try evaluate(surface, candidate)).subtract(target).length()
                if distance < best:
                    best = distance
                    start = candidate
    return try improve(surface, target, start)

## Newton inversion with finite-difference tangents. Every evaluation equals
## the plain evaluate() of the same parameters; only repeated bases and the
## accepted candidate's point are reused, so the iterates are unchanged.
func improve(surface: Surface, target: Vector3, seed: Vector3) -> Vector3!:
    var uv = seed
    var u: Knot
    var v: Knot
    var p = Vector3()
    var known = false
    for iteration in 0..<48:
        if not known:
            # Evaluation order as before: the u check precedes the v check.
            try u.at(surface.ku, surface.nu, surface.du, uv.x)
            try v.at(surface.kv, surface.nv, surface.dv, uv.y)
            p = try point_of(surface, &u, &v)
        known = false
        let residual = Vector3(target.x - p.x, target.y - p.y, target.z - p.z)
        let residual_length = math.hypot(math.hypot(residual.x, residual.y), residual.z)
        # Layout intersections need parameter precision finer than a grid snap.
        # The acceptance tolerance below is unchanged; this only solves the UV
        # inverse more accurately when the edge actually lies on the support.
        if residual_length < 0.000000000001:
            break
        let ua = math.max(0.0, uv.x - 0.00001)
        let ub = math.min(1.0, uv.x + 0.00001)
        let va = math.max(0.0, uv.y - 0.00001)
        let vb = math.min(1.0, uv.y + 0.00001)
        var side: Knot
        try side.at(surface.ku, surface.nu, surface.du, ub)
        let pub_ = try point_of(surface, &side, &v)
        try side.at(surface.ku, surface.nu, surface.du, ua)
        let pua = try point_of(surface, &side, &v)
        try side.at(surface.kv, surface.nv, surface.dv, vb)
        let pvb = try point_of(surface, &u, &side)
        try side.at(surface.kv, surface.nv, surface.dv, va)
        let pva = try point_of(surface, &u, &side)
        let su = 1.0 / (ub - ua)
        let sv = 1.0 / (vb - va)
        let du = Vector3((pub_.x - pua.x) * su, (pub_.y - pua.y) * su, (pub_.z - pua.z) * su)
        let dv = Vector3((pvb.x - pva.x) * sv, (pvb.y - pva.y) * sv, (pvb.z - pva.z) * sv)
        let aa = du.x * du.x + du.y * du.y + du.z * du.z
        let ab = du.x * dv.x + du.y * dv.y + du.z * dv.z
        let bb = dv.x * dv.x + dv.y * dv.y + dv.z * dv.z
        let det = aa * bb - ab * ab
        if det <= aa * bb * 0.000000000001:
            break
        let ur = du.x * residual.x + du.y * residual.y + du.z * residual.z
        let vr = dv.x * residual.x + dv.y * residual.y + dv.z * residual.z
        var dx = math.clamp((bb * ur - ab * vr) / det, -0.25, 0.25)
        var dy = math.clamp((aa * vr - ab * ur) / det, -0.25, 0.25)
        # Solve the remaining tangent when a constrained parameter cannot move.
        # Clamping the unconstrained 2D Newton step alone made edge projections
        # seed-dependent when STEP curves differ slightly from the support.
        if (uv.x <= 1e-12 and dx < 0.0) or (uv.x >= 1.0 - 1e-12 and dx > 0.0):
            dx = 0.0
            dy = math.clamp(vr / bb, -0.25, 0.25)
        if (uv.y <= 1e-12 and dy < 0.0) or (uv.y >= 1.0 - 1e-12 and dy > 0.0):
            dy = 0.0
            dx = math.clamp(ur / aa, -0.25, 0.25)
        var accepted = false
        var factor: f64 = 1.0
        var cu: Knot
        var cv: Knot
        for step in 0..<12:
            let candidate = Vector3(math.clamp(uv.x + dx * factor, 0.0, 1.0), math.clamp(uv.y + dy * factor, 0.0, 1.0), 0.0)
            try cu.at(surface.ku, surface.nu, surface.du, candidate.x)
            try cv.at(surface.kv, surface.nv, surface.dv, candidate.y)
            let q = try point_of(surface, &cu, &cv)
            if separation(q, target) < residual_length:
                uv = candidate
                u = cu
                v = cv
                p = q
                known = true
                accepted = true
                break
            factor *= 0.5
        if not accepted:
            break
    return uv

pub func project(surface: Surface, target: Vector3, seed: Vector3, tolerance: f64) -> Vector3!:
    var uv = try improve(surface, target, seed)
    if (try evaluate(surface, uv)).subtract(target).length() <= tolerance:
        return uv
    let first = uv
    var best: f64 = 1e100
    for u in 0..<9:
        for v in 0..<9:
            let candidate = Vector3((f64)u / 8.0, (f64)v / 8.0, 0.0)
            let distance = (try evaluate(surface, candidate)).subtract(target).length()
            if distance < best:
                best = distance
                uv = candidate
    uv = try improve(surface, target, uv)
    # An edited surface's trims may be off it by design: the nearest point
    # found from the seed or the lattice is the answer (without the costlier
    # local starts below).
    if surface.edited:
        if (try evaluate(surface, uv)).subtract(target).length() > (try evaluate(surface, first)).subtract(target).length(): return first
        return uv
    if (try evaluate(surface, uv)).subtract(target).length() > tolerance:
        # Narrow knot spans can disappear entirely between uniform seeds. Use
        # nearby control points' Greville parameters as additional local starts.
        var distances: f64[16]
        let seeds = try new Vector3[16] --- in memory.heap
        defer free(seeds) in memory.heap
        for index in 0..<(usize)16: seeds[index] = Vector3()
        for index in 0..<(usize)16: distances[index] = 1e100
        for u in 0..<(usize)surface.nu:
            for v in 0..<(usize)surface.nv:
                let delta = surface.points[u * (usize)surface.nv + v].subtract(target)
                let distance = delta.dot(delta)
                if distance >= distances[15]: continue
                var index: usize = 15
                while index > 0 and distance < distances[index - 1]:
                    distances[index] = distances[index - 1]
                    seeds[index] = seeds[index - 1]
                    index -= 1
                distances[index] = distance
                seeds[index] = Vector3(greville(surface.ku, u, surface.nu, surface.du), greville(surface.kv, v, surface.nv, surface.dv), 0.0)
        for index in 0..<(usize)16:
            if distances[index] == 1e100: break
            let candidate = try improve(surface, target, seeds[index])
            if (try evaluate(surface, candidate)).subtract(target).length() <= tolerance: return candidate
            if (try evaluate(surface, candidate)).subtract(target).length() < (try evaluate(surface, uv)).subtract(target).length(): uv = candidate
        var residual = (try evaluate(surface, uv)).subtract(target).length()
        let from_seed = (try evaluate(surface, first)).subtract(target).length()
        if from_seed < residual:
            uv = first
            residual = from_seed
        # A trim the surface holds within its trim tolerance (the B-rep's
        # validity contract, Surface.trim_tolerance) is on it: its nearest
        # point found.
        if residual <= surface.trims_within(tolerance): return uv
        var buffer: u8[512]
        let owned = try interop.copy_text(try strings.format(buffer, f"NURBS boundary projection exceeds CAD tolerance: residual {residual}, tolerance {tolerance}"))
        if let owner = owned.owner: _ = ownership.pool_object(owner)
        error(invalid, owned.value)
    return uv

func greville(knots: const f64[], at: usize, count: i64, degree: i64) -> f64:
    var sum: f64 = 0.0
    for index in 1..<(usize)degree + 1: sum += knots[at + index]
    let low = knots[(usize)degree]
    return math.clamp((sum / (f64)degree - low) / (knots[(usize)count] - low), 0.0, 1.0)

## The knot parameter of `target` on a B-spline curve (STEP edges trimmed
## inside its knot domain), from `preferred`: each nonempty span searched
## (golden sections over sixteen parts), not a uniform grid that misses
## short spans; an error beyond `tolerance`.
pub func curve_parameter(points: const Vector3[], weights: const f64[], degree: i64, knots: const f64[], target: Vector3, preferred: f64, tolerance: f64) -> f64!:
    var best = preferred
    var distance = (try NurbsCurve.at(points, weights, degree, knots, best)).subtract(target).length()
    if distance <= tolerance: return best
    for span in (usize)degree..<points.length:
        if knots[span + 1] <= knots[span]: continue
        # Split each span before a bounded scalar minimization; a degree-eight
        # curve may have several local distance minima in one span.
        for part in 0..<16:
            var a = knots[span] + (knots[span + 1] - knots[span]) * (f64)part / 16.0
            var b = knots[span] + (knots[span + 1] - knots[span]) * (f64)(part + 1) / 16.0
            var left = b - (b - a) * 0.6180339887498949
            var right = a + (b - a) * 0.6180339887498949
            var dl = (try NurbsCurve.at(points, weights, degree, knots, left)).subtract(target).length()
            var dr = (try NurbsCurve.at(points, weights, degree, knots, right)).subtract(target).length()
            for iteration in 0..<48:
                if dl < dr:
                    b = right
                    right = left
                    dr = dl
                    left = b - (b - a) * 0.6180339887498949
                    dl = (try NurbsCurve.at(points, weights, degree, knots, left)).subtract(target).length()
                else:
                    a = left
                    left = right
                    dl = dr
                    right = a + (b - a) * 0.6180339887498949
                    dr = (try NurbsCurve.at(points, weights, degree, knots, right)).subtract(target).length()
            if math.min(dl, dr) < distance:
                distance = math.min(dl, dr)
                best = left if dl < dr else right
    if distance > tolerance:
        var buffer: u8[256]
        let text = try interop.copy_text(try strings.format(buffer, f"B-spline endpoint residual {distance} exceeds tolerance {tolerance}"))
        if let owner = text.owner: _ = ownership.pool_object(owner)
        error(invalid, text.value)
    return best