Bezier.py

####################################################################################################
#
# Patro - A Python library to make patterns for fashion design
# Copyright (C) 2017 Fabrice Salvaire
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.
#
####################################################################################################

r"""Module to implement Bézier curve.

Definitions
-----------

A Bézier curve is defined by a set of control points :math:`\mathbf{P}_0` through
:math:`\mathbf{P}_n`, where :math:`n` is called its order (:math:`n = 1` for linear, 2 for
quadratic, 3 for cubic etc.). The first and last control points are always the end points of the
curve;

In the following :math:`0 \le t \le 1`.

Linear Bézier Curves
---------------------

Given distinct points :math:`\mathbf{P}_0` and :math:`\mathbf{P}_1`, a linear Bézier curve is simply
a straight line between those two points. The curve is given by

.. math::
    \begin{align}
    \mathbf{B}(t) &= \mathbf{P}_0 + t (\mathbf{P}_1 - \mathbf{P}_0) \\
                  &= (1-t) \mathbf{P}_0 + t \mathbf{P}_1
    \end{align}

and is equivalent to linear interpolation.

Quadratic Bézier Curves
-----------------------

A quadratic Bézier curve is the path traced by the function :math:`\mathbf{B}(t)`, given points
:math:`\mathbf{P}_0`, :math:`\mathbf{P}_1`, and :math:`\mathbf{P}_2`,

.. math::
    \mathbf{B}(t) = (1 - t)[(1 - t) \mathbf{P}_0 + t \mathbf{P}_1] + t [(1 - t) \mathbf{P}_1 + t \mathbf{P}_2]

which can be interpreted as the linear interpolant of corresponding points on the linear Bézier
curves from :math:`\mathbf{P}_0` to :math:`\mathbf{P}_1` and from :math:`\mathbf{P}_1` to
:math:`\mathbf{P}_2` respectively.

Rearranging the preceding equation yields:

.. math::
    \begin{align}
    \mathbf{B}(t) &= (1 - t)^{2} \mathbf{P}_0 + 2(1 - t)t \mathbf{P}_1 + t^{2} \mathbf{P}_2 \\
                  &= (\mathbf{P}_0 - 2\mathbf{P}_1 + \mathbf{P}_2) t^2 +
                     (-2\mathbf{P}_0 + 2\mathbf{P}_1) t +
                     \mathbf{P}_0
    \end{align}

This can be written in a way that highlights the symmetry with respect to :math:`\mathbf{P}_1`:

.. math::
    \mathbf{B}(t) = \mathbf{P}_1 + (1 - t)^{2} ( \mathbf{P}_0 - \mathbf{P}_1) + t^{2} (\mathbf{P}_2 - \mathbf{P}_1)

Which immediately gives the derivative of the Bézier curve with respect to `t`:

.. math::
    \mathbf{B}'(t) = 2(1 - t) (\mathbf{P}_1 - \mathbf{P}_0) + 2t (\mathbf{P}_2 - \mathbf{P}_1)

from which it can be concluded that the tangents to the curve at :math:`\mathbf{P}_0` and
:math:`\mathbf{P}_2` intersect at :math:`\mathbf{P}_1`. As :math:`t` increases from 0 to 1, the
curve departs from :math:`\mathbf{P}_0` in the direction of :math:`\mathbf{P}_1`, then bends to
arrive at :math:`\mathbf{P}_2` from the direction of :math:`\mathbf{P}_1`.

The second derivative of the Bézier curve with respect to :math:`t` is

.. math::
    \mathbf{B}''(t) = 2 (\mathbf{P}_2 - 2 \mathbf{P}_1 + \mathbf{P}_0)

Cubic Bézier Curves
-------------------

Four points :math:`\mathbf{P}_0`, :math:`\mathbf{P}_1`, :math:`\mathbf{P}_2` and
:math:`\mathbf{P}_3` in the plane or in higher-dimensional space define a cubic Bézier curve.  The
curve starts at :math:`\mathbf{P}_0` going toward :math:`\mathbf{P}_1` and arrives at
:math:`\mathbf{P}_3` coming from the direction of :math:`\mathbf{P}_2`. Usually, it will not pass
through :math:`\mathbf{P}_1` or :math:`\mathbf{P}_2`; these points are only there to provide
directional information. The distance between :math:`\mathbf{P}_1` and :math:`\mathbf{P}_2`
determines "how far" and "how fast" the curve moves towards :math:`\mathbf{P}_1` before turning
towards :math:`\mathbf{P}_2`.

Writing :math:`\mathbf{B}_{\mathbf P_i,\mathbf P_j,\mathbf P_k}(t)` for the quadratic Bézier curve
defined by points :math:`\mathbf{P}_i`, :math:`\mathbf{P}_j`, and :math:`\mathbf{P}_k`, the cubic
Bézier curve can be defined as an affine combination of two quadratic Bézier curves:

.. math::
    \mathbf{B}(t) = (1-t) \mathbf{B}_{\mathbf P_0,\mathbf P_1,\mathbf P_2}(t) +
                        t \mathbf{B}_{\mathbf P_1,\mathbf P_2,\mathbf P_3}(t)

The explicit form of the curve is:

.. math::
    \begin{align}
    \mathbf{B}(t) &= (1-t)^3 \mathbf{P}_0 + 3(1-t)^2t \mathbf{P}_1 + 3(1-t)t^2 \mathbf{P}_2 + t^3\mathbf{P}_3 \\
                  &= (\mathbf{P}_3 - 3\mathbf{P}_2 + 3\mathbf{P}_1 - \mathbf{P}_0) t^3 +
                     3(\mathbf{P}_2 - 2\mathbf{P}_1 + \mathbf{P}_0) t^2 +
                     3(\mathbf{P}_1 - \mathbf{P}_0) t  +
                     \mathbf{P}_0
    \end{align}

For some choices of :math:`\mathbf{P}_1` and :math:`\mathbf{P}_2` the curve may intersect itself, or
contain a cusp.

The derivative of the cubic Bézier curve with respect to :math:`t` is

.. math::
    \mathbf{B}'(t) = 3(1-t)^2 (\mathbf{P}_1 - \mathbf{P}_0) + 6(1-t)t (\mathbf{P}_2 - \mathbf{P}_1) + 3t^2 (\mathbf{P}_3 - \mathbf{P}_2)

The second derivative of the Bézier curve with respect to :math:`t` is

.. math::
    \mathbf{B}''(t) = 6(1-t) (\mathbf{P}_2 - 2 \mathbf{P}_1 + \mathbf{P}_0) +  6t (\mathbf{P}_3 - 2 \mathbf{P}_2 + \mathbf{P}_1)

Recursive definition
--------------------

A recursive definition for the Bézier curve of degree :math:`n` expresses it as a point-to-point
linear combination of a pair of corresponding points in two Bézier curves of degree :math:`n-1`.

Let :math:`\mathbf{B}_{\mathbf{P}_0\mathbf{P}_1\ldots\mathbf{P}_n}` denote the Bézier curve
determined by any selection of points :math:`\mathbf{P}_0`, :math:`\mathbf{P}_1`, :math:`\ldots`,
:math:`\mathbf{P}_{n-1}`.

The recursive definition is

.. math::
    \begin{align}
    \mathbf{B}_{\mathbf{P}_0}(t) &= \mathbf{P}_0 \\[1em]
    \mathbf{B}(t) &= \mathbf{B}_{\mathbf{P}_0\mathbf{P}_1\ldots\mathbf{P}_n}(t) \\
                  &= (1-t) \mathbf{B}_{\mathbf{P}_0\mathbf{P}_1\ldots\mathbf{P}_{n-1}}(t) +
                         t \mathbf{B}_{\mathbf{P}_1\mathbf{P}_2\ldots\mathbf{P}_n}(t)
    \end{align}

The formula can be expressed explicitly as follows:

.. math::
    \begin{align}
    \mathbf{B}(t) &= \sum_{i=0}^n b_{i,n}(t) \mathbf{P}_i \\
                  &= \sum_{i=0}^n {n\choose i}(1 - t)^{n - i}t^i \mathbf{P}_i \\
                  &= (1 - t)^n \mathbf{P}_0 +
                     {n\choose 1}(1 - t)^{n - 1}t \mathbf{P}_1 +
                     \cdots +
                     {n\choose n - 1}(1 - t)t^{n - 1} \mathbf{P}_{n - 1} +
                     t^n \mathbf{P}_n
    \end{align}

where :math:`b_{i,n}(t)` are the Bernstein basis polynomials of degree :math:`n` and :math:`n
\choose i` are the binomial coefficients.

Degree elevation
----------------

A Bézier curve of degree :math:`n` can be converted into a Bézier curve of degree :math:`n + 1` with
the same shape.

To do degree elevation, we use the equality

.. math::
       \mathbf{B}(t) = (1-t) \mathbf{B}(t) + t \mathbf{B}(t)

Each component :math:`\mathbf{b}_{i,n}(t) \mathbf{P}_i` is multiplied by :math:`(1-t)` and
:math:`t`, thus increasing a degree by one, without changing the value.

For arbitrary :math:`n`, we have

.. math::
    \begin{align}
    \mathbf{B}(t) &= (1 - t) \sum_{i=0}^n \mathbf{b}_{i,n}(t) \mathbf{P}_i +
                           t \sum_{i=0}^n \mathbf{b}_{i,n}(t) \mathbf{P}_i \\
                  &= \sum_{i=0}^n \frac{n + 1 - i}{n + 1} \mathbf{b}_{i, n + 1}(t) \mathbf{P}_i +
                     \sum_{i=0}^n \frac{i + 1}{n + 1} \mathbf{b}_{i + 1, n + 1}(t) \mathbf{P}_i \\
                  &= \sum_{i=0}^{n + 1} \mathbf{b}_{i, n + 1}(t)
                                        \left(\frac{i}{n + 1}         \mathbf{P}_{i - 1} +
                                              \frac{n + 1 - i}{n + 1} \mathbf{P}_i\right) \\
                  &= \sum_{i=0}^{n + 1} \mathbf{b}_{i, n + 1}(t) \mathbf{P'}_i
    \end{align}

Therefore the new control points are

.. math::
      \mathbf{P'}_i = \frac{i}{n + 1} \mathbf{P}_{i - 1} + \frac{n + 1 - i}{n + 1} \mathbf{P}_i

It introduces two arbitrary points :math:`\mathbf{P}_{-1}` and :math:`\mathbf{P}_{n+1}` which are
cancelled in :math:`\mathbf{P'}_i`.

Matrix Forms
------------

.. math::
     \mathbf{B}(t) = \mathbf{Transformation} \; \mathbf{Control} \; \mathbf{Basis} \; \mathbf{T}(t)

.. math::
     \begin{align}
     \mathbf{B^2}(t) &= \mathbf{Tr}
                        \begin{pmatrix}
                          P_{1x} & P_{2x} & P_{3x} \\
                          P_{1y} & P_{2x} & P_{3x} \\
                          1      & 1      & 1
                        \end{pmatrix}
                        \begin{pmatrix}
                          1 & -2 &  1 \\
                          0 &  2 & -2 \\
                          0 &  0 &  1
                        \end{pmatrix}
                        \begin{pmatrix}
                          1 \\
                          t \\
                          t^2
                        \end{pmatrix} \\[1em]
     \mathbf{B^3}(t) &= \mathbf{Tr}
                        \begin{pmatrix}
                          P_{1x} & P_{2x} & P_{3x} & P_{4x} \\
                          P_{1y} & P_{2x} & P_{3x} & P_{4x} \\
                          0      & 0      & 0      & 0      \\
                          1      & 1      & 1      & 1
                        \end{pmatrix}
                        \begin{pmatrix}
                          1 & -3 &  3 & -1 \\
                          0 &  3 & -6 &  3 \\
                          0 &  0 &  3 & -3 \\
                          0 &  0 &  0 &  1
                        \end{pmatrix}
                        \begin{pmatrix}
                          1 \\
                          t \\
                          t^2 \\
                          t^3
                        \end{pmatrix}
    \end{align}

.. B(t) = P0 (1 - 2t + t^2) +
          P1 (    2t - t^2) +
          P2           t^2

Symbolic Calculation
--------------------

.. code-block:: py3

    >>> from sympy import *

    >>> P0, P1, P2, P3, P, t = symbols('P0 P1 P2 P3 P t')

    >>> B2 = (1-t)*((1-t)*P0 + t*P1) + t*((1-t)*P1 + t*P2)
    >>> collect(expand(B2), t)
    P0 + t**2*(P0 - 2*P1 + P2) + t*(-2*P0 + 2*P1)

    >>> B2_012 = (1-t)*((1-t)*P0 + t*P1) + t*((1-t)*P1 + t*P2)
    >>> B2_123 = (1-t)*((1-t)*P1 + t*P2) + t*((1-t)*P2 + t*P3)
    >>> B3 = (1-t)*B2_012 + t*B2_123
    >>> collect(expand(B2), t)
    P0 + t**3*(-P0 + 3*P1 - 3*P2 + P3) + t**2*(3*P0 - 6*P1 + 3*P2) + t*(-3*P0 + 3*P1)

"""

