deform3.py

import numpy as np
import random
import math

def approximate_jacobian(f, eps=1e-8):
    def jac(xi):
        fi = f(xi)
        m = fi.size
        n = xi.size
        J = np.zeros((m, n), float)
        for j in range(n):
            dx = np.zeros(n)
            dx[j] = eps
            J[:, j] = (f(xi + dx) - fi) / eps
        return J
    return jac

def analyze(J, tol=1e-6):
    U, S, Vt = np.linalg.svd(J, full_matrices=True)
    # Count singular values > tol → rank
    rank = np.sum(S > tol)
    # Nullity = #columns - rank
    n = J.shape[1]
    nullity = n - rank
    
    movable = []
    # Nullspace basis = last `nullity` columns of V = rows of Vt.T
    if nullity > 0:
        nullspace = Vt.T[:, rank:]
        # Find which variable indices have any significant component
        # across the nullspace basis vectors
        for i in range(n):
            # Compute the L2 norm of row i of `nullspace`
            comp_norm = np.linalg.norm(nullspace[i, :])
            if comp_norm > tol:
                movable.append(i)
    else:
        nullspace = np.zeros((n, 0))
    
    return int(nullity), movable

def solve(f, jac, x, tol=1e-6, max_iterations=100):
    n = x.size
    for i in range(max_iterations):
        fi = f(x)
        f_norm = np.linalg.norm(fi, ord=np.inf)
        if f_norm < tol:
            return x
        J = jac(x)
        dx = np.linalg.solve(J, -fi)
        x += dx
    raise NoConvergence(f"||F||max = {f_norm}")

def solve_soft(f, f_jac, g, g_jac, x, tol=1e-6, max_iterations=100, nudge=1e-2):
    x += perform_solver_step(g(x), g_jac(x))
    x = solve_least_squares(f, f_jac, x, tol, max_iterations, nudge)
    gi = g(x)
    g_norm_prev = np.linalg.norm(gi, ord=np.inf)
    for i in range(10):
        x += perform_solver_step(gi, g_jac(x))
        x = solve_least_squares(f, f_jac, x, tol, max_iterations, nudge)
        gi = g(x)
        g_norm = np.linalg.norm(gi, ord=np.inf)
        if g_norm >= g_norm_prev:
            return x
        g_norm_prev = g_norm
    return x

def solve_least_squares(f, jac, x, tol=1e-6, max_iterations=100, nudge=1e-2):
    n = x.size
    fi = f(x)
    f_norm = np.linalg.norm(fi, ord=np.inf)
    f_norm_prev = f_norm
    for i in range(max_iterations):
        if f_norm < tol:
            return x
        x += perform_solver_step(fi, jac(x))

        fi = f(x)
        f_norm = np.linalg.norm(fi, ord=np.inf)
        if f_norm >= f_norm_prev:
            x += np.random.uniform(-nudge, +nudge, n)
        f_norm_prev = f_norm
    raise NoConvergence(f"||F||max = {f_norm}")

def perform_solver_step(fi, J):
    # Levenberg-Marquardt
    lm_lambda = 1e-6
    fi = J.T @ fi
    J  = J.T @ J + lm_lambda * np.eye(J.shape[1])
    dx, *_ = np.linalg.lstsq(J, -fi, rcond=None)
    return dx

class NoConvergence(Exception):
    pass

# Add this in if needed.
# Armijo
#         alpha = alpha0
#         if damping:
#             f_norm = np.linalg.norm(fi)
#             while True:
#                 x_new = x + alpha * dx
#                 if np.linalg.norm(f(x_new)) <= (1 - c * alpha) * f_norm:
#                     break
#                 alpha *= rho
#         step = alpha * dx if damping else dx

solver_demo.py

from solver.deform3 import approximate_jacobian, analyze, solve_soft
import pygame
import numpy as np
import math

from solver.common import Group, setup, scalar
from solver import two

