# Module to find incircles and osculating circles of a closed convex curve.
# Last edited on 2021-03-08 11:46:06 by jstolfi

import rn
import sys
import math

def osculating(p, u, cvt):
  # Given a point {p} on a closed smooth curve, the local unit
  # tangent vector {u}, and the curvature {cvt}, returns the center {ctr}
  # and radius {rad} of the osculating circle at that point.  Assumes
  # that a positive {cvt} means that the curve is bending counterclockwise from {u}.
  # The curvature should not be zero.

  assert cvt != 0
  rad = 1/cvt
  ctr = rn.mix(1.0, p, rad, (-u[1],+u[0]))
  return ctr, rad
  # ----------------------------------------------------------------------
  
def incircles(curve):
  # Expects {curve} to be a list of triples {(o, u, cvt)}, where {o} is
  # a point on the boundary curve of some figure, {u} is the local
  # tangent vector to that curve, and {cvt} is its local curvature.  Assumes
  # that a positive {cvt} means that the curve is bending counterclockwise from {u}.
  # The curvature should not be zero.  Assumes that {curve} has no "closer"
  # point, and does not return any.
  #
  # For each input triple, finds the /local incircle/, the largest
  # circle {C(o)} that goes through {o} with tangent vector {u} and
  # contains no other point of the curve. That circle will be the
  # osculating circle, or a smaller circle if the latter contains some
  # other point of the curve. In the second case, the circle {C(o)} will go
  # through some other /pinning point/ {q} distinct from {o}.
  #
  # The result will be a list of triples {(ctr, q)}, one for each
  # input triple, where {ctr} is the center of that circle, and {q} is its
  # pinning point.  If {C(o)} is the osculating circle, {q} will be {None}.
  #
  # In the ideal continuous curve, the pinning point could be anywhere on the curve
  # at some positive distance from {o}.  The procedure however
  # will consider only the points given in the {curve} list as potential
  # pinning points.  It is assumed that he samepling is dense enough 
  # for this error tobe negligble.
  #
  # In the ideal curve, the incircle {C(o)} may have two or more pinning 
  # points, or even infinitely many of them.  However, the points {o} with such
  # conditions are isolated, so we can ignore this possibility.
  
  np = len(curve)
  assert curve[np-1] != curve[0]
  
  # Create the initial result with the osculating circles and no {Q}s:
  res = []
  for k in range(np):
    o, u, cvt = curve[k]
    ctr, rad = osculating(o, u, cvt)
    # Check the other points of the curve:
    rin = rad # Radius of current incircle {C(o)}.
    cin = ctr # Center of current incircle {C(o)}.
    q = None  # Current pinning poing of {C(o)} 
    for dk in range(np-1):
      k1 = (k + dk + 1) % np
      o1, u1, cvt1 = curve[k1]
      d1 = rn.dist(o1, cin)
      fuzz = 0.25 # Allow points inside the circle by this much (pixels).
      if d1 < rin - fuzz:
        # Shrink the circle:
        w = rn.sub(o, o1)
        ww = rn.norm_sqr(w)
        vw = rn.dot(w, (-u[1],+u[0]))
        rin1 = -0.5*ww/vw
        if vw >= -1.0e-8 or rin1 > rin + 1.0e-8:
          sys.stderr.write("inconsistent geometry:\n")
          debug(o,u,cvt,ctr,rad,cin,rin,o1,d1,w,vw,rin1)
        # The new radius {rin1} should be less than {rin}, but just in case:
        if rin1 < rin:
          rin = rin1;
          cin = rn.mix(1.0, o, rin, (-u[1],+u[0]))
          q = o1
    res.append((cin, q))

  assert len(res) == np
  return res
  # ----------------------------------------------------------------------

def debug(o,u,cvt,ctr,rad,cin,rin,o1,d1,w,vw,rin1):
  # Debugging info
  sys.stderr.write("  o =    ( %12.7f  %12.7f )\n" % (o[0],o[1]))
  sys.stderr.write("  u =    ( %12.7f  %12.7f )\n" % (u[0],u[1]))
  sys.stderr.write("  v =    ( %12.7f  %12.7f )\n" % (-u[1],+u[0]))
  sys.stderr.write("  ctr =  ( %12.7f  %12.7f ) rad = %12.7f\n" % (ctr[0],ctr[1],1/cvt))
  sys.stderr.write("  cin =  ( %12.7f  %12.7f ) rin = %12.7f\n" % (cin[0],cin[1],rin))
  sys.stderr.write("  o1 =   ( %12.7f  %12.7f ) d1  = %12.7f\n" % (o1[0],o1[1],d1))
  sys.stderr.write("  w =    ( %12.7f  %12.7f ) vw  = %12.7f\n" % (w[0],w[1,vw]))
  sys.stderr.write("  rin1 = %12.7f\n" % rin1)
  return 
  # ----------------------------------------------------------------------