# Fixme:
#   max distance to the chord for linear approximation
#   fitting

# C0 = continuous
# G1 = geometric continuity
#    Tangents point to the same direction
# C1 = parametric continuity
#    Tangents are the same, implies G1
# C2 = curvature continuity
#    Tangents and their derivatives are the same

####################################################################################################

__all__ = [
    'QuadraticBezier2D',
    'CubicBezier2D',
]

####################################################################################################

from math import log, sqrt

import numpy as np

from Patro.Common.Math.Root import quadratic_root, cubic_root, fifth_root
from .Interpolation import interpolate_two_points
from .Line import Line2D
from .Primitive import Primitive3P, Primitive4P, PrimitiveNP, Primitive2DMixin
from .Transformation import AffineTransformation
from .Vector import Vector2D

####################################################################################################

class BezierMixin2D(Primitive2DMixin):

    """Mixin to implements 2D Bezier Curve."""

    LineInterpolationPrecision = 0.05

    ##############################################

    def interpolated_length(self, dt=None):

        """Length of the curve obtained via line interpolation"""

        if dt is None:
            dt = self.LineInterpolationPrecision / (self.end_point - self.start_point).magnitude

        length = 0
        t = 0
        while t < 1:
            t0 = t
            t = min(t + dt, 1)
            length += (self.point_at_t(t) - self.point_at_t(t0)).magnitude

        return length

    ##############################################

    def length_at_t(self, t, cache=False):

        """Compute the length of the curve at *t*."""

        if cache: # lookup cache
            if not hasattr(self, '_length_cache'):
                self._length_cache = {}
            length = self._length_cache.get(t, None)
            if length is not None:
                return length

        length = self.split_at_t(t).length

        if cache: # save
            self._length_cache[t] = length

        return length

    ##############################################

    def t_at_length(self, length, precision=1e-6):

        """Compute t for the given length. Length must lie in [0, curve length] range]. """

        if length < 0:
            raise ValueError('Negative length')
        if length == 0:
            return 0
        curve_length = self.length # Fixme: cache ?
        if (curve_length - length) <= precision:
            return 1
        if length > curve_length:
            raise ValueError('Out of length')

        # Search t for length using dichotomy
        # convergence rate :
        #  10 iterations corresponds to curve length / 1024
        #  16                                        / 65536
        # start range
        inf = 0
        sup = 1
        while True:
            middle = (sup + inf) / 2
            length_at_middle = self.length_at_t(middle, cache=True) # Fixme: out of memory, use LRU ???
            # exit condition
            if abs(length_at_middle - length) <= precision:
                return middle
            elif length_at_middle < length:
                inf = middle
            else: # length < length_at_middle
                sup = middle

    ##############################################

    def split_at_two_t(self, t1, t2):

        if t1 == t2:
            return self.point_at_t(t1)

        if t2 < t1:
            # Fixme: raise ?
            t1, t2 = t2, t1

        # curve = self
        # if t1 > 0:
        curve = self.split_at_t(t1)[1] # right
        if t2 < 1:
            # Interpolate the parameter at t2 in the new curve
            t = (t2 - t1) / (1 - t1)
            curve = curve.split_at_t(t)[0] # left

        return curve

    ##############################################

    def _map_to_line(self, line):

        transformation = AffineTransformation.Rotation(-line.v.orientation)
        # Fixme: use __vector_cls__
        transformation *= AffineTransformation.Translation(Vector2D(0, -line.p.y))
        # Fixme: better API ?
        return self.clone().transform(transformation)

    ##############################################

    def non_parametric_curve(self, line):

        """Return the non-parametric Bezier curve D(ti, di(t)) where di(t) is the distance of the curve from
        the baseline of the fat-line, ti is equally spaced in [0, 1].

        """

        ts = np.arange(0, 1, 1/(self.number_of_points-1))
        distances = [line.distance_to_line(p) for p in self.points]
        points = [Vector2D(t, d) for t, f in zip(ts, distances)]
        return self.__class__(*points)

    ##############################################

    def distance_to_point(self, point):

        p = self.closest_point(point)
        if p is not None:
            return (point - p).magnitude
        else:
            return None