def solve_constraints(dragging=None):
    group = Group()
    constrs = []
    points = [two.point(group=group, pos=pos) for pos in sketch]
    for bunch, flavor, amt in constraints:
        if flavor == "dist":
            p1, p2 = bunch
            constrs.append(
                two.distance(scalar(value=amt*2), points[p1], points[p2]))
        if flavor == "angle":
            p1, p2, p3 = bunch
            constrs.append(
                two.phi(scalar(value=amt), points[p1], points[p2], points[p3]))
        if flavor == "anchor":
            points[bunch[0]].group = None

    if dragging is not None:
       constrs.append(
           two.drag(points[dragging], two.point(pos=pygame.mouse.get_pos())))

    f, g, x0, interp = setup(group, constrs)
    jac = approximate_jacobian(f)
    soft_jac = approximate_jacobian(g)
    sol = solve_soft(f, jac, g, soft_jac, x0)
    variables = interp(sol)
    for constraint in constrs:
        constraint.calc(variables)
    for i, pt in enumerate(points):
        sketch[i] = np.array(pt.pos, float)
#    print(analyze(jac(sol)))

def distance(p, q):
    qp = q - p
    return math.sqrt(np.dot(qp, qp))

def normal(p):
    mag2 = np.dot(p, p)
    return p / math.sqrt(mag2) if mag2 > 0 else p*0

def angle(p, q, r):
    vi = p-q
    vk = r-q
    ni, nk = np.linalg.norm(vi), np.linalg.norm(vk)
    if ni < 1e-12 or nk < 1e-12:
        return 0
    else:
        cos0 = np.dot(vi, vk)/(ni*nk)
        cos0 = np.clip(cos0, -1.0, 1.0)
        theta = np.arccos(cos0)
        return theta
    #npq = normal(p-q)
    #nrq = normal(r-q)
    #return math.acos(np.clip(np.dot(npq, nrq), -1, +1))

SCREEN_WIDTH = 800
SCREEN_HEIGHT = 600

pygame.init()

pygame.init()
font   = pygame.font.SysFont('Arial', 16)
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT))
clock  = pygame.time.Clock()

sketch = []
constraints = []

def erase_distance_constraint(a, b):
    prev_constraints = constraints[:]
    constraints.clear()
    for xx in prev_constraints:
        if xx[0] != (a,b) and xx[0] != (b,a):
            constraints.append(xx)

def erase_angle_constraint(a, b, c):
    prev_constraints = constraints[:]
    constraints.clear()
    for xx in prev_constraints:
        if xx[0] != (a,b,c) and xx[0] != (c,b,a):
            constraints.append(xx)

def float_point(a):
    prev_constraints = constraints[:]
    constraints.clear()
    for xx in prev_constraints:
        if xx[0] != (a,):
            constraints.append(xx)

def fix_point(a):
    float_point(a)
    constraints.append(((a,), "anchor", None))

def add_distance_constraint(a, b, dist):
    if a != b:
        erase_distance_constraint(a,b)
        constraints.append(((a,b), "dist", dist))

def add_angle_constraint(a, b, c, angle):
    if a != b and b != c and c != a:
        erase_angle_constraint(a,b,c)
        constraints.append(((a,b,c), "angle", angle))

def erase_constraints(i):
    prev_constraints = constraints[:]
    constraints.clear()
    for xx in prev_constraints:
        if i not in xx[0]:
            xx0 = tuple(x - 1*(x > i) for x in xx[0])
            constraints.append((xx0,) + xx[1:])

