Source

geoalg / geoalg / polybezier.py

#!/usr/bin/env python
#coding:utf-8
# Purpose:
# Created: 02.08.12
# Copyright (C) 2012, Manfred Moitzi
# License: GPLv3

from __future__ import division
from __future__ import print_function
from __future__ import unicode_literals
from __future__ import absolute_import

__author__ = "mozman <mozman@gmx.at>"

import math

from geoalg.nurbs import Bezier4P
from geoalg.vector2d import Vec2, NULLVEC
from geoalg.util import almost_equal_vectors
from geoalg import Ray2D

class PolyBezier(object):
    def __init__(self, points, start_angle=None, end_angle=None):
        self.start_angle = start_angle
        self.end_angle = end_angle
        self.breakpoints = [Vec2(p) for p in points]
        if self.nbreakpoints < 3:
            raise ValueError("3 or more points required.")

        self.tangents = self._default_tangents()
        self.bezier_part_lengths = None
        self.length = None
        self.update()

    def update(self):
        """ Call is required after modifying tangents or breakpoints.
        """
        self.bezier_part_lengths = list(self._bezier_part_lengths())
        self.length = sum(self.bezier_part_lengths)

    @property
    def nbreakpoints(self):
        return len(self.breakpoints)

    def _default_tangents(self):
        def adjacent_lines(i):
            p1 = self.breakpoints[i-1]
            p2 = self.breakpoints[i]
            p3 = self.breakpoints[i+1]
            l1 = Vec2.from_points(p1, p2)
            l2 = Vec2.from_points(p2, p3)
            return l1, l2

        def default_tangent(i):
            l1, l2 = adjacent_lines(i)
            u = (l1 + l2).normalized()
            left = u * l1.length() / -4.
            right = u * l2.length() / 4.
            return Tangent(left, right)

        def first_tangent():
            length = abs(tangents[1].left)
            if self.start_angle:
                angle = self.start_angle
            else:
                angle = tangents[1].left.angle() + math.pi
            return Tangent(NULLVEC, Vec2.from_angle(angle, length))

        def last_tangent():
            length = abs(tangents[-1].right)
            if self.end_angle:
                angle = self.end_angle
            else:
                angle = tangents[-1].right.angle() + math.pi
            return Tangent(Vec2.from_angle(angle, length), NULLVEC)

        tangents = [Tangent(NULLVEC, NULLVEC)]
        for i in range(1, len(self.breakpoints)-1):
            tangents.append(default_tangent(i))

        tangents[0] = first_tangent()
        tangents.append(last_tangent())
        return tangents

    def _bezier_part_lengths(self, segments=100):
        for bezier_part in self.iter_bezier_parts():
            yield bezier_part.approximated_length(segments)

    def iter_bezier_parts(self):
        for i in  range(self.nbreakpoints-1):
            yield self.bezier_part(i)

    def bezier_part(self, num):
        if num < 0: # -1 for last part, like python list
            num += self.nbreakpoints - 1
        p1 = self.breakpoints[num]
        p4 = self.breakpoints[num+1]
        p2 = p1 + self.tangents[num].right
        p3 = p4 + self.tangents[num+1].left
        return Bezier4P((p1, p2, p3, p4))

    def approximate(self, segments=20):
        """ Calculate curve points.

        :param int segments: segment count for each bezier part.
        """
        points = [self.breakpoints[0]]
        for bezier in self.iter_bezier_parts():
            pts = bezier.approximate(segments)
            next(pts) # skip first point, because equals last point of prev part
            points.extend(pts)
        return points

    def find_position(self, p, passes=1, segments=100, epsilon=0.00001):
        """ Find curve position near point p.

        :param 2-tuple p: curve position
        :param int passes: count of search passes
        :param float epsilon: precision (min distance of result position)
        """
        def _find_position(startpos, endpos):
            """ Find curve position near point p in range from  startpos to endpos.
            """
            def interpolate(start, end):
                if start > end:
                    start, end = end, start
                ps = Vec2(self.get_point(start))
                pe = Vec2(self.get_point(end))
                ray1 = Ray2D(ps, pe)
                ip =  ray1.intersect(ray1.normal_through(p))
                a = ps.distance(ip)
                b = pe.distance(ip)
                return start + (end - start) * (a / (a + b))

            def curve_distances(startpos, delta):
                for i in range(segments+1):
                    position = startpos + i * delta
                    curve_point = Vec2(self.get_point(position))
                    yield p.distance(curve_point), position

            if startpos > endpos:
                startpos, endpos = endpos, startpos

            delta = (endpos - startpos) / segments
            start, end = sorted(curve_distances(startpos, delta))[:2]
            if start[0] < epsilon:
                return start[1], 0.
            else:
                return interpolate(start[1], end[1]), abs(start[1] - end[1])

        p = Vec2(p)
        startpos = 0.
        endpos = self.length
        passes = max(1, passes)
        while passes:
            pos, distance_of_best_results = _find_position(startpos, endpos)
            if distance_of_best_results > 0.: #refine search
                passes -= 1
                d2 = 2 * distance_of_best_results
                startpos = max(pos - d2, 0.)
                endpos = min(pos + d2, self.length)
            else: # distance get_point(pos) to p < epsilon
                break
        return pos

    def find_part(self, position):
        """ Find bezier curve part associated to curve position.

        :param float position: in range [0 .. self.length]
        :returns tuple: (bezier_part, t) t in range of [0 .. 1]
        """
        end_of_part = 0.0
        for part_num, part_length in enumerate(self.bezier_part_lengths):
            start_of_part = end_of_part
            end_of_part += part_length
            if start_of_part <= position <= end_of_part:
                t = (position - start_of_part) / part_length
                return self.bezier_part(part_num), min(t, 1.0)
        raise ValueError('position out of range')

    def get_point(self, position):
        """ Get curve point at position.

        :param float position: in range [0 .. self.length]
        :returns: Vec2
        """
        # no caching, because breakpoints and tangents might be changing in lifetime,
        # or reset cache in method update()
        bezier, t = self.find_part(position)
        # 0 <= t <= 1.
        return bezier.get_point(t)

    def get_tangent(self, position):
        """Get curve tangent at point position.

        :param float position: in range [0 .. self.length]
        :returns: Vec2
        """
        # no caching, because breakpoints and tangents might be changing in lifetime,
        # or reset cache in method update()
        bezier, t = self.find_part(position)
        # 0 <= t <= 1.
        return bezier.get_tangent(t)