####################################################################################################

class QuadraticBezier2D(BezierMixin2D, Primitive3P):

    """Class to implements 2D Quadratic Bezier Curve."""

    BASIS = np.array((
        (1, -2,  1),
        (0,  2, -2),
        (0,  0,  1),
    ))

    INVERSE_BASIS = np.array((
        (-2,  1, -2),
        (-1, -3,  1),
        (-1, -1, -2),
    ))

    ##############################################

    def __init__(self, p0, p1, p2):
        Primitive3P.__init__(self, p0, p1, p2)

    ##############################################

    def __repr__(self):
        return self.__class__.__name__ + '({0._p0}, {0._p1}, {0._p2})'.format(self)

    ##############################################

    @property
    def length(self):

        r"""Compute the length of the curve.

        Algorithm

          * http://www.gamedev.net/topic/551455-length-of-a-generalized-quadratic-bezier-curve-in-3d
          * Dave Eberly Posted October 25, 2009

        The quadratic Bezier is

        .. math::
            \mathbf{B}(t) = (1-t)^2 \mathbf{P}_0 + 2t(1-t) \mathbf{P}_1 + t^2 \mathbf{P}_2

        The derivative is

        .. math::
            \mathbf{B'}(t) = -2(1-t) \mathbf{P}_0 + (2-4t) \mathbf{P}_1 + 2t \mathbf{P}_2

        The length of the curve for :math:`0 <= t <= 1` is

        .. math::
            \int_0^1 \sqrt{(x'(t))^2 + (y'(t))^2} dt

        The integrand is of the form :math:`\sqrt{c t^2 + b t + a}`

        You have three separate cases: :math:`c = 0`, :math:`c > 0`, or :math:`c < 0`.

        The case :math:`c = 0` is easy.

        For the case :math:`c > 0`, an antiderivative is

        .. math::
            \frac{2ct + b}{4c} \sqrt{ct^2 + bt + a}  +  \frac{k}{2\sqrt{c}} \ln{\left(2\sqrt{c(ct^2 + bt + a)} + 2ct + b\right)}

        For the case :math:`c < 0`, an antiderivative is

        .. math::
            \frac{2ct + b}{4c} \sqrt{ct^2 + bt + a}  -  \frac{k}{2\sqrt{-c}} \arcsin{\frac{2ct + b}{\sqrt{-q}}}

        where :math:`k = \frac{4c}{q}` with :math:`q = 4ac - b^2`.

        """

        A0 = self._p1 - self._p0
        A1 = self._p0 - self._p1 * 2 + self._p2
        if A1.magnitude_square != 0:
            c = 4 * A1.dot(A1)
            b = 8 * A0.dot(A1)
            a = 4 * A0.dot(A0)
            q = 4 * a * c - b * b
            two_cb = 2 * c + b
            sum_cba = c + b + a
            m0 = 0.25 / c
            m1 = q / (8 * c**1.5)
            return (m0 * (two_cb * sqrt(sum_cba) - b * sqrt(a)) +
                    m1 * (log(2 * sqrt(c * sum_cba) + two_cb) - log(2 * sqrt(c * a) + b)))
        else:
            return 2 * A0.magnitude

    ##############################################

    def point_at_t(self, t):
        # if 0 < t or 1 < t:
        #     raise ValueError()
        u = 1 - t
        return self._p0 * u**2 + self._p1 * 2 * t * u + self._p2 * t**2

    ##############################################

    def split_at_t(self, t):

        """Split the curve at given position"""

        if t <= 0:
            return None, self
        elif t >= 1:
            return self, None
        else:
            p01 = interpolate_two_points(self._p0, self._p1, t)
            p12 = interpolate_two_points(self._p1, self._p2, t)
            p = interpolate_two_points(p01, p12, t) # p = p012
            # p = self.point_at_t(t)
            return (QuadraticBezier2D(self._p0, p01, p), QuadraticBezier2D(p, p12, self._p2))

   ##############################################

    @property
    def tangent0(self):
        return (self._p1 - self._p0).normalise()

    ##############################################

    @property
    def tangent1(self):
        return (self._p2 - self._p1).normalise()

    ##############################################

    @property
    def normal0(self):
        return self.tangent0.normal()

    ##############################################

    @property
    def tangent1(self):
        return self.tangent1.normal()

    ##############################################

    def tangent_at(self, t):
        u = 1 - t
        return (self._p1 - self._p0) * u + (self._p2 - self._p1) * t

    ##############################################

    def intersect_line(self, line):

        """Find the intersections of the curve with a line.

        Algorithm

        * Apply a transformation to the curve that maps the line onto the X-axis.
        * Then we only need to test the Y-values for a zero.

        """

        # u = 1 - t
        # B = p0 * u**2  +  p1 * 2*t*u  +  p2 * t**2
        # collect(expand(B), t)
        # solveset(B, t)

        curve = self._map_to_line(line)

        p0 = curve.p0.y
        p1 = curve.p1.y
        p2 = curve.p2.y

        return quadratic_root(
            p2 - 2*p1 + p0, # t**2
            2*(p1 - p0),    # t
            p0,
        )

        ### a = p0 - 2*p1 + p2 # t**2
        ### # b = 2*(-p0 + p1) # t
        ### b = -p0 + p1 # was / 2 !!!
        ### c = p0
        ###
        ### # discriminant = b**2 - 4*a*c
        ### # discriminant = 4 * (p1**2 - p0*p2)
        ### discriminant = p1**2 - p0*p2 # was / 4 !!!
        ###
        ### if discriminant < 0:
        ###     return None
        ### elif discriminant == 0:
        ###     return -b / a # dropped 2
        ### else:
        ###      # dropped 2
        ###     y1 = (-b - sqrt(discriminant)) / a
        ###     y2 = (-b + sqrt(discriminant)) / a
        ###     return y1, y2

    ##############################################

    def fat_line(self):

        line = Line2D.from_two_points(self._p0, self._p3)
        d1 = line.distance_to_line(self._p1)
        d_min = min(0, d1 / 2)
        d_max = max(0, d1 / 2)

        return (line, d_min, d_max)

    ##############################################

    def closest_point(self, point):

        """Return the closest point on the curve to the given *point*.

        Reference

        * https://hal.archives-ouvertes.fr/inria-00518379/document
          Improved Algebraic Algorithm On Point Projection For Bézier Curves
          Xiao-Diao Chen, Yin Zhou, Zhenyu Shu, Hua Su, Jean-Claude Paul

        """

        # Condition:
        #   (P - B(t)) . B'(t) = 0   where t in [0,1]
        #
        #   P. B'(t) - B(t). B'(t) = 0

        # A = P1 - P0
        # B = P2 - P1 - A
        # M = P0 - P

        # Q(t)  = P0*(1-t)**2 + P1*2*t*(1-t) + P2*t**2
        # Q'(t) = -2*P0*(1 - t) + 2*P1*(1 - 2*t) + 2*P2*t
        #       = 2*(A + B*t)

        # Q = P0 * (1-t)**2  +  P1 * 2*t*(1-t)  +  P2 * t**2
        # Qp = simplify(Q.diff(t))
        # collect(expand((P*Qp - Q*Qp)/-2), t)

        # (P0**2 - 4*P0*P1 + 2*P0*P2 + 4*P1**2 - 4*P1*P2 + P2**2) * t**3
        # (-3*P0**2 + 9*P0*P1 - 3*P0*P2 - 6*P1**2 + 3*P1*P2) * t**2
        # (-P*P0 + 2*P*P1 - P*P2 + 3*P0**2 - 6*P0*P1 + P0*P2 + 2*P1**2) * t
        # P*P0 - P*P1 - P0**2 + P0*P1

        # factorisation
        # (P0 - 2*P1 + P2)**2 * t**3
        # 3*(P1 - P0)*(P0 - 2*P1 + P2) * t**2
        # ...
        # (P0 - P)*(P1 - P0)

        # B**2 * t**3
        # 3*A*B * t**2
        # (2*A**2 + M*B) * t
        # M*A

        A = self._p1 - self._p0
        B = self._p2 - self._p1 - A
        M = self._p0 - point

        roots = cubic_root(
            B.magnitude_square,
            3*A.dot(B),
            2*A.magnitude_square + M.dot(B),
            M.dot(A),
        )
        t = [root for root in roots if 0 <= root <= 1]
        if not t:
            return None
        elif len(t) > 1:
            # Fixme: crash application !!!
            raise NameError("Found more than one root: {}".format(t))
        else:
            return self.point_at_t(t)

    ##############################################

    def to_cubic(self):

        r"""Elevate the quadratic Bézier curve to a cubic Bézier cubic with the same shape.

        The new control points are

        .. math::
            \begin{align}
            \mathbf{P'}_0 &= \mathbf{P}_0 \\
            \mathbf{P'}_1 &= \mathbf{P}_0 + \frac{2}{3} (\mathbf{P}_1 - \mathbf{P}_0) \\
            \mathbf{P'}_1 &= \mathbf{P}_2 + \frac{2}{3} (\mathbf{P}_1 - \mathbf{P}_2) \\
            \mathbf{P'}_2 &= \mathbf{P}_2
            \end{align}

        """

        p1 = (self._p0 + self._p1 * 2) / 3
        p2 = (self._p2 + self._p1 * 2) / 3

        return CubicBezier2D(self._p0, p1, p2, self._p3)

