Intersection between bezier curve and a line segment

Question

I am writing a game in Python (with pygame) that requires me to generate random but nice-looking \"sea\" for each new game. After a long search I settled on an algorithm that in

Answer 1

I've finally got to a working code to illustrate the method suggested by Mark Thornton. Below is the Python code for the intersection routine, together with pygame code to test it visually. The cubic roots solution can be written based on this question.

import pygame
from pygame.locals import *
import sys
import random
from math import sqrt, fabs, pow
from lines import X, Y
import itertools
import pygame
from pygame import draw, Color
import padlib
from roots_detailed import cubicRoots


def add_points(*points):
    X = 0
    Y = 0
    for (x,y) in points:
        X += x
        Y += y
    return (X,Y)

def diff_points(p2, p1):
    # p2 - p1
    return (X(p2)-X(p1), Y(p2)-Y(p1));

def scale_point(factor, p):
    return (factor * X(p), factor*Y(p))

def between(v0, v, v1):
    if v0 > v1: v0, v1 = v1, v0
    return v >= v0 and v <= v1


# the point is guaranteed to be on the right line
def pointOnLineSegment(l1, l2, point):
    return between(X(l1), X(point), X(l2)) and between(Y(l1), Y(point), Y(l2))


def rotate(x, y, R1, R2, R3, R4):
    return (x*R1 + y*R2, x*R3 + y * R4);

def findIntersections(p0, p1, m0, m1, l1, l2):
    # We're solving the equation of one segment of Kochanek-Bartels
    # spline intersecting with a line segment
    # The spline is described at http://en.wikipedia.org/wiki/Cubic_Hermite_spline 
    # The discussion on the adopted solution can be found at https://stackoverflow.com/questions/1813719/intersection-between-bezier-curve-and-a-line-segment
    # 
    # The equation we're solving is 
    #
    # h00(t) p0 + h10(t) m0 + h01(t) p1 + h11(t) m1 = u + v t1
    #
    # where 
    #
    # h00(t) = 2t^3 - 3t^2 + 1
    # h10(t) = t^3 - 2t^2 + t
    # h01(t) = -2t^3 + 3t^2
    # h11(t) = t^3 - t^2
    # u = l1
    # v = l2-l1

    u = l1
    v = diff_points(l2, l1);

    # The first thing we do is to move u to the other side:
    #
    # h00(t) p0 + h10(t) m0 + h01(t) p1 + h11(t) m1 - u = v t1
    #
    # Then we're looking for matrix R that would turn (v t1) into
    # ({|v|, 0} t1). This is rotation of coordinate system matrix,
    # described at http://mathworld.wolfram.com/RotationMatrix.html
    #
    # R(h00(t) p0 + h10(t) m0 + h01(t) p1 + h11(t) m1 - u) = R(v t1) = {|v|, 0}t1
    #
    # We only care about R[1,0] and R[1,1] because it lets us solve
    # the equation for y coordinate where y == 0 (intersecting the
    # spline segment with the x axis of rotated coordinate
    # system). I'll call R[1,0] = R3 and R[1,1] = R4 . 

    v_abs = sqrt(v[0] ** 2 + v[1] ** 2)
    R1 =  X(v) / v_abs
    R2 =  Y(v) / v_abs
    R3 = -Y(v) / v_abs
    R4 =  X(v) / v_abs


    # The letters x and y are denoting x and y components of vectors
    # p0, p1, m0, m1, and u.

    p0x = p0[0]; p0y = p0[1]
    p1x = p1[0]; p1y = p1[1]
    m0x = m0[0]; m0y = m0[1]
    m1x = m1[0]; m1y = m1[1]
    ux = X(u); uy = Y(u)

    #
    #
    #   R3(h00(t) p0x + h10(t) m0x + h01(t) p1x + h11(t) m1x - ux) +
    # + R4(h00(t) p0y + h10(t) m0y + h01(t) p1y + h11(t) m1y - uy) = 0
    #
    # Opening all parentheses and simplifying for hxx we get:
    #
    #   h00(t) p0x R3 + h10(t) m0x R3 + h01(t) p1x R3 + h11(t) m1x R3 - ux R3 +
    # + h00(t) p0y R4 + h10(t) m0y R4 + h01(t) p1y R4 + h11(t) m1y R4 - uy R4 = 0
    # 
    #   h00(t) p0x R3 + h10(t) m0x R3 + h01(t) p1x R3 + h11(t) m1x R3 - ux R3 + 
    # + h00(t) p0y R4 + h10(t) m0y R4 + h01(t) p1y R4 + h11(t) m1y R4 - uy R4 = 0
    # 
    #   (1)
    #   h00(t) (p0x R3 + p0y R4) + h10(t) (m0x R3 + m0y R4) + 
    #   h01(t) (p1x R3 + p1y R4) + h11(t) (m1x R3 + m1y R4) - (ux R3 + uy R4) = 0
    #
    # We now introduce new substitution

    K00 = p0x * R3 + p0y * R4
    K10 = m0x * R3 + m0y * R4
    K01 = p1x * R3 + p1y * R4
    K11 = m1x * R3 + m1y * R4
    U = ux * R3 + uy * R4

    # Expressed in those terms, equation (1) above becomes
    #
    # h00(t) K00 + h10(t) K10 + h01(t) K01 + h11(t) K11 - U = 0
    #
    # We will now substitute the expressions for hxx(t) functions
    #
    # (2t^3 - 3t^2 + 1) K00 + (t^3 - 2t^2 + t) K10 + (-2t^3 + 3t^2) K01 + (t^3 - t^2) K11 - U = 0
    # 
    #   2 K00 t^3 - 3 K00 t^2 + K00 + 
    # + K10 t^3 - 2 K10 t^2 + K10 t - 
    # - 2 K01 t^3 + 3 K01 t^2 + 
    # + K11 t^3  - K11 t^2 - U = 0
    # 
    #   2 K00 t^3 - 3 K00 t^2 +    0t +  K00 
    # + K10   t^3 - 2 K10 t^2 + K10 t
    # - 2 K01 t^3 + 3 K01 t^2 
    # +   K11 t^3 -   K11 t^2 +    0t -   U = 0
    # 
    #  (2 K00 + K10 - 2K01 + K11) t^3 
    # +(-3 K00 - 2K10 + 3 K01 - K11) t^2
    # + K10 t
    # + K00 - U = 0
    # 
    # 
    # (2 K00 + K10 - 2K01 + K11) t^3 + (-3 K00 - 2K10 + 3 K01 - K11) t^2 + K10 t + K00 - U = 0
    #
    # All we need now is to solwe a cubic equation
    valuesOfT = cubicRoots((2 * K00 + K10 - 2 * K01 + K11),
                           (-3 * K00 - 2 * K10 + 3 * K01 - K11),
                           (K10),
                           K00 - U)
    # We can then put the values of it into our original spline segment
    # formula to find the potential intersection points.  Any point
    # that's on original line segment is an intersection

    def h00(t): return 2 * t**3 - 3 * t**2 + 1
    def h10(t): return t**3 - 2 * t**2 + t
    def h01(t): return -2 * t**3 + 3 * t**2
    def h11(t): return t**3 - t**2

    intersections = []
    for t in valuesOfT:
        if t < 0 or t > 1.0: continue
        # point = h00(t) * p0 + h10(t) * m0 + h01(t) * p1 + h11(t) * m1
        point = add_points(
            scale_point(h00(t), p0),
            scale_point(h10(t), m0),
            scale_point(h01(t), p1),
            scale_point(h11(t), m1)
            )

        if pointOnLineSegment(l1, l2, point): intersections.append(point)


    return intersections

