# Module that defines single arcs for {sploine.py} splines.
# Last edited on 2021-06-08 13:42:11 by jstolfi

# Ideally, {interpolate} and {arc_length} should be self-consistent and
# consistent with each other. Namely, if state {stc} is interpolated
# between {sta} and {stb}, then the arc defined by these two should be
# the union of the arcs from {sta} to {stc} and from {stc} to {stb}.
# Also {arc_length(sta,stb)} should be the real length of the arc
# between the two states. Moreover, the interpolated arc should be an
# arcof circle whenever the the two end states are compatible with that.
# This ideal is still not achieved. Could it be?

import sploine_arc_IMP
import sploine

def length(ua,ta, ub,tb):
  # Estmates the length of the arc defined by the extremal
  # position-tangent pairs {ua,ta} to {ub,tb}. 
  return sploine_arc_IMP.length(ua,ta, ub,tb)
   
def acceleration(ua,ta, ub,tb):
  # Estimates the average acceleration vector of the arc defined by the extremal
  # position-tangent pairs {ua,ta} to {ub,tb}.
  return sploine_arc_IMP.acceleration(ua,ta, ub,tb)
  
def eval_circle(r, ua,ub,txC,RC,angC):
  # Interpolates an arc of circle {C(r)} at the parameter value {r}.
  # Returns the position {ur = C(r)}, the tangent vector {tr}, and the
  # speed {zR} (such that {A'(r) = zr*tr}).
  # 
  # The endpoints of the arc will be {ua,ub}.  If {ub=ua} then 
  # the result will be {ur=ua}, {tr} arbitrary, {zR} zero.
  # 
  # The angular span of the circle will be {angC} (radians) and its
  # radius will be {RC}.  The circle will be traversed 
  # at constant speed (derivative rel to {r}) equal to {RC*angC}.
  #
  # The unit vector {txC} should be in the plane of the circle
  # perpendicular to the vector {ub-ua}. The angle {angC} and radius
  # {RC} must satisfy {sin(angC/2) = (|ub-ua|/2)/RC}; in particular, if
  # {angC} is zero {RC} should be {+inf}, and then {txC} is irrelvant.
  return sploine_arc_IMP.eval_circle(r, ua,ub,txC,RC,angC)

def fit_circle(d,c,s):
  # Computes the elements of a circle given the distance {d} between two
  # ends of an arc, and the co-sine {c} and sine {s} that the arc makes
  # with the chord at each end-point. 
  #
  # Returns {ang,R,L,F} where {ang} is the angle spanned by the arc, {R}
  # is the radius of the circle, {L} is the arc's length, and {F} is the
  # sagitta (max deviation of the arc from the chord).
  #
  # The values {d,s} must be non-negative. The following are special cases:
  # 
  #   {d=0}:      returns {0,0,0,0}
  #
  #   {d>0,c=0}:  returns {pi/2,d/2,pi*d,d/2}
  #
  #   {d>0,c=+1}: returns {0,+inf,d,0}
  #
  #   {d>0,c=-1}: returns {pi,+inf,+inf,+inf}
  # 
  return sploine_arc_IMP.fit_circle(d,c,s)
  
def split_circle_bezier(ua,ta, ub,tb):
  # Given the positions {ua,ub} and tangents {ta,tb} of two states,
  # chooses an arc of circle {C(r)} and a cubic curve arc {B(r)} such that
  # the the arc between those two states is {A(r) = B(r)+C(r)} by
  # definition.
  #
  # The circular term {C(r)} will always start at {ua} and end at {ub}.
  # It is described by the endpoints {ua,ub} and the vector {fC = uC-um} 
  # where {uC} is the arc's midpoint and {um} is the midpoint of the chord.
  # Derived parameters are the unit vectors {txC} and {tyC} parallel to {fC} and to the 
  # chord vector {ub-ua}; the circle's radius
  # {RC}; and the subtended angle {angC}. If the length of {fC} is zero, {RC} is infinite and {angC} is
  # zero; the arc is a straight line.  The circle arc is such that {fC} is finite an 
  # not too big compared to the chord length {dab}.
  # The curve {C(r)} traces this arc of circle at constant speed {zC}
  # equal to the length of the arc, {RC*angC}.
  #
  # The cubic correction {B} starts at the origin with some speed {zaB}
  # and tangent {taB}, and ends at the origin with some speed {zbB} and
  # tangent {tbB}, such that {zC*taC + zaB*taB} is a multiple of {ta}
  # and {zC*tbC + zbB*tbB} is a multiple of {tb}. The speeds {zaB} and
  # {zbB} are chosen "appropriately".
  #
  # The procedure returns the circular arc parameters {txC,tyC,RC,angC} and the
  # two middle Bézier control vectors {umB,unB} of the cubic correction.
  # 
  return sploine_arc_IMP.split_circle_bezier(ua,ta, ub,tb)

def bezier_controls(sta, stb, vel):
  # Given two states {sta,stb} which are not {None}, returns four
  # position vectors {ua,um,un,ub} that are the Bézier control points of
  # the arc that interpolates between their positions. The arc will
  # start at the position {ua} of {sta} in the direction of the tangent
  # of {sta} with speed {vel}, and end at the position {ub} of {stb}
  # with direction of the tangent of {stb} with the same speed.
  return sploine_arc_IMP.bezier_controls(sta, stb, vel)
  
def eval_bezier(r, ua, um, un, ub):
  # Given the four Bézier control points {ua,um,un,ub} of a cubic curve
  # in {n}-space, evaluates the curve at the Bézier parameter {r},
  # yielding a point {ur}. In particular, when {r} is zero the result
  # {ur} will be {ua}, and when {r} is 1 the result will be {ub}. Also
  # returns the tangent vector {tar} at the point {ur}. Each result is a
  # tuple of floats of the same length {n}.
  return sploine_arc_IMP.eval_bezier(r, ua, um, un, ub)
  
def eval_bezier_corr(r, um, un):
  # Same as {eval_bezier(r, o, um, un, o)} where {o} is the zero vector.
  return sploine_arc_IMP.eval_bezier_corr(r, um, un)
  
# TEST TOOLS

def circle_example_parms(ang, R):
  # Returns {ua,ta,ub,tb} where {ua,ub} are positions and {ta,yb} are 
  # tangents at the ends of an arbitrary arc of circle in some
  # {n}-space that spans {ang} radians and has radius {R}. 
  #
  # The radius {R} can be {+inf} if and only if {ang} is zero.  In this case, 
  # generates an arbitrary straight line segment.
  return sploine_arc_IMP.circle_example_parms(ang, R)

def check_circle_like(ua,ta, ub,tb):
  # Check that the position and tangent pairs {ua,ta} and {ub.tb}
  # are consistent with the ends of a plane circle.
  return sploine_arc_IMP.check_circle_like(ua,ta, ub,tb)