alight = 0
blight = 0
clight = 0
highlight = 0
running = True
while running:
    dt = clock.tick(30) / 1000.0
    for ev in pygame.event.get():
        if ev.type == pygame.QUIT:
            running = False
        elif ev.type == pygame.MOUSEBUTTONDOWN:
            if ev.button == 1:
                sketch.append(np.array(ev.pos))
            elif ev.button == 3 and len(sketch) > 0:
                i = min(range(len(sketch)), key=lambda i: np.dot(*[sketch[i] - ev.pos]*2))
                del sketch[i]
                erase_constraints(i)
        elif ev.type == pygame.MOUSEMOTION:
            if ev.buttons[1]:
                solve_constraints(highlight)
            elif len(sketch) > 0:
                highlight = min(range(len(sketch)), key=lambda i: np.dot(*[sketch[i] - ev.pos]*2))
            else:
                highlight = 0
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_a:
            alight = highlight
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_s:
            blight = highlight
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_d:
            clight = highlight
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_1:
            add_distance_constraint(alight, blight, 10)
            solve_constraints()
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_2:
            add_distance_constraint(alight, blight, 20)
            solve_constraints()
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_3:
            add_distance_constraint(alight, blight, 30)
            solve_constraints()
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_4:
            add_distance_constraint(alight, blight, 40)
            solve_constraints()
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_5:
            add_distance_constraint(alight, blight, 50)
            solve_constraints()
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_6:
            erase_distance_constraint(alight, blight)
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_q:
            add_angle_constraint(alight, blight, clight, 0)
            solve_constraints()
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_w:
            add_angle_constraint(alight, blight, clight, 0.25*math.pi)
            solve_constraints()
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_e:
            add_angle_constraint(alight, blight, clight, 0.5*math.pi)
            solve_constraints()
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_r:
            add_angle_constraint(alight, blight, clight, 0.75*math.pi)
            solve_constraints()
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_t:
            add_angle_constraint(alight, blight, clight, 1.0*math.pi)
            solve_constraints()
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_y:
            erase_angle_constraint(alight, blight, clight)
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_f:
            fix_point(alight)
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_g:
            float_point(alight)
        elif ev.type == pygame.KEYDOWN and ev.key == pygame.K_SPACE:
            try:
                solve_constraints()
            except:
                import traceback
                traceback.print_exc()

    screen.fill((30, 30, 30))
    for p in sketch:
        pygame.draw.circle(screen, (255,255,255), p, 2)
    if highlight < len(sketch):
        pygame.draw.circle(screen, (255,255,255), sketch[highlight], 4, 1)
    if clight < len(sketch):
        pygame.draw.circle(screen, (255,0,255), sketch[clight], 4, 1)
    if blight < len(sketch):
        pygame.draw.circle(screen, (255,255,0), sketch[blight], 4, 1)
    if alight < len(sketch):
        pygame.draw.circle(screen, (0,255,255), sketch[alight], 4, 1)

    for group, flavor, amount in constraints:
        points = tuple(sketch[i] for i in group)
        if flavor == 'dist':
            center = (points[0] + points[1]) / 2
            delta = (points[0] - points[1])
            dist = math.sqrt(np.dot(delta,delta))
            norm = delta / dist if dist > 0 else 0
            pygame.draw.line(screen, (200,200,200), center + norm*amount, center - norm*amount, 2)
       
        if flavor == 'angle':
            if abs(angle(*points) - amount) < 1e-4:
                pygame.draw.line(screen, (255,255,0), (points[0] + points[1])/2, points[1], 2)
                pygame.draw.line(screen, (255,255,0), points[1], (points[1] + points[2])/2, 2)
            else:
                pygame.draw.line(screen, (255,0,0), (points[0] + points[1])/2, points[1], 2)
                pygame.draw.line(screen, (255,0,0), points[1], (points[1] + points[2])/2, 2)
    pygame.display.flip()

three.py

# needs little bit of rewriting but otherwise fine.
from dataclasses import dataclass, field
from typing import List, Dict, Optional, Callable, Tuple, Any, Set, Union
from .common import expr, scalar, constraint

Vec3 = Tuple[float, float, float]

@dataclass(eq=False, kw_only=True)
class point(expr):
    pos : Vec3 = (0.0, 0.0, 0.0)
    span = 3
    def calc(self, variables):
        if self.group == variables.group:
            self.pos = variables[self]

    def initialize(self, initial):
        initial(self, *self.pos)

