from numpy import *
from numpy.linalg import *
from visual import *

p0 = [-5,0]
p1 = [-2,5]
p2 = [2,5]
p3 = [5,0]
v0 = [0,10]
v1 = [3,0]

PB = array([p0,p1,p2,p3])
PH = array([p0,p3,v0,v1])

MB = array([[-1,3,-3,1],
           [3,-6,3,0],
           [-3,3,0,0],
           [1,0,0,0]])

MH = array([[2,-2,1,1],
           [-3,3,-2,-1],
           [0,0,1,0],
           [1,0,0,0]])

def evalT(t):
    return [t**3, t**2, t, 1]

def Bezier(PB):
    points(pos = PB, color = color.blue)
    curve(pos = PB, color = color.green)
    Ct = dot(MB, PB)
    tlist = arange(0, 1.01, 0.01)
    allpoints = []
    for t in tlist:
        allpoints.append(dot(evalT(t), Ct))
    curve(pos = allpoints, color = color.red)

def Hermit(PH):
    points(pos = PH[0:1], shaftwidth=0.01, color = color.blue)
    arrow(pos=PH[0], axis=PH[2], shaftwidth = 0.01, color = color.green)
    arrow(pos=PH[1], axis=PH[3], shaftwidth = 0.01, color = color.green)
    Ct = dot(MH, PH)
    tlist = arange(0, 1.01, 0.01)
    allpoints = []
    for t in tlist:
        allpoints.append(dot(evalT(t), Ct))
    curve(pos = allpoints, color=color.red)

MHinv = inv(MH)
MBinv = inv(MB)

#Bezier([p0,p1,p2,p3])
Hermit([p0,p3,v0,v1])

# Write a program B2H to convert a Bezier curve into a Hermit Curve.
# Write a program H2B to convert a Hermit curve into a Bezier Curve.