####################################################################################################

_Sqrt3 = sqrt(3)
_Div18Sqrt3 = 18 / _Sqrt3
_OneThird = 1 / 3
_Sqrt3Div36 = _Sqrt3 / 36

class CubicBezier2D(BezierMixin2D, Primitive4P):

    """Class to implements 2D Cubic Bezier Curve."""

    InterpolationPrecision = 0.001

    BASIS = np.array((
        (1, -3,  3, -1),
        (0,  3, -6,  3),
        (0,  0,  3, -3),
        (0,  0,  0,  1),
    ))

    INVERSE_BASIS = np.array((
        (1,    1,   1, 1),
        (0,  1/3, 2/3, 1),
        (0,    0, 1/3, 1),
        (0,    0,   0, 1),
    ))

    #######################################

    def __init__(self, p0, p1, p2, p3):
        Primitive4P.__init__(self, p0, p1, p2, p3)

    ##############################################

    def __repr__(self):
        return self.__class__.__name__ + '({0._p0}, {0._p1}, {0._p2}, {0._p3})'.format(self)

    ##############################################

    def to_spline(self):
        from .Spline import CubicUniformSpline2D
        basis = np.dot(self.BASIS, CubicUniformSpline2D.INVERSE_BASIS)
        points = np.dot(self.point_array, basis).transpose()
        return CubicUniformSpline2D(*points)

    ##############################################

    @property
    def length(self):
        return self.adaptive_length_approximation()

    ##############################################

    def point_at_t(self, t):
        # if 0 < t or 1 < t:
        #     raise ValueError()
        return (self._p0 +
                (self._p1 - self._p0) * 3 * t  +
                (self._p2 - self._p1*2 + self._p0) * 3 * t**2 +
                (self._p3 - self._p2*3 + self._p1*3 - self._p0) * t**3)

    # interpolate = point_at_t

    ##############################################

    def _q_point(self):
        """Return the control point for mid-point quadratic approximation"""
        return (self._p2*3 - self._p3 + self._p1*3 - self._p0) / 4

    ##############################################

    def mid_point_quadratic_approximation(self):
        """Return the mid-point quadratic approximation"""
        p1 = self._q_point()
        return QuadraticBezier2D(self._p0, p1, self._p3)

    ##############################################

    def split_at_t(self, t):

        """Split the curve at given position"""

        p01 = interpolate_two_points(self._p0, self._p1, t)
        p12 = interpolate_two_points(self._p1, self._p2, t)
        p23 = interpolate_two_points(self._p2, self._p3, t)
        p012 = interpolate_two_points(p01, p12, t)
        p123 = interpolate_two_points(p12, p23, t)
        p = interpolate_two_points(p012, p123, t) # p = p0123
        # p = self.point_at_t(t)

        return (CubicBezier2D(self._p0, p01, p012, p), CubicBezier2D(p, p123, p23, self._p3))

    ##############################################

    def _d01(self):
        """Return the distance between 0 and 1 quadratic aproximations"""
        return (self._p3 - self._p2 * 3 + self._p1 * 3 - self._p0).magnitude / 2

    ##############################################

    def _t_max(self):
        """Return the split point for adaptive quadratic approximation"""
        return (_Div18Sqrt3 * self.InterpolationPrecision / self._d01())**_OneThird

    ##############################################

    def q_length(self):
        """Return the length of the mid-point quadratic approximation"""
        return self.mid_point_quadratic_approximation().length

    ##############################################

    def adaptive_length_approximation(self):

        """Return the length of the adaptive quadratic approximation"""

        segments = []
        segment = self
        t_max = segment._t_max()
        while t_max < 1:
            split = segment.split_at_t(t_max)
            segments.append(split[0])
            segment = split[1]
            t_max = segment._t_max()
        segments.append(segment)

        return sum([segment.q_length() for segment in segments])

    ##############################################

    @property
    def tangent1(self):
        return (self._p3 - self._p2).normalise()

    ##############################################

    def tangent_at(self, t):
        u = 1 - t
        return (self._p1 - self._p0) * u**2 + (self._p2 - self._p1) * 2 * t * u + (self._p3 - self._p2) * t**2

    ##############################################

    def intersect_line(self, line):

        """Find the intersections of the curve with a line."""

        # Algorithm: same as for quadratic

        # u = 1 - t
        # B = p0 *     u**3        +
        #     p1 * 3 * u**2 * t    +
        #     p2 * 3 * u    * t**2 +
        #     p3 *            t**3
        # B = p0 +
        #    (p1 - p0) * 3 * t  +
        #    (p2 - p1 * 2 + p0) * 3 * t**2 +
        #    (p3 - p2 * 3 + p1 * 3 - p0) * t**3
        # solveset(B, t)

        curve = self._map_to_line(line)

        p0 = curve.p0.y
        p1 = curve.p1.y
        p2 = curve.p2.y
        p3 = curve.p3.y

        return cubic_root(
            p3 - 3*p2 + 3*p1 - p0,
            3 * (p2 - p1 * 2 + p0),
            3 * (p1 - p0),
            p0,
        )

    ##############################################

    def fat_line(self):

        line = Line2D.from_two_points(self._p0, self._p3)
        d1 = line.distance_to_line(self._p1)
        d2 = line.distance_to_line(self._p2)
        if d1*d2 > 0:
            factor = 3 / 4
        else:
            factor = 4 / 9
        d_min = factor * min(0, d1, d2)
        d_max = factor * max(0, d1, d2)

        return (line, d_min, d_max)

    ##############################################

    def _clipping_convex_hull(self):

        line_03 = Line2D(self._p0, self._p3)
        d1 = line_03.distance_to_line(self._p1)
        d2 = line_03.distance_to_line(self._p2)

        # Check if p1 and p2 are on the same side of the line [p0, p3]
        if d1 * d2 < 0:
            # p1 and p2 lie on different sides of [p0, p3].
            # The hull is a quadrilateral and line [p0, p3] is not part of the hull.
            # The top part includes p1, we will reverse it later if that is not the case.
            hull = [
                [self._p0, self._p1, self._p3], # top part
                [self._p0, self._p2, self._p3]  # bottom part
            ]
            flip = d1 < 0
        else:
            # p1 and p2 lie on the same sides of [p0, p3]. The hull can be a triangle or a
            # quadrilateral and line [p0, p3] is part of the hull.  Check if the hull is a triangle
            # or a quadrilateral.  Also, if at least one of the distances for p1 or p2, from line
            # [p0, p3] is zero then hull must at most have 3 vertices.
             # Fixme: check cross product
            y0, y1, y2, y3 = [p.y for p in self.points]
            if abs(d1) < abs(d2):
                pmax = p2;
                # apex is y0 in this case, and the other apex point is y3
                # vector yapex -> yapex2 or base vector which is already part of the hull
                # V30xV10 * V10xV20
                cross_product = ((y1 - y0) - (y3 - y0)/3) * (2*(y1 - y0) - (y2 - y0)) /3
            else:
                pmax = p1;
                # apex is y3 and the other apex point is y0
                # vector yapex -> yapex2 or base vector which is already part of the hull
                # V32xV30 * V32xV31
                cross_product = ((y3 - y2) - (y3 - y0)/3) * (2*(y3 - y2) - (y3 + y1)) /3

            # Compare cross products of these vectors to determine if the point is in the triangle
            # [p3, pmax, p0], or if it is a quadrilateral.
            has_null_distance = d1 == 0 or d2 == 0 # Fixme: don't need to compute cross_product
            if cross_product < 0 or has_null_distance:
                # hull is a triangle
                hull = [
                    [self._p0, pmax, self._p3], # top part is a triangle
                    [self._p0, self._p3],       # bottom part is just an edge
                ]
            else:
                hull = [
                    [self._p0, self._p1, self._p2, self._p3], # top part is a quadrilateral
                    [self._p0, self._p3],                     # bottom part is just an edge
                ]
            flip =  d1 < 0 if d1 else d2 < 0

        if flip:
            hull.reverse()

        return hull

    ##############################################

    @staticmethod
    def _clip_convex_hull(hull_top, hull_bottom, d_min, d_max) :


        #              Top   /----
        #                   /     ---/
        #                  /        /
        # d_max -------------------*---
        #                /        / t_max
        #         t_min /        /
        # d_min -------*---------------
        #             /        /
        #            /    ----/   Bottom