+ static const unsigned dx[V6]= { +1, +1, 0, -1, -1, 0 },
+ dy[V6]= { 0, +Y1, +Y1, 0, -Y1, -Y1 };
+ unsigned x, y;
+
+ y= (v1 & YMASK) + dy[e];
+ if (y & ~YMASK) return -1;
+
+ x= (v1 & XMASK) + dx[e];
+ if (x & ~XMASK) {
+ y= (Y-1)*Y1 - y;
+ x &= XMASK;;
+ }
+ return x | y;
+}
+
+#define D3 3
+
+#define FOR_VERTEX(v) \
+ for ((v)=0; (v)<N; (v)++)
+
+#define FOR_VPEDGE(v,e) \
+ for ((e)=0; (e)<V6; (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))
+
+#define FOR_COORD(k) \
+ for ((k)=0; (k)<D3; (k)++)
+
+#define K FOR_COORD(k)
+
+static double hypotD(const double p[], const double q[]) {
+ 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);
+}
+
+#ifdef FP_FAST_FMA
+# define ffsqa(factor,term) fma((factor),(factor),(term))
+#else
+# define ffsqu(factor,term) ((factor)*(factor)+(term))
+#endif
+
+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= 0; K d2= ffsqa(m[k] - mprime[k], d2);
+ l= hypotD(vertices[ri][k] - vertices[si][k]);
+
+ sigma_bd2_l += b * d2 / l;
+ }
+
+