+#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;
+ }
+
+