@dataclass(eq=False, kw_only=True)
class plane(expr):
    pos : Vec3 = (0.0, 0.0, 0.0)
    span = 3
    def calc(self, variables):
        if self.group == variables.group:
            self.pos = variables[self]

    def initialize(self, initial):
        initial(self, *self.pos)

@dataclass(eq=False, kw_only=True)
class line(expr):
    pos : Vec3 = (0.0, 0.0, 0.0)
    vec : Vec3 = (0.0, 0.0, 0.0)
    def calc(self, variables):
        assert False

@dataclass(eq=False)
class point_on_plane(point):
    a : point
    t : plane
    def calc(self, variables):
        assert False

    def initialize(self, initial):
        self.a.initialize(initial)
        self.t.initialize(initial)

@dataclass(eq=False)
class point_on_line(point):
    a : point
    t : line
    def calc(self, variables):
        assert False

    def initialize(self, initial):
        self.a.initialize(initial)
        self.t.initialize(initial)

@dataclass(eq=False)
class line_between(line):
    a : point
    b : point
    def calc(self, variables):
        self.a.calc(variables)
        self.b.calc(variables)
        self.pos = self.a.pos
        self.vec = sub(self.b.pos, self.a.pos)

    def initialize(self, initial):
        self.a.initialize(initial)
        self.b.initialize(initial)

def sub(p, q):
    return p[0]-q[0], p[1]-q[1], p[2]-q[2]

@dataclass(eq=False)
class same(constraint):
    a : point
    b : point

    def compute(self, variables):
        self.a.calc(variables)
        self.b.calc(variables)
        yield a.pos[0] - b.pos[0]
        yield a.pos[1] - b.pos[1]
        yield a.pos[2] - b.pos[2]

    def initialize(self, initial):
        self.a.initialize(initial)
        self.b.initialize(initial)

@dataclass(eq=False)
class distance(constraint):
    d : scalar
    a : point
    b : point
    def compute(self, variables):
        self.a.calc(variables)
        self.b.calc(variables)
        self.d.calc(variables)
        dx = a.pos[0] - b.pos[0]
        dy = a.pos[1] - b.pos[1]
        dz = a.pos[2] - b.pos[2]
        d  = self.d.value
        yield dx*dx + dy*dy + dz*dz - d*d

    def initialize(self, initial):
        self.d.initialize(initial)
        self.a.initialize(initial)
        self.b.initialize(initial)

@dataclass(eq=False)
class phi(constraint):
    d : scalar
    a : point
    b : point
    c : point
    def compute(self, variables):
        self.a.calc(variables)
        self.b.calc(variables)
        self.c.calc(variables)
        self.d.calc(variables)
        c  = math.cos(self.d.value)
        vx = a.pos[0] - b.pos[0]
        vy = a.pos[1] - b.pos[1]
        vz = a.pos[2] - b.pos[2]
        vl = math.sqrt(vx*vx + vy*vy + vz*vz)
        wx = c.pos[0] - b.pos[0]
        wy = c.pos[1] - b.pos[1]
        wz = c.pos[2] - b.pos[2]
        wl = math.sqrt(wx*wx + wy*wy + wz*wz)
        yield vx*wx + vy*wy + vz*wz - vl*wl*c

    def initialize(self, initial):
        self.d.initialize(initial)
        self.a.initialize(initial)
        self.b.initialize(initial)
        self.c.initialize(initial)

@dataclass(eq=False)
class line_line_distance(constraint):
    d : scalar
    a : line
    b : line
    def compute(self, variables):
        assert False

    def initialize(self, initial):
        self.d.initialize(initial)
        self.a.initialize(initial)
        self.b.initialize(initial)

two.py

from dataclasses import dataclass, field
from typing import List, Dict, Optional, Callable, Tuple, Any, Set, Union
from .common import expr, scalar, constraint
import math

