From 5ca187f1eea8669e7b64d38840d85c82a3868501 Mon Sep 17 00:00:00 2001
From: Ian Jackson <ijackson@chiark.greenend.org.uk>
Date: Sun, 8 Apr 2018 13:37:26 +0100
Subject: [PATCH] curveopt: new approach

Signed-off-by: Ian Jackson <ijackson@chiark.greenend.org.uk>
---
 findcurve.c | 21 +++---------
 symbolic.py | 97 ++++++++++++++++++++++++++++++++++++++++++++++-------
 2 files changed, 90 insertions(+), 28 deletions(-)

diff --git a/findcurve.c b/findcurve.c
index 275b501..055018f 100644
--- a/findcurve.c
+++ b/findcurve.c
@@ -46,6 +46,7 @@ static void prepare(double X[] /* startpoint */) {
 
 static double cb_Efunc(void *xp) {
   const double *X = xp;
+  int P;
   DECLARE_F_G;
   CALCULATE_F_G;
 
@@ -53,27 +54,15 @@ static double cb_Efunc(void *xp) {
   int i,j;
   printf(" Efunc\n");
   for (j=0; j<3; j++) {
-    for (i=0; i<NP; i++)
-      printf(" %7.4f", POINT(i)[j]);
+    for (P=0; P<NP; P++)
+      printf(" %7.4f", POINT(P)[j]);
     printf("\n");
   }
   printf("             ");
 #endif
 
-  double e = 0;
-  int P;
-  for (P=0; P<NP-3; P++) {
-    double P_cost;
-    CALCULATE_COST;
-#ifdef DEBUG
-    printf(" %7.4f", P_cost);
-#endif
-    e += P_cost;
-  }
-#ifdef DEBUG
-  printf("\n");
-#endif
-  return e;
+  CALCULATE_COST;
+  return cost;
 }
 
 static void cb_step(const gsl_rng *rng, void *xp, double step_size) {
diff --git a/symbolic.py b/symbolic.py
index 90af6e7..d505e8c 100644
--- a/symbolic.py
+++ b/symbolic.py
@@ -9,6 +9,10 @@ import numpy as np
 
 from moedebug import *
 
+# When cse() is called for something containing BaseVector, it
+# produces infinite recursion.
+#def cse(x, *a, **kw): return ((), (x,))
+
 def dprint(*args):
   if not dbg_enabled(): return
   print(*args)
@@ -21,6 +25,8 @@ def dbg(*args):
     print('\n          =\n')
     pprint(cse(eval(vn)))
 
+def sqnorm(v): return v & v
+
 N = CoordSysCartesian('N')
 
 calculated = False
@@ -56,26 +62,88 @@ def vector_component(v, ix):
 
 #def subst_vect():
 
+iterations = []
+
+class SomeIteration():
+  def __init__(ar, names, size, expr):
+    ar.names_string = names
+    ar.names = names.split(' ')
+    ar.name = ar.names[0]
+    ar.size = size
+    ar.expr = expr
+    if dbg_enabled():
+      print('\n   ' + ar.name + '\n')
+      print(expr)
+    iterations.append(ar)
+
+  def gen_calculate_cost(ar):
+    ar._gen_array()
+    cprint('for (P=0; P<(%s); P++) {' % ar.size)
+    ar._cassign()
+    cprint('}')
+
+class ScalarArray(SomeIteration):
+  def _gen_array(ar):
+    cprint('double A_%s[%s];' % (ar.name, ar.size))
+  def gen_references(ar):
+    for ai in range(0, len(ar.names)):
+      ar._gen_reference(ai, ar.names[ai])
+  def _gen_reference(ar, ai, an):
+    cprintraw('#define %s A_%s[P%+d]' % (an, ar.name, ai))
+  def _cassign(ar):
+    cassign(ar.expr, ar.name, 'tmp_'+ar.name)
+  def s(ar):
+    return symbols(ar.names_string)
+
+class CoordArray(ScalarArray):
+  def _gen_array(ar):
+    cprint('double A_%s[%s][3];' % (ar.name, ar.size))
+  def _gen_reference(ar, ai, an):
+    ScalarArray._gen_reference(ar, ai, an)
+    gen_point_coords_macro(an)
+  def _cassign(ar):
+    cassign_vector(ar.expr, ar.name, 'tmp_'+ar.name)
+  def s(ar):
+    return vector_symbols(ar.names_string)
+
+class CostComponent(SomeIteration):
+  def __init__(cc, size, expr):
+    cc.size = size
+    cc.expr = expr
+    iterations.append(cc)
+  def gen_references(cc): pass
+  def _gen_array(cc): pass
+  def _cassign(cc):
+    cassign(cc.expr, 'P_cost', 'tmp_cost')
+    cprint('cost += P_cost;')
+
 def calculate():
   global calculated
   if calculated: return
 
-  Q = B + 0.5 * (B - A)
-  R = C + 0.5 * (C - D)
-  QR = R - Q
-  BC = C - B
-  global cost_ABCD
-  cost_ABCD = (QR & QR) / (BC & BC)
-  dbg('cost_ABCD')
-  dprint(A)
+  # ---------- actual cost computation formulae ----------
 
   global F, G
   F = E + En * EFl
   G = H + Hn * HGl
 
-  # diff_A_i = cost_ABCD.diff(vector_component(A, 0))
-  # global diff_A_i
-  # dbg('diff_A_i')
+  global a,b, al,bl, au,bu
+  a,  b  = CoordArray ('a_ b_',   'NP-1', B-A           ).s() # [mm]
+  al, bl = ScalarArray('al bl',   'NP-1', a.magnitude() ).s() # [mm]
+  au, bu = CoordArray ('au bu',   'NP-1', a / al        ).s() # [1]
+
+  tan_theta = (au ^ bu) / (au & bu)     # [1]     bending
+  curvature = tan_theta / sqrt(al * bl) # [1/mm]  bending per unit length
+
+  global mu, nu
+  mu, nu = CoordArray ('mu nu', 'NP-2', curvature ).s() # [1/mm]
+
+  CostComponent('NP-3', sqnorm(mu - nu)) # [1/mm^2]
+
+  d_density = 1/al - 1/bl # [1/mm]
+  CostComponent('NP-2', pow(d_density, 2)) # [1/mm^2]
+
+  # ---------- end of cost computation formulae ----------
 
   calculated = True
 
@@ -181,6 +249,9 @@ def gen_point_references():
     cprintraw('#define %s POINT(P%+d)' % (abcd[ai], ai))
     gen_point_coords_macro(abcd[ai])
 
+  for si in iterations:
+    si.gen_references()
+
   cprintraw('')
 
 def gen_prepare():
@@ -203,7 +274,9 @@ def gen_calculate_FG():
 
 def gen_calculate_cost():
   cprint('#define CALCULATE_COST')
-  cassign(cost_ABCD,'P_cost','tmp_P_cost')
+  cprint('double cost=0, P_cost;')
+  for si in iterations:
+    si.gen_calculate_cost()
   cprintraw('')
 
 def gen_C():
-- 
2.30.2