class Tangent(object):
    def __init__(self, left, right, align=True):
        self._left = Vec2(left)
        self._right = Vec2(right)
        self.align = align

        if align: # align right tangent
            self.left = self._left

    @property
    def left(self):
        return self._left

    @left.setter
    def left(self, vector):
        self._left = Vec2(vector)
        if self.align and not self._left.isNULL:
            self._right = abs(self._right) * -self._left.normalized()

    @property
    def right(self):
        return self._right

    @right.setter
    def right(self, vector):
        self._right = Vec2(vector)
        if self.align and not self._right.isNULL:
            self._left = abs(self._left) * -self._right.normalized()

    def isaligned(self, places=6):
        return almost_equal_vectors(self._left.normalized(), -self._right.normalized(), places)

    def rotate_left(self, angle):
        sum_angle = self._left.angle() + angle
        self.left = Vec2.from_angle(sum_angle, abs(self._left))

    def rotate_right(self, angle):
        sum_angle = self._right.angle() + angle
        self.right = Vec2.from_angle(sum_angle, abs(self._right))

    def set_left_angle(self, angle):
        self.left = Vec2.from_angle(angle, abs(self._left))

    def set_right_angle(self, angle):
        self.right = Vec2.from_angle(angle, abs(self._right))

    def set_length(self, left=None, right=None):
        if left:
            self._left = self._left.normalized() * float(left)
        if right:
            self._right = self._right.normalized() * float(right)