def findIntersectionsManyCurves(p0_array, p1_array, m0_array, m1_array, u, v):
    result = [];
    for (p0, p1, m0, m1) in itertools.izip(p0_array, p1_array, m0_array, m1_array):
        result.extend(findIntersections(p0, p1, m0, m1, u, v))
    return result


def findIntersectionsManyCurvesManyLines(p0, p1, m0, m1, points):
    result = [];

    for (u,v) in itertools.izip(*[iter(points)]*2):
        result.extend(findIntersectionsManyCurves(p0, p1, m0, m1, u, v))

    return result

class EventsEmitter(object):
    def __init__(self):
        self.consumers = []

    def emit(self, eventName, *params):
        for method in self.consumers:
            funcName = method.im_func.func_name if hasattr(method, "im_func") else method.func_name
            if funcName == eventName:
                method(*params)
    def register(self, method):
        self.consumers.append(method)

    def unregister(self, method):
        self.consumers.remove(method)



class BunchOfPointsModel(EventsEmitter):
    def __init__(self):
        EventsEmitter.__init__(self)
        self.pts = []


    def points(self):
        return self.pts.__iter__()

    def pointsSequence(self):
        return tuple(self.pts)

    def have(self, point):
        return point in self.pts

    def addPoint(self,p):
        self.pts.append(p)
        self.emit("pointsChanged", p)

    def replacePoint(self, oldP, newP):
        idx = self.pts.index(oldP)
        self.pts[idx] = newP
        self.emit("pointsChanged", newP)


    def removePoint(self, p):
        self.point.remove(p)
        self.emit("pointsChanged", p)


