+
+/*---------- triangle bad normals cost ----------*/
+
+ /*
+ *
+ * Q `-_
+ * / | `-_
+ * / | `-.
+ * / | S
+ * / | _,-'
+ * / | _,-'
+ * / , P '
+ * / ,-'
+ * /,-'
+ * /'
+ * R
+ *
+ * Let delta = angle between two triangles' normals
+ *
+ * Giving energy contribution:
+ *
+ * 2
+ * E = F . delta
+ * vd, edge PQ vd
+ */
+
+double edge_angle_cost(const Vertices vertices) {
+ double sq[D3], pq[D3], qr[D3], sqp[D3], pqr[D3], rs[D3];
+ double x[D3], y[D3];
+ int pi,e,qi,ri,si, k;
+// double our_epsilon=1e-6;
+ double total_cost= 0;
+
+ FOR_EDGE(pi,e,qi) {
+// if (!(RIM_VERTEX_P(pi) || RIM_VERTEX_P(qi))) continue;
+
+ si= EDGE_END2(pi,(e+V6-1)%V6); if (si<0) continue;
+ ri= EDGE_END2(pi,(e +1)%V6); if (ri<0) continue;
+
+ K {
+ sq[k]= vertices[si][k] - vertices[qi][k];
+ pq[k]= vertices[pi][k] - vertices[qi][k];
+ qr[k]= vertices[qi][k] - vertices[ri][k];
+ }
+ xprod(sqp, pq,sq);
+ xprod(pqr, pq,qr);
+
+ double dot= dotprod(sqp,pqr);
+ xprod(x,sqp,pqr);
+
+ K rs[k]= -vertices[ri][k] + vertices[si][k];
+ xprod(y, pq,rs);
+
+ double delta=
+ pow(atan2(magnD(x), dot), 2.0) * pow(magnD2(pq), 2.0) /
+ (pow(magnD(y), 0.3) + 1e-6);
+ double cost= pow(delta, 2.0);
+
+//double cost= pow(magnD(spqxpqr), 3);
+//assert(dot>=-1 && dot <=1);
+//double cost= 1-dot;
+ total_cost += cost;
+ }
+
+ return total_cost;
+}
+
+/*---------- small triangles cost ----------*/
+
+ /*
+ *
+ * Q `-_
+ * / | `-_
+ * / | `-.
+ * / | S
+ * / | _,-'
+ * / | _,-'
+ * / , P '
+ * / ,-'
+ * /,-'
+ * /'
+ * R
+ *
+ * Let delta = angle between two triangles' normals
+ *
+ * Giving energy contribution:
+ *
+ * 2
+ * E = F . delta
+ * vd, edge PQ vd
+ */
+
+double small_triangles_cost(const Vertices vertices) {
+ double pq[D3], ps[D3];
+ double x[D3];
+ int pi,e,qi,si, k;
+// double our_epsilon=1e-6;
+ double total_cost= 0;
+
+ FOR_EDGE(pi,e,qi) {
+// if (!(RIM_VERTEX_P(pi) || RIM_VERTEX_P(qi))) continue;
+
+ si= EDGE_END2(pi,(e+V6-1)%V6); if (si<0) continue;
+
+ K {
+ pq[k]= vertices[qi][k] - vertices[pi][k];
+ ps[k]= vertices[si][k] - vertices[pi][k];
+ }
+ xprod(x, pq,ps);
+
+ double cost= 1/(magnD2(x) + 0.01);
+
+//double cost= pow(magnD(spqxpqr), 3);
+//assert(dot>=-1 && dot <=1);
+//double cost= 1-dot;
+ total_cost += cost;
+ }
+
+ return total_cost;
+}