/*
* We try to find an optimal triangle grid
- *
- * Vertices in strip are numbered as follows:
- *
- * ___ L-2 ___ L-1 ___ 0 ___ 1 ___ 2 ___ 3 ___ 4 __
- * W-1 W-1 0 0 0 0 0
- * / \ / \ / \ / \ / \ / \ / \
- * / \ / \ / \ / \ / \ / \ / \
- * L-3 ___ L-2 ___ L-1 ___ 0 ___ 1 ___ 2 ___ 3 ___ 4
- * W-2 W-2 W-2 1 1 1 1 1
- * \ / \ / \ / \ / \ / \ / \ /
- * \ / \ / \ / \ / \ / \ / \ /
- * ___ L-3 ___ L-2 ___ L-1 ___ 0 ___ 1 ___ 2 ___ 3 __
- * W-3 W-3 W-3 2 2 2 2
- *
- * . . . . . . . . . . . . . . .
- *
- * L-4 ___ L-3 ___ L-2 ___ L-1 ___ 0 ___ 1 ___ 2 ___ 3
- * 1 1 1 1 W-2 W-2 W-2 W-2
- * \ / \ / \ / \ / \ / \ / \ /
- * \ / \ / \ / \ / \ / \ / \ /
- * ___ L-4 ___ L-3 ___ L-2 ___ L-1 ___ 0 ___ 1 ___ 2 __
- * 0 0 0 0 W-1 W-1 W-1
- *
- * Node x,y for
- * 0 <= x < L x = distance along L = length
- * 0 <= y < W y = distance across W = width
- *
- * Vertices are in reading order from diagram above ie x varies fastest.
- *
- * W must be even. The actual location opposite (0,0) is (L-(W-1)/2,0),
- * and likewise opposite (0,W-1) is ((W-1)/2,0).
- *
- * To label edges, we counte anticlockwise[*] from to-the-right:
- *
- * \2 /1
- * \ /
- * ___ 0 __
- * 3 1 0
- * / \
- * 4/ 5\
- *
- * [*] That is, in the direction represented as anticlockwise for
- * the vertices (0,*)..(4,*) in the diagram above; and of course
- * that is clockwise for the vertices (L-5,*)..(L-1,*). The
- * numbering has an actual discontinuity between (L-1,*) and (0,*).
- *
- * When we iterate over edges, we iterate first over vertices and then
- * over edges 0 to 2, disregarding edges 3 to 5.
*/
-#define LN2W 4
-#define L (1<<LN2W)
-#define W 10
-#define N (L*W)
-#define YMASK (~0u << LN2W)
-#define XMASK (~YMASK)
-
/* Energy has the following parts right now:
*/
/*
* Find R', the `expected' location of R, by
* reflecting S in M (the midpoint of QP).
*
- * Let d = |RR'|
- * m = |PQ|
+ * Let 2d = |RR'|
+ * b = |PQ|
* l = |RS|
*
* Giving energy contribution:
*
* 2
- * m d
+ * b d
* E = F . ----
- * vd, edge PQ vd l
+ * vd, edge PQ vd 3
+ * l
+ *
+ * (The dimensions of this are those of F_vd.)
*
- * By symmetry, this calculation gives the same answer
- * with R and S exchanged. Looking at the projection in
- * the RMS plane:
+ * By symmetry, this calculation gives the same answer with R and S
+ * exchanged. Looking at the projection in the RMS plane:
*
*
* S'
* ,'
* ,'
- * R' ,' d" = |SS'| = |RR'| = d
+ * R' ,' 2d" = |SS'| = |RR'| = 2d
* `-._ ,'
- * `-._ ,'
- * ` M
- * ,' `-._
- * ,' `-._
- * ,' ` S
- * ,'
- * ,'
- * R
+ * `-._ ,' By congruent triangles,
+ * ` M with M' = midpoint of RS,
+ * ,' `-._ |MM'| = |RR'|/2 = d
+ * ,' `-._
+ * ,' ` S So use
+ * ,' M' _ , - ' d = |MM'|
+ * ,' _ , - '
+ * R - '
*
* We choose this value for l (rather than |RM|+|MS|, say, or |RM|)
* because we want this symmetry and because we're happy to punish
* bending more than uneveness in the metric.
*
- * In practice to avoid division by zero we'll add epsilon to l and
- * the huge energy ought then to be sufficient for the model to
+ * In practice to avoid division by zero we'll add epsilon to l^3
+ * and the huge energy ought then to be sufficient for the model to
* avoid being close to R=S.
*/
-static int edge_end2(int v1, int e) {
- if (!(v1 &
-
- if (e==1 || e==2)
- if (v2 < L)
-
-
-#define FOR_VERTEX(v) \
- for ((v)=0; (v)<N; (v)++)
-
-#define FOR_VPEDGE(v,e) \
- for ((e)=0; (e)<6; (e)++)
-
-#define EDGE_END2 edge_end2
-
-#define FOR_VEDGE(v1,e,v2) \
- FOR_VPEDGE((v1),(e))
- if (((v2)= EDGE_END2((v1),(e))) >= 0) ; else
-
-#define FOR_EDGE(v1,e,v2) \
- FOR_VERTEX((v1)) \
- FOR_VEDGE((v1),(e),(v2))
-
-static double energy_function(const double vertices[N][3]) {
- int vertex;
-
- FOR_VERTEX {
-
+double hypotD(const double p[D3], const double q[D3]) {
+ int k;
+ double pq[D3];
+ gsl_vector v;
+
+ K pq[_k]= p[k] - q[k];
+ v.size= D3;
+ v.stride= 1;
+ v.vector.data= pq;
+ /* owner and block ought not to be used */
+
+ return gsl_blas_snrm2(&v);
+}
+
+double hypotD2(const double p[D3], const double q[D3]) {
+ double d2= 0;
+ K d2= ffsqa(p[k] - q[k], d2);
+ return d2;
+}
+
+#ifdef FP_FAST_FMA
+# define ffsqa(factor,term) fma((factor),(factor),(term))
+#else
+# define ffsqa(factor,term) ((factor)*(factor)+(term))
+#endif
+
+static const l3_epsison= 1e-6;
+
+static double energy_function(const double vertices[N][D3]) {
+ int pi,e,qi,ri,si, k;
+ double m[D3], mprime[D3], b, d2, l, sigma_bd2_l;
+
+ FOR_EDGE(pi,e,qi) {
+ ri= EDGE_END2(pi,(e+1)%V6); if (r<0) continue;
+ si= EDGE_END2(pi,(e+5)%V6); if (s<0) continue;
+ assert(ri == EDGE_END2(qi,(e+2)%V6));
+ assert(si == EDGE_END2(qi,(e+4)%V6));
+
+ K m[k]= (vertices[pi][k] + vertices[qi][k]) * 0.5;
+ K mprime[k]= (vertices[ri][k] + vertices[si][k]) * 0.5;
+ b= hypotD(vertices[pi], vertices[qi]);
+ d2= hypotD2(m, mprime);
+ l= hypotD(vertices[ri][k] - vertices[si][k]);
+ l3 = l*l*l + l3_epsilon;
+
+ sigma_bd2_l += b * d2 / l3;
+ }
+
+
static gsl_multimin_fminimizer *minimiser;