class BunchOfPointsCompositeModel(object):
    def __init__(self, m1, m2):
        self.m1 = m1
        self.m2 = m2

    def points(self):
        return itertools.chain(self.m1.points(), self.m2.points())

    def have(self, point):
        return self.m1.have(point) or self.m2.have(point)


    def replacePoint(self, oldP, newP):
        if self.m1.have(oldP):
            self.m1.replacePoint(oldP, newP)
        else:
            self.m2.replacePoint(oldP, newP)

    def removePoint(self, p):
        if self.m1.have(p):
            self.m1.removePoint(p)
        else:
            self.m2.removePoint(p)

    def register(self, method):
        self.m1.register(method)
        self.m2.register(method)

    def unregister(self, method):
        self.m1.unregister(method)
        self.m2.unregister(method)

class BunchOfPointsDragController(EventsEmitter):
    def __init__(self, model):
        EventsEmitter.__init__(self)
        self.model = model
        self.draggedPoint = None

    def mouseMovedTo(self, x,y):
        if self.draggedPoint != None:
            newPoint = (x,y)
            draggedPoint = self.draggedPoint
            self.draggedPoint = newPoint
            self.model.replacePoint(draggedPoint, newPoint)
    def buttonDown(self, x,y):
        if self.draggedPoint == None:
            closePoint = self.getCloseEnoughPoint(x,y)
            if closePoint != None:
                self.draggedPoint = closePoint
                self.emit("dragPointChanged",closePoint)

    def buttonUp(self, x,y):
        self.mouseMovedTo(x,y)
        self.draggedPoint = None
        self.emit("dragPointChanged", None)

    def getCloseEnoughPoint(self, x,y):
        minSquareDistance = 25
        closestPoint = None
        for point in self.model.points():
            dx = X(point) - x
            dy = Y(point) - y
            distance = dx*dx + dy*dy
            if minSquareDistance > distance:
                closestPoint = point
                minSquareDistance = distance
        return closestPoint

    def isDraggedPoint(self, p):
        return p is self.draggedPoint

class CurvesLinesViewPointsView(object):
    def __init__(self, screen, modelCurves, modelLines, model, controller):
        self.screen = screen
        self.modelLines = modelLines
        self.modelCurves = modelCurves
        self.controller = controller
        controller.register(self.dragPointChanged)
        model.register(self.pointsChanged)

    def draw(self):
        self.screen.fill(Color("black"))
        pygame.draw.lines(self.screen, Color("cyan"), 0, self.modelLines.pointsSequence(), 3)
        (p0, p1, m0, m1) =  padlib.BezierCurve(screen,modelCurves.pointsSequence(),3,100,Color("magenta"))

        self.drawPointSet(self.modelCurves.points(),
                          lambda(p):self.controller.isDraggedPoint(p),
                          Color("white"), Color("red"))
        self.drawPointSet(self.modelLines.points(),
                          lambda(p):self.controller.isDraggedPoint(p),
                          Color("lightgray"), Color("red"))


        self.drawSimplePointSet(findIntersectionsManyCurvesManyLines(p0, p1, m0, m1,self.modelLines.points()),
                          Color("blue"))




    def drawSimplePointSet(self, points, normalColor):
        self.drawPointSet(points, lambda(p):True, None, normalColor);

    def drawPointSet(self, points, specialPoint, normalColor, specialColor):
        for p in points:
            if specialPoint(p):
                draw.circle(self.screen, specialColor, p, 6)
            else:
                draw.circle(self.screen, normalColor, p, 2)
        pygame.display.update()

    def dragPointChanged(self, p): self.draw()
    def pointsChanged(self, p): self.draw()


class PygameEventsDistributor(EventsEmitter):
    def __init__(self):
        EventsEmitter.__init__(self)
    def processEvent(self, e):
        if e.type == MOUSEMOTION:
            self.emit("mouseMovedTo", e.pos[0], e.pos[1])
        elif e.type == MOUSEBUTTONDOWN:
            self.emit("buttonDown", e.pos[0], e.pos[1])
        elif e.type == MOUSEBUTTONUP:
            self.emit("buttonUp", e.pos[0], e.pos[1])


modelLines = BunchOfPointsModel()
modelCurves = BunchOfPointsModel()
model = BunchOfPointsCompositeModel(modelLines, modelCurves);
controller = BunchOfPointsDragController(model)

distributor = PygameEventsDistributor()
distributor.register(controller.mouseMovedTo)
distributor.register(controller.buttonUp)
distributor.register(controller.buttonDown)

pygame.init()
screen = pygame.display.set_mode((640, 480))

modelCurves.addPoint((29,34))
modelCurves.addPoint((98,56))
modelCurves.addPoint((200, 293))
modelCurves.addPoint((350, 293))

modelLines.addPoint((23,123))
modelLines.addPoint((78,212))

view = CurvesLinesViewPointsView(screen, modelCurves, modelLines, model, controller)


keepGoing = True

try:
    while (keepGoing):
        for event in pygame.event.get():
            if event.type == QUIT:
                keepGoing = False
                break
            distributor.processEvent(event)
        pass
finally:
    pygame.quit()