Vec3 = Tuple[float, float, float]

@dataclass(eq=False, kw_only=True)
class point(expr):
    pos : Vec3 = (0.0, 0.0)
    span = 2
    def calc(self, variables):
        if self.group == variables.group:
            self.pos = variables[self]

    def initialize(self, initial):
        initial(self, *self.pos)

@dataclass(eq=False, kw_only=True)
class line(expr):
    pos : Vec3 = (0.0, 0.0)
    span = 2
    def calc(self, variables):
        if self.group == variables.group:
            self.pos = variables[self]

    def initialize(self, initial):
        initial(self, *self.pos)

@dataclass(eq=False)
class point_on_line(point):
    a : point
    t : line
    def calc(self, variables):
        assert False

    def initialize(self, initial):
        self.a.initialize(initial)
        self.t.initialize(initial)

@dataclass(eq=False)
class same(constraint):
    a : point
    b : point

    def calc(self, variables):
        self.a.calc(variables)
        self.b.calc(variables)

    def hard(self, variables):
        self.calc(variables)
        yield self.a.pos[0] - self.b.pos[0]
        yield self.a.pos[1] - self.b.pos[1]

    def initialize(self, initial):
        self.a.initialize(initial)
        self.b.initialize(initial)

@dataclass(eq=False)
class drag(constraint):
    a : point
    b : point

    def calc(self, variables):
        self.a.calc(variables)
        self.b.calc(variables)

    def soft(self, variables, _):
        self.calc(variables)
        yield (self.a.pos[0] - self.b.pos[0]) * 1000
        yield (self.a.pos[1] - self.b.pos[1]) * 1000

    def initialize(self, initial):
        self.a.initialize(initial)
        self.b.initialize(initial)

@dataclass(eq=False)
class distance(constraint):
    d : scalar
    a : point
    b : point
    def calc(self, variables):
        self.a.calc(variables)
        self.b.calc(variables)
        self.d.calc(variables)

    def hard(self, variables):
        self.calc(variables)
        dx = self.a.pos[0] - self.b.pos[0]
        dy = self.a.pos[1] - self.b.pos[1]
        d  = self.d.value
        yield dx*dx + dy*dy - d*d

    def initialize(self, initial):
        self.d.initialize(initial)
        self.a.initialize(initial)
        self.b.initialize(initial)

@dataclass(eq=False)
class phi(constraint):
    d : scalar
    a : point
    b : point
    c : point
    def calc(self, variables):
        self.a.calc(variables)
        self.b.calc(variables)
        self.c.calc(variables)
        self.d.calc(variables)

    def hard(self, variables):
        self.calc(variables)
        c  = math.cos(self.d.value)
        vx = self.a.pos[0] - self.b.pos[0]
        vy = self.a.pos[1] - self.b.pos[1]
        vl = math.sqrt(vx*vx + vy*vy)
        wx = self.c.pos[0] - self.b.pos[0]
        wy = self.c.pos[1] - self.b.pos[1]
        wl = math.sqrt(wx*wx + wy*wy)
        yield vx*wx + vy*wy - vl*wl*c

    def soft(self, variables, previous):
        self.a.calc(previous)
        self.b.calc(previous)
        self.c.calc(previous)
        prev_a = self.a.pos
        prev_b = self.b.pos
        prev_c = self.c.pos
        self.calc(variables)
        now_a = self.a.pos
        now_b = self.b.pos
        now_c = self.c.pos
        yield dist(prev_a, prev_b) - dist(now_a, now_b)
        yield dist(prev_c, prev_b) - dist(now_c, now_b)

    def initialize(self, initial):
        self.d.initialize(initial)
        self.a.initialize(initial)
        self.b.initialize(initial)
        self.c.initialize(initial)

def dist(v, w):
    dx = v[0] - w[0]
    dy = v[1] - w[1]
    return math.sqrt(dx*dx + dy*dy)