energy.c

   1 /*
   2  * We try to find an optimal triangle grid
   3  */
   4
   5 #include "common.h"
   6 #include "minimise.h"
   7 #include "mgraph.h"
   8 #include "parallel.h"
   9
  10 double vertex_mean_edge_lengths[N];
  11
  12 static double vertex_areas[N];
  13 static double edge_lengths[N][V6];
  14 static double rim_vertex_angles[N];
  15
  16 static double best_energy= DBL_MAX;
  17
  18 static void addcost(double *energy, double tweight, double tcost, int pr);
  19
  20 /*---------- main energy computation, weights, etc. ----------*/
  21
  22 typedef double CostComputation(const Vertices vertices, int section);
  23 typedef void PreComputation(const Vertices vertices, int section);
  24
  25 typedef struct {
  26   double weight;
  27   CostComputation *fn;
  28 } CostContribution;
  29
  30 #define NPRECOMPS ((sizeof(precomps)/sizeof(precomps[0])))
  31 #define NCOSTS ((sizeof(costs)/sizeof(costs[0])))
  32 #define COST(weight, compute) { (weight),(compute) },
  33
  34 static PreComputation *const precomps[]= {
  35   compute_edge_lengths,
  36   compute_vertex_areas,
  37   compute_rim_twist_angles
  38 };
  39
  40 static const CostContribution costs[]= {
  41
  42 #if XBITS==3
  43 #define STOP_EPSILON 1e-6
  44     COST(  3e3,   vertex_displacement_cost)
  45     COST( 0.4e3,  rim_proximity_cost)
  46     COST(  1e7,   edge_angle_cost)
  47                   #define EDGE_ANGLE_COST_CIRCCIRCRAT (0.2/1.7)
  48     COST(  1e2,   small_triangles_cost)
  49     COST(  1e12,   noncircular_rim_cost)
  50 #endif
  51
  52 #if XBITS==4
  53 #define STOP_EPSILON 5e-3
  54     COST(  3e4,   vertex_displacement_cost) // NB this is probably wrong now
  55     COST(  3e4,   vertex_edgewise_displ_cost) // we have changed the power
  56     COST( 2e2,  rim_proximity_cost)
  57     COST( 1e4,  rim_twist_cost)
  58     COST(  1e12,   noncircular_rim_cost)
  59     COST(  10e1,   nonequilateral_triangles_cost)
  60 //    COST(  1e1,   small_triangles_cost)
  61 //    COST(  1e6,   edge_angle_cost)
  62                   #define EDGE_ANGLE_COST_CIRCCIRCRAT (0.5/1.7)
  63 #endif
  64
  65 #if XBITS==5
  66 #define STOP_EPSILON 7e-4
  67     COST(  3e4,   vertex_displacement_cost)
  68     COST(  3e4,   vertex_edgewise_displ_cost)
  69     COST(  2e-1,  rim_proximity_cost)
  70     COST(  3e3,  rim_twist_cost)
  71     COST(  1e12,   noncircular_rim_cost)
  72     COST(   3e2,   nonequilateral_triangles_cost)
  73 //    COST(  1e1,   small_triangles_cost)
  74 //    COST(  1e6,   edge_angle_cost)
  75                   #define EDGE_ANGLE_COST_CIRCCIRCRAT (0.5/1.7)
  76 #endif
  77
  78 #if XBITS==6
  79 #define STOP_EPSILON 1.2e-4
  80     COST(  3e4,   vertex_displacement_cost)
  81     COST(  3e4,   vertex_edgewise_displ_cost)
  82     COST( 2e-1,  rim_proximity_cost)
  83     COST(  1e3,  rim_twist_cost)
  84     COST(  1e12,   noncircular_rim_cost)
  85     COST(  10e1,   nonequilateral_triangles_cost)
  86 //    COST(  1e1,   small_triangles_cost)
  87 //    COST(  1e6,   edge_angle_cost)
  88                   #define EDGE_ANGLE_COST_CIRCCIRCRAT (0.5/1.7)
  89 #endif
  90
  91 #if XBITS>=7 /* nonsense follows but never mind */
  92 #define STOP_EPSILON 1e-6
  93     COST(  3e5,   line_bending_cost)
  94     COST( 10e2,   edge_length_variation_cost)
  95     COST( 9.0e1,  rim_proximity_cost) // 5e1 is too much
  96                                       // 2.5e1 is too little
  97     // 0.2e1 grows compared to previous ?
  98     // 0.6e0 shrinks compared to previous ?
  99
 100     COST(  1e12,   edge_angle_cost)
 101                   #define EDGE_ANGLE_COST_CIRCCIRCRAT (0.5/1.3)
 102     COST(  1e18,   noncircular_rim_cost)
 103 #endif
 104
 105 };
 106
 107 const double edge_angle_cost_circcircrat= EDGE_ANGLE_COST_CIRCCIRCRAT;
 108
 109 void energy_init(void) {
 110   stop_epsilon= STOP_EPSILON;
 111 }
 112
 113 /*---------- energy computation machinery ----------*/
 114
 115 void compute_energy_separately(const struct Vertices *vs,
 116                          int section, void *energies_v, void *totals_v) {
 117   double *energies= energies_v;
 118   int ci;
 119
 120   for (ci=0; ci<NPRECOMPS; ci++) {
 121     precomps[ci](vs->a, section);
 122     inparallel_barrier();
 123   }
 124   for (ci=0; ci<NCOSTS; ci++)
 125     energies[ci]= costs[ci].fn(vs->a, section);
 126 }
 127
 128 void compute_energy_combine(const struct Vertices *vertices,
 129                          int section, void *energies_v, void *totals_v) {
 130   int ci;
 131   double *energies= energies_v;
 132   double *totals= totals_v;
 133
 134   for (ci=0; ci<NCOSTS; ci++)
 135     totals[ci] += energies[ci];
 136 }
 137
 138 double compute_energy(const struct Vertices *vs) {
 139   static int bests_unprinted;
 140
 141   double totals[NCOSTS], energy;
 142   int ci, printing;
 143
 144   printing= printing_check(pr_cost,0);
 145
 146   if (printing) printf("%15lld c>e |", evaluations);
 147
 148   for (ci=0; ci<NCOSTS; ci++)
 149     totals[ci]= 0;
 150
 151   inparallel(vs,
 152              compute_energy_separately,
 153              compute_energy_combine,
 154              sizeof(totals) /* really, size of energies */,
 155              totals);
 156
 157   energy= 0;
 158   for (ci=0; ci<NCOSTS; ci++)
 159     addcost(&energy, costs[ci].weight, totals[ci], printing);
 160
 161   if (printing) printf("| total %# e |", energy);
 162
 163   if (energy < best_energy) {
 164     FILE *best_f;
 165     int r;
 166
 167     if (printing) {
 168       printf(" BEST");
 169       if (bests_unprinted) printf(" [%4d]",bests_unprinted);
 170       bests_unprinted= 0;
 171     } else {
 172       bests_unprinted++;
 173     }
 174
 175     best_f= fopen(best_file_tmp,"wb");  if (!best_f) diee("fopen new out");
 176     r= fwrite(vs->a,sizeof(vs->a),1,best_f); if (r!=1) diee("fwrite");
 177     if (fclose(best_f)) diee("fclose new best");
 178     if (rename(best_file_tmp,best_file)) diee("rename install new best");
 179
 180     best_energy= energy;
 181   }
 182   if (printing) {
 183     putchar('\n');
 184     flushoutput();
 185   }
 186
 187   evaluations++;
 188   return energy;
 189 }
 190
 191 static void addcost(double *energy, double tweight, double tcost, int pr) {
 192   double tenergy= tweight * tcost;
 193   if (pr) printf(/*" %# e >"*/ " %# e* |", /*tcost,*/ tenergy);
 194   *energy += tenergy;
 195 }
 196
 197 /*---------- Precomputations ----------*/
 198
 199 void compute_edge_lengths(const Vertices vertices, int section) {
 200   int v1,e,v2;
 201
 202   FOR_EDGE(v1,e,v2, OUTER)
 203     edge_lengths[v1][e]= hypotD(vertices[v1],vertices[v2]);
 204 }
 205
 206 void compute_vertex_areas(const Vertices vertices, int section) {
 207   int v0,v1,v2, e1,e2;
 208 //  int k;
 209
 210   FOR_VERTEX(v0, OUTER) {
 211     double total= 0.0, edges_total=0;
 212     int count= 0;
 213
 214     FOR_VEDGE(v0,e1,v1) {
 215       e2= (e1+1) % V6;
 216       v2= EDGE_END2(v0,e2);
 217       if (v2<0) continue;
 218
 219       edges_total += edge_lengths[v0][e1];
 220
 221 //      double e1v[D3], e2v[D3], av[D3];
 222 //      K {
 223 //      e1v[k]= vertices[v1][k] - vertices[v0][k];
 224 //      e2v[k]= vertices[v2][k] - vertices[v0][k];
 225 //      }
 226 //      xprod(av, e1v, e2v);
 227 //      total += magnD(av);
 228
 229       count++;
 230     }
 231     vertex_areas[v0]= total / count;
 232     vertex_mean_edge_lengths[v0]= edges_total / count;
 233   }
 234 }
 235
 236 /*---------- displacement of vertices across a midpoint ----------*/
 237
 238   /*
 239    * Subroutine used where we have
 240    *
 241    *        R - - - - - - - M . -  -  -  -  R'
 242    *                            ` .
 243    *                                ` .
 244    *                                    S
 245    *
 246    * and wish to say that the vector RM should be similar to MS
 247    * or to put it another way S = M + RM
 248    *
 249    * NB this is not symmetric wrt R and S since it divides by
 250    * |SM| but not |RM| so you probably want to call it twice.
 251    *
 252    * Details:
 253    *
 254    *   Let  R' = M + SM
 255    *        D  = R' - R
 256    *
 257    * Then the (1/delta)th power of the cost is
 258    *      proportional to |D|, and
 259    *      inversely proportional to |SM|
 260    * except that |D| is measured in a wierd way which counts
 261    * distance in the same direction as SM 1/lambda times as much
 262    * ie the equipotential surfaces are ellipsoids around R',
 263    * lengthened by lambda in the direction of RM.
 264    *
 265    * So
 266    *                                                               delta
 267    *                [       -1                                    ]
 268    *   cost      =  [ lambda  . ( D . SM/|SM| ) + | D x SM/|SM| | ]
 269    *       R,S,M    [ ------------------------------------------- ]
 270    *                [                      |SM|                   ]
 271    *
 272    */
 273
 274 static double vertex_one_displ_cost(const double r[D3], const double s[D3],
 275                                     const double midpoint[D3],
 276                                     double delta, double inv_lambda) {
 277   const double smlen2_epsilon= 1e-12;
 278   double sm[D3], d[D3], ddot, dcross[D3];
 279   int k;
 280
 281   K sm[k]= -s[k] + midpoint[k];
 282   K d[k]= midpoint[k] + sm[k] - r[k];
 283   ddot= dotprod(d,sm);
 284   xprod(dcross, d,sm);
 285   double smlen2= magnD2(sm);
 286   double cost_basis= inv_lambda * ddot + magnD(dcross);
 287   double cost= pow(cost_basis / (smlen2 + smlen2_epsilon), delta);
 288
 289   return cost;
 290 }
 291
 292 /*---------- displacement of vertices opposite at a vertex ----------*/
 293
 294   /*
 295    *   At vertex Q considering edge direction e to R
 296    *   and corresponding opposite edge to S.
 297    *
 298    *   This is vertex displacement as above with M=Q
 299    */
 300
 301 double vertex_displacement_cost(const Vertices vertices, int section) {
 302   const double inv_lambda= 1.0/1; //2;
 303   const double delta= 6;
 304
 305   int si,e,qi,ri;
 306   double total_cost= 0;
 307
 308   FOR_EDGE(qi,e,ri, OUTER) {
 309     si= EDGE_END2(qi,(e+3)%V6); if (si<0) continue;
 310
 311     total_cost += vertex_one_displ_cost(vertices[ri], vertices[si], vertices[qi],
 312                                         delta, inv_lambda);
 313   }
 314   return total_cost;
 315 }
 316
 317 /*---------- displacement of vertices opposite at an edge ----------*/
 318
 319   /*
 320    *   At edge PQ considering vertices R and S (see diagram
 321    *   below for overly sharp edge cost).
 322    *
 323    *   Let  M  = midpoint of PQ
 324    */
 325
 326 double vertex_edgewise_displ_cost(const Vertices vertices, int section) {
 327   const double inv_lambda= 1.0/1; //2;
 328   const double delta= 6;
 329
 330   int pi,e,qi,ri,si, k;
 331   double m[D3];
 332   double total_cost= 0;
 333
 334   FOR_EDGE(pi,e,qi, OUTER) {
 335     si= EDGE_END2(pi,(e+V6-1)%V6);  if (si<0) continue;
 336     ri= EDGE_END2(pi,(e   +1)%V6);  if (ri<0) continue;
 337
 338     K m[k]= 0.5 * (vertices[pi][k] + vertices[qi][k]);
 339
 340     total_cost += vertex_one_displ_cost(vertices[ri], vertices[si], m,
 341                                         delta, inv_lambda);
 342   }
 343   return total_cost;
 344 }
 345
 346
 347 /*---------- at-vertex edge angles ----------*/
 348
 349   /*
 350    * Definition:
 351    *
 352    *    At each vertex Q, in each direction e:
 353    *
 354    *                                  e
 355    *                           Q ----->----- R
 356    *                      _,-'\__/
 357    *                  _,-'       delta
 358    *               P '
 359    *
 360    *                      r
 361    *       cost    = delta          (we use r=3)
 362    *           Q,e
 363    *
 364    *
 365    * Calculation:
 366    *
 367    *      Let vector A = PQ
 368    *                 B = QR
 369    *
 370    *                   -1   A . B
 371    *      delta =  tan     -------
 372    *                      | A x B |
 373    *
 374    *      which is always in the range 0..pi because the denominator
 375    *      is nonnegative.  We add epsilon to |AxB| to avoid division
 376    *      by zero.
 377    *
 378    *                     r
 379    *      cost    = delta
 380    *          Q,e
 381    */
 382
 383 double line_bending_cost(const Vertices vertices, int section) {
 384   static const double axb_epsilon= 1e-6;
 385   static const double exponent_r= 4;
 386
 387   int pi,e,qi,ri, k;
 388   double  a[D3], b[D3], axb[D3];
 389   double total_cost= 0;
 390
 391   FOR_EDGE(qi,e,ri, OUTER) {
 392     pi= EDGE_END2(qi,(e+3)%V6); if (pi<0) continue;
 393
 394 //if (!(qi&XMASK)) fprintf(stderr,"%02x-%02x-%02x (%d)\n",pi,qi,ri,e);
 395
 396     K a[k]= -vertices[pi][k] + vertices[qi][k];
 397     K b[k]= -vertices[qi][k] + vertices[ri][k];
 398
 399     xprod(axb,a,b);
 400
 401     double delta= atan2(magnD(axb) + axb_epsilon, dotprod(a,b));
 402     double cost= pow(delta,exponent_r);
 403
 404     total_cost += cost;
 405   }
 406   return total_cost;
 407 }
 408
 409 /*---------- edge length variation ----------*/
 410
 411   /*
 412    * Definition:
 413    *
 414    *    See the diagram above.
 415    *                                r
 416    *       cost    = ( |PQ| - |QR| )
 417    *           Q,e
 418    */
 419
 420 double edge_length_variation_cost(const Vertices vertices, int section) {
 421   double diff, cost= 0, exponent_r= 2;
 422   int q, e,r, eback;
 423
 424   FOR_EDGE(q,e,r, OUTER) {
 425     eback= edge_reverse(q,e);
 426     diff= edge_lengths[q][e] - edge_lengths[q][eback];
 427     cost += pow(diff,exponent_r);
 428   }
 429   return cost;
 430 }
 431
 432 /*---------- proportional edge length variation ----------*/
 433
 434   /*
 435    * Definition:
 436    *
 437    *    See the diagram above.
 438    *                                r
 439    *       cost    = ( |PQ| - |QR| )
 440    *           Q,e
 441    */
 442
 443 double prop_edge_length_variation_cost(const Vertices vertices, int section) {
 444   const double num_epsilon= 1e-6;
 445
 446   double cost= 0, exponent_r= 2;
 447   int q, e,r, eback;
 448
 449   FOR_EDGE(q,e,r, OUTER) {
 450     eback= edge_reverse(q,e);
 451     double le= edge_lengths[q][e];
 452     double leback= edge_lengths[q][eback];
 453     double diff= le - leback;
 454     double num= MIN(le, leback);
 455     cost += pow(diff / (num + num_epsilon), exponent_r);
 456   }
 457   return cost;
 458 }
 459
 460 /*---------- rim proximity cost ----------*/
 461
 462 static void find_nearest_oncircle(double oncircle[D3], const double p[D3]) {
 463   /* By symmetry, nearest point on circle is the one with
 464    * the same angle subtended at the z axis. */
 465   oncircle[0]= p[0];
 466   oncircle[1]= p[1];
 467   oncircle[2]= 0;
 468   double mult= 1.0/ magnD(oncircle);
 469   oncircle[0] *= mult;
 470   oncircle[1] *= mult;
 471 }
 472
 473 double rim_proximity_cost(const Vertices vertices, int section) {
 474   double oncircle[D3], cost=0;
 475   int v;
 476
 477   FOR_VERTEX(v, OUTER) {
 478     int y= v >> YSHIFT;
 479     int nominal_edge_distance= y <= Y/2 ? y : Y-1-y;
 480     if (nominal_edge_distance==0) continue;
 481
 482     find_nearest_oncircle(oncircle, vertices[v]);
 483
 484     cost +=
 485       vertex_mean_edge_lengths[v] *
 486       (nominal_edge_distance*nominal_edge_distance) /
 487       (hypotD2(vertices[v], oncircle) + 1e-6);
 488   }
 489   return cost;
 490 }
 491
 492 /*---------- noncircular rim cost ----------*/
 493
 494 double noncircular_rim_cost(const Vertices vertices, int section) {
 495   int vy,vx,v;
 496   double cost= 0.0;
 497   double oncircle[3];
 498
 499   FOR_RIM_VERTEX(vy,vx,v, OUTER) {
 500     find_nearest_oncircle(oncircle, vertices[v]);
 501
 502     double d2= hypotD2(vertices[v], oncircle);
 503     cost += d2*d2;
 504   }
 505   return cost;
 506 }
 507
 508 /*---------- rim contact angle rotation ----------*/
 509
 510 void compute_rim_twist_angles(const Vertices vertices, int section) {
 511   double oncircle[D3], distance[D3];
 512   int vpy,vpx,v,k;
 513
 514   FOR_NEAR_RIM_VERTEX(vpy,vpx,v, 1,OUTER) {
 515     find_nearest_oncircle(oncircle, vertices[v]);
 516     /* we are interested in the angle subtended at the rim, from the
 517      * rim's point of view. */
 518     K distance[k]= vertices[v][k] - oncircle[k];
 519     double distance_positive_z= distance[3];
 520     double distance_radial_outwards= dotprod(distance, oncircle);
 521     rim_vertex_angles[v]= atan2(distance_positive_z, distance_radial_outwards);
 522   }
 523 }
 524
 525 double rim_twist_cost(const Vertices vertices, int section) {
 526   double total_cost= 0;
 527   int vpy,vpx,v0,v1;
 528
 529   FOR_NEAR_RIM_VERTEX(vpy,vpx,v0, 1,OUTER) {
 530     v1= EDGE_END2(v0,0);  assert(v1!=0);
 531     double delta= rim_vertex_angles[v0] - rim_vertex_angles[v1];
 532     if (delta < M_PI) delta += 2*M_PI;
 533     if (delta > M_PI) delta -= 2*M_PI;
 534
 535     double cost= pow(delta, 4);
 536     total_cost += cost;
 537   }
 538
 539   return total_cost;
 540 }
 541
 542 /*---------- overly sharp edge cost ----------*/
 543
 544   /*
 545    *
 546    *                Q `-_
 547    *              / |    `-_                           P'Q' ------ S'
 548    *             /  |       `-.                      _,' `.       .
 549    *            /   |           S                 _,'     :      .
 550    *           /    |      _,-'                _,'        :r    .r
 551    *          /     |  _,-'                R' '           `.   .
 552    *         /    , P '                        ` .   r     :  .
 553    *        /  ,-'                                  `  .   :
 554    *       /,-'                                          ` C'
 555    *      /'
 556    *     R
 557    *
 558    *
 559    *
 560    *  Let delta =  angle between two triangles' normals
 561    *
 562    *  Giving energy contribution:
 563    *
 564    *                                   2
 565    *    E             =  F   .  delta
 566    *     vd, edge PQ      vd
 567    */
 568
 569 double edge_angle_cost(const Vertices vertices, int section) {
 570   double pq1[D3], rp[D3], ps[D3], rp_2d[D3], ps_2d[D3], rs_2d[D3];
 571   double a,b,c,s,r;
 572   const double minradius_base= 0.2;
 573
 574   int pi,e,qi,ri,si, k;
 575 //  double our_epsilon=1e-6;
 576   double total_cost= 0;
 577
 578   FOR_EDGE(pi,e,qi, OUTER) {
 579 //    if (!(RIM_VERTEX_P(pi) || RIM_VERTEX_P(qi))) continue;
 580
 581     si= EDGE_END2(pi,(e+V6-1)%V6);  if (si<0) continue;
 582     ri= EDGE_END2(pi,(e   +1)%V6);  if (ri<0) continue;
 583
 584     K {
 585       pq1[k]= -vertices[pi][k] + vertices[qi][k];
 586       rp[k]=  -vertices[ri][k] + vertices[pi][k];
 587       ps[k]=  -vertices[pi][k] + vertices[si][k];
 588     }
 589
 590     normalise(pq1,1,1e-6);
 591     xprod(rp_2d, rp,pq1); /* projects RP into plane normal to PQ */
 592     xprod(ps_2d, ps,pq1); /* likewise PS */
 593     K rs_2d[k]= rp_2d[k] + ps_2d[k];
 594     /* radius of circumcircle of R'P'S' from Wikipedia
 595      * `Circumscribed circle' */
 596     a= magnD(rp_2d);
 597     b= magnD(ps_2d);
 598     c= magnD(rs_2d);
 599     s= 0.5*(a+b+c);
 600     r= a*b*c / sqrt((a+b+c)*(a-b+c)*(b-c+a)*(c-a+b) + 1e-6);
 601
 602     double minradius= minradius_base + edge_angle_cost_circcircrat*(a+b);
 603     double deficit= minradius - r;
 604     if (deficit < 0) continue;
 605     double cost= deficit*deficit;
 606
 607     total_cost += cost;
 608   }
 609
 610   return total_cost;
 611 }
 612
 613 /*---------- small triangles cost ----------*/
 614
 615   /*
 616    * Consider a triangle PQS
 617    *
 618    * Cost is 1/( area^2 )
 619    */
 620
 621 double small_triangles_cost(const Vertices vertices, int section) {
 622   double pq[D3], ps[D3];
 623   double x[D3];
 624   int pi,e,qi,si, k;
 625 //  double our_epsilon=1e-6;
 626   double total_cost= 0;
 627
 628   FOR_EDGE(pi,e,qi, OUTER) {
 629 //    if (!(RIM_VERTEX_P(pi) || RIM_VERTEX_P(qi))) continue;
 630
 631     si= EDGE_END2(pi,(e+V6-1)%V6);  if (si<0) continue;
 632
 633     K {
 634       pq[k]= vertices[qi][k] - vertices[pi][k];
 635       ps[k]= vertices[si][k] - vertices[pi][k];
 636     }
 637     xprod(x, pq,ps);
 638
 639     double cost= 1/(magnD2(x) + 0.01);
 640
 641 //double cost= pow(magnD(spqxpqr), 3);
 642 //assert(dot>=-1 && dot <=1);
 643 //double cost= 1-dot;
 644     total_cost += cost;
 645   }
 646
 647   return total_cost;
 648 }
 649
 650 /*---------- nonequilateral triangles cost ----------*/
 651
 652   /*
 653    * Consider a triangle PQR
 654    *
 655    * let edge lengths a=|PQ| b=|QR| c=|RP|
 656    *
 657    * predicted edge length p = 1/3 * (a+b+c)
 658    *
 659    * compute cost for each x in {a,b,c}
 660    *
 661    *
 662    *      cost      = (x-p)^2 / p^2
 663    *          PQR,x
 664    */
 665
 666 double nonequilateral_triangles_cost(const Vertices vertices, int section) {
 667   double pr[D3], abc[3];
 668   int pi,e0,e1,qi,ri, k,i;
 669   double our_epsilon=1e-6;
 670   double total_cost= 0;
 671
 672   FOR_EDGE(pi,e0,qi, OUTER) {
 673     e1= (e0+V6-1)%V6;
 674     ri= EDGE_END2(pi,e1);  if (ri<0) continue;
 675
 676     K pr[k]= -vertices[pi][k] + vertices[ri][k];
 677
 678     abc[0]= edge_lengths[pi][e0]; /* PQ */
 679     abc[1]= edge_lengths[qi][e1]; /* QR */
 680     abc[2]= magnD(pr);
 681
 682     double p= (1/3.0) * (abc[0]+abc[1]+abc[2]);
 683     double p_inv2= 1/(p*p + our_epsilon);
 684
 685     for (i=0; i<3; i++) {
 686       double diff= (abc[i] - p);
 687       double cost= diff*diff * p_inv2;
 688       total_cost += cost;
 689     }
 690   }
 691
 692   return total_cost;
 693 }