|
@@ -0,0 +1,931 @@
|
|
|
+#include <math.h>
|
|
|
+#include <stdlib.h>
|
|
|
+#include <stdio.h>
|
|
|
+#include <string.h>
|
|
|
+#define R (sqrt(5)-1.)*0.5
|
|
|
+#define C (1.0-R)
|
|
|
+#define SHFT2(a,b,c) (a)=(b);(b)=(c);
|
|
|
+#define SHFT3(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
|
|
|
+
|
|
|
+
|
|
|
+//.........................COMPILE TO SHARED LIBRARY FOR USE WITH PYTHON.........................................
|
|
|
+
|
|
|
+// gcc -shared overlap_algorithm.c -o overlap_algorithm.so -lm -fPIC -O3
|
|
|
+
|
|
|
+
|
|
|
+//..................................LINE MINIMIZATION ALGORITHMS.................................................
|
|
|
+
|
|
|
+
|
|
|
+double gss_eigenvalue_minim(double a, double b, double eps, double t, double *matA, double *matB, int early_stop,
|
|
|
+ double f ( double x, double *mat1, double *mat2 ), double *xmin, int *feval) {
|
|
|
+
|
|
|
+ double f1,f2,x0,x1,x2,x3,m,tol,t2;
|
|
|
+ int num_eval;
|
|
|
+
|
|
|
+ x0=a;
|
|
|
+ x3=b;
|
|
|
+
|
|
|
+ x1 = a + C * (b - a);
|
|
|
+ x2 = a + R * (b - a);
|
|
|
+
|
|
|
+ f1=f(x1, matA, matB);
|
|
|
+ f2=f(x2, matA, matB);
|
|
|
+ num_eval = 2;
|
|
|
+
|
|
|
+ for ( ; ; )
|
|
|
+ {
|
|
|
+ m = 0.5 * ( x1 + x2 ) ;
|
|
|
+ tol = eps * fabs ( m ) + t;
|
|
|
+ t2 = 2.0 * tol;
|
|
|
+/*
|
|
|
+ Check the stopping criterion.
|
|
|
+*/
|
|
|
+ if (fabs(x3-x0) < t2)
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( early_stop == 1 && (f1 < -1. || f2 < -1.)) {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (f2 < f1) {
|
|
|
+ SHFT3(x0,x1,x2,R*x1+C*x3);
|
|
|
+ SHFT2(f1,f2,f(x2, matA, matB));
|
|
|
+ num_eval++;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ SHFT3(x3,x2,x1,R*x2+C*x0);
|
|
|
+ SHFT2(f2,f1,f(x1, matA, matB));
|
|
|
+ num_eval++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (f1 < f2) {
|
|
|
+ *xmin=x1;
|
|
|
+ *feval = num_eval;
|
|
|
+ return f1;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ *xmin=x2;
|
|
|
+ *feval = num_eval;
|
|
|
+ return f2;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double brent_eigenvalue_minim ( double a, double b, double eps, double t,
|
|
|
+ double *matA, double *matB, int early_stop,
|
|
|
+ double f ( double x, double *mat1, double *mat2 ), double *x, int *feval)
|
|
|
+
|
|
|
+/*
|
|
|
+
|
|
|
+ Adapted version of LOCAL_MIN function for finding eigenvalue extrema.
|
|
|
+
|
|
|
+ Authors of original minimization code:
|
|
|
+
|
|
|
+ Orignal FORTRAN77 version by Richard Brent.
|
|
|
+ C version by John Burkardt.
|
|
|
+
|
|
|
+ Licensing:
|
|
|
+
|
|
|
+ This code is distributed under the GNU LGPL license.
|
|
|
+
|
|
|
+ Parameters:
|
|
|
+
|
|
|
+ Input, double A, B, the endpoints of the interval.
|
|
|
+
|
|
|
+ Input, double EPS, a positive relative error tolerance.
|
|
|
+ EPS should be no smaller than twice the relative machine precision,
|
|
|
+ and preferably not much less than the square root of the relative
|
|
|
+ machine precision.
|
|
|
+
|
|
|
+ Input, double T, a positive absolute error tolerance.
|
|
|
+
|
|
|
+ Input, double F ( double x ,double *mat1, double *mat2 ), a user-supplied
|
|
|
+ function whose local minimum along x is being sought, with matrices mat1
|
|
|
+ and mat2 as parameters.
|
|
|
+
|
|
|
+ Output, double *X, the estimated value of an abscissa
|
|
|
+ for which F attains a local minimum value in [A,B].
|
|
|
+
|
|
|
+ Output, double *feval, the number of function evaluations.
|
|
|
+
|
|
|
+ Output, double LOCAL_MIN, the value F(X).
|
|
|
+*/
|
|
|
+{
|
|
|
+ double c;
|
|
|
+ double d;
|
|
|
+ double e;
|
|
|
+ double fu;
|
|
|
+ double fv;
|
|
|
+ double fw;
|
|
|
+ double fx;
|
|
|
+ double m;
|
|
|
+ double p;
|
|
|
+ double q;
|
|
|
+ double r;
|
|
|
+ double sa;
|
|
|
+ double sb;
|
|
|
+ double t2;
|
|
|
+ double tol;
|
|
|
+ double u;
|
|
|
+ double v;
|
|
|
+ double w;
|
|
|
+ int num_eval;
|
|
|
+/*
|
|
|
+ C is the square of the inverse of the golden ratio.
|
|
|
+*/
|
|
|
+ c = 0.5 * ( 3.0 - sqrt ( 5.0 ) );
|
|
|
+
|
|
|
+ sa = a;
|
|
|
+ sb = b;
|
|
|
+ *x = sa + c * ( b - a );
|
|
|
+ w = *x;
|
|
|
+ v = w;
|
|
|
+ e = 0.0;
|
|
|
+ fx = f ( *x, matA, matB );
|
|
|
+ fw = fx;
|
|
|
+ fv = fw;
|
|
|
+ num_eval = 1;
|
|
|
+
|
|
|
+ for ( ; ; )
|
|
|
+ {
|
|
|
+ m = 0.5 * ( sa + sb ) ;
|
|
|
+ tol = eps * fabs ( *x ) + t;
|
|
|
+ t2 = 2.0 * tol;
|
|
|
+/*
|
|
|
+ Check the stopping criterion.
|
|
|
+*/
|
|
|
+ if ( fabs ( *x - m ) <= t2 - 0.5 * ( sb - sa ) )
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+/*
|
|
|
+ If we are looking for the maximum of smallest eigenvalue and the
|
|
|
+ calculation goes over the value of 1 (under -1 because algorithm finds min),
|
|
|
+ we can stop as that means the ellipses certainly do not overlap
|
|
|
+*/
|
|
|
+ if ( early_stop == 1 && fx < -1.) {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+/*
|
|
|
+ Fit a parabola.
|
|
|
+*/
|
|
|
+ r = 0.0;
|
|
|
+ q = r;
|
|
|
+ p = q;
|
|
|
+
|
|
|
+ if ( tol < fabs ( e ) )
|
|
|
+ {
|
|
|
+ r = ( *x - w ) * ( fx - fv );
|
|
|
+ q = ( *x - v ) * ( fx - fw );
|
|
|
+ p = ( *x - v ) * q - ( *x - w ) * r;
|
|
|
+ q = 2.0 * ( q - r );
|
|
|
+ if ( 0.0 < q )
|
|
|
+ {
|
|
|
+ p = - p;
|
|
|
+ }
|
|
|
+ q = fabs ( q );
|
|
|
+ r = e;
|
|
|
+ e = d;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( fabs ( p ) < fabs ( 0.5 * q * r ) &&
|
|
|
+ q * ( sa - *x ) < p &&
|
|
|
+ p < q * ( sb - *x ) )
|
|
|
+ {
|
|
|
+/*
|
|
|
+ Take the parabolic interpolation step.
|
|
|
+*/
|
|
|
+ d = p / q;
|
|
|
+ u = *x + d;
|
|
|
+/*
|
|
|
+ F must not be evaluated too close to A or B.
|
|
|
+*/
|
|
|
+ if ( ( u - sa ) < t2 || ( sb - u ) < t2 )
|
|
|
+ {
|
|
|
+ if ( *x < m )
|
|
|
+ {
|
|
|
+ d = tol;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ d = - tol;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+/*
|
|
|
+ A golden-section step.
|
|
|
+*/
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if ( *x < m )
|
|
|
+ {
|
|
|
+ e = sb - *x;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ e = sa - *x;
|
|
|
+ }
|
|
|
+ d = c * e;
|
|
|
+ }
|
|
|
+/*
|
|
|
+ F must not be evaluated too close to X.
|
|
|
+*/
|
|
|
+ if ( tol <= fabs ( d ) )
|
|
|
+ {
|
|
|
+ u = *x + d;
|
|
|
+ }
|
|
|
+ else if ( 0.0 < d )
|
|
|
+ {
|
|
|
+ u = *x + tol;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ u = *x - tol;
|
|
|
+ }
|
|
|
+
|
|
|
+ fu = f ( u, matA, matB );
|
|
|
+ num_eval++;
|
|
|
+/*
|
|
|
+ Update A, B, V, W, and X.
|
|
|
+*/
|
|
|
+ if ( fu <= fx )
|
|
|
+ {
|
|
|
+ if ( u < *x )
|
|
|
+ {
|
|
|
+ sb = *x;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ sa = *x;
|
|
|
+ }
|
|
|
+ v = w;
|
|
|
+ fv = fw;
|
|
|
+ w = *x;
|
|
|
+ fw = fx;
|
|
|
+ *x = u;
|
|
|
+ fx = fu;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if ( u < *x )
|
|
|
+ {
|
|
|
+ sa = u;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ sb = u;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( fu <= fw || w == *x )
|
|
|
+ {
|
|
|
+ v = w;
|
|
|
+ fv = fw;
|
|
|
+ w = u;
|
|
|
+ fw = fu;
|
|
|
+ }
|
|
|
+ else if ( fu <= fv || v == *x || v == w )
|
|
|
+ {
|
|
|
+ v = u;
|
|
|
+ fv = fu;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ *feval = num_eval;
|
|
|
+ return fx;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double r8_epsilon ( )
|
|
|
+/*
|
|
|
+ Purpose:
|
|
|
+
|
|
|
+ R8_EPSILON returns the R8 round off unit.
|
|
|
+
|
|
|
+ Discussion:
|
|
|
+
|
|
|
+ R8_EPSILON is a number R which is a power of 2 with the property that,
|
|
|
+ to the precision of the computer's arithmetic,
|
|
|
+ 1 < 1 + R
|
|
|
+ but
|
|
|
+ 1 = ( 1 + R / 2 )
|
|
|
+
|
|
|
+ Parameters:
|
|
|
+
|
|
|
+ Output, double R8_EPSILON, the R8 round-off unit.
|
|
|
+*/
|
|
|
+{
|
|
|
+ const double value = 2.220446049250313E-016;
|
|
|
+
|
|
|
+ return value;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//.................................EIGENVALUE AND EIGENVECTOR CALCULATION.............................................
|
|
|
+
|
|
|
+
|
|
|
+double determinant(double* a){
|
|
|
+ double det = a[0] * (a[4]*a[8] - a[7]*a[5]) - a[3] * (a[1]*a[8] - a[7]*a[2]) + a[6] * (a[1]*a[5] - a[4]*a[2]);
|
|
|
+ return det;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void eigenvalues(double* matrix, double* eigs){
|
|
|
+
|
|
|
+ double p1 = matrix[1]*matrix[1] + matrix[2]*matrix[2] + matrix[5]*matrix[5];
|
|
|
+ double q, p2, p;
|
|
|
+ double aux[9];
|
|
|
+ double r, phi;
|
|
|
+ double pi = M_PI;
|
|
|
+
|
|
|
+ if (p1 == 0) {
|
|
|
+ eigs[0] = matrix[0];
|
|
|
+ eigs[1] = matrix[4];
|
|
|
+ eigs[2] = matrix[8];
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ q = (matrix[0]+matrix[4]+matrix[8])/3;
|
|
|
+ p2 = (matrix[0]-q)*(matrix[0]-q) + (matrix[4]-q)*(matrix[4]-q) + (matrix[8]-q)*(matrix[8]-q) + 2*p1;
|
|
|
+ p = sqrt(p2 / 6);
|
|
|
+
|
|
|
+ for (int i=0; i<9; i++) {
|
|
|
+ aux[i] = matrix[i];
|
|
|
+ if (i % 4 == 0) {
|
|
|
+ aux[i] -= q;
|
|
|
+ }
|
|
|
+ aux[i] = (1 / p) * aux[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ r = determinant(aux) / 2;
|
|
|
+
|
|
|
+ if (r <= -1){
|
|
|
+ phi = pi / 3;
|
|
|
+ }
|
|
|
+ else if (r >= 1){
|
|
|
+ phi = 0;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ phi = acos(r) / 3;
|
|
|
+ }
|
|
|
+
|
|
|
+ eigs[2] = q + 2 * p * cos(phi);
|
|
|
+ eigs[0] = q + 2 * p * cos(phi + (2*pi/3));
|
|
|
+ eigs[1] = 3 * q - eigs[0] - eigs[2];
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void eigenvector(double* matrix, int which, double* vector){
|
|
|
+
|
|
|
+ double eigs[3];
|
|
|
+ double mat_lbd1[9];
|
|
|
+ double mat_lbd2[9];
|
|
|
+ double product[9];
|
|
|
+ double norm1, norm2, norm3, norm;
|
|
|
+ int idx;
|
|
|
+
|
|
|
+ eigenvalues(matrix, eigs);
|
|
|
+
|
|
|
+ for (int i=0; i<9; i++) {
|
|
|
+ mat_lbd1[i] = matrix[i];
|
|
|
+ mat_lbd2[i] = matrix[i];
|
|
|
+ if (i % 4 == 0) {
|
|
|
+ if (which == 1) {
|
|
|
+ mat_lbd1[i] -= eigs[1];
|
|
|
+ mat_lbd2[i] -= eigs[2];
|
|
|
+ }
|
|
|
+ else if (which == 2) {
|
|
|
+ mat_lbd1[i] -= eigs[0];
|
|
|
+ mat_lbd2[i] -= eigs[2];
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ mat_lbd1[i] -= eigs[0];
|
|
|
+ mat_lbd2[i] -= eigs[1];
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int i=0; i<3; i++) {
|
|
|
+ for (int k=0; k<3; k++) {
|
|
|
+ product[3*i+k] = mat_lbd1[3*i] * mat_lbd2[k] + mat_lbd1[3*i+1] * mat_lbd2[3+k] + mat_lbd1[3*i+2] * mat_lbd2[6+k];
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ norm1 = product[0]*product[0] + product[3]*product[3] + product[6]*product[6];
|
|
|
+ norm2 = product[1]*product[1] + product[4]*product[4] + product[7]*product[7];
|
|
|
+ norm3 = product[2]*product[2] + product[5]*product[5] + product[8]*product[8];
|
|
|
+
|
|
|
+ if (norm1 > norm2 && norm1 > norm3) {
|
|
|
+ idx = 0;
|
|
|
+ norm = norm1;
|
|
|
+ }
|
|
|
+ else if (norm2 > norm1 && norm2 > norm3) {
|
|
|
+ idx = 1;
|
|
|
+ norm = norm2;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ idx = 2;
|
|
|
+ norm = norm3;
|
|
|
+ }
|
|
|
+
|
|
|
+ norm = 1/sqrt(norm);
|
|
|
+
|
|
|
+ vector[0] = product[idx]*norm;
|
|
|
+ vector[1] = product[3+idx]*norm;
|
|
|
+ vector[2] = product[6+idx]*norm;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//.....................................EIGENVALUE EXTREMIZATION.................................................
|
|
|
+
|
|
|
+
|
|
|
+double neg_min_eig(double lbd, double* matA, double* matB){
|
|
|
+
|
|
|
+ double lcb[9];
|
|
|
+ double eigs[3];
|
|
|
+ double result;
|
|
|
+
|
|
|
+ for (int i=0; i<9; i++) {
|
|
|
+ lcb[i] = lbd * matA[i] + (1 - lbd) * matB[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ eigenvalues(lcb, eigs);
|
|
|
+
|
|
|
+ result = -eigs[0];
|
|
|
+
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double med_eig(double lbd, double* matA, double* matB){
|
|
|
+
|
|
|
+ double lcb[9];
|
|
|
+ double eigs[3];
|
|
|
+ double result;
|
|
|
+
|
|
|
+ for (int i=0; i<9; i++) {
|
|
|
+ lcb[i] = lbd * matA[i] + (1 - lbd) * matB[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ eigenvalues(lcb, eigs);
|
|
|
+
|
|
|
+ result = eigs[1];
|
|
|
+
|
|
|
+
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double max_min_eig_brent(double* matA, double* matB, int early_stop, double* t_point, int* feval){
|
|
|
+
|
|
|
+ double a = r8_epsilon();
|
|
|
+ double b = 1. - r8_epsilon();
|
|
|
+ double e = r8_epsilon(); // relative error
|
|
|
+ double t = 1.0E-7; // absolute error
|
|
|
+ double solution;
|
|
|
+ double initial = 0.;
|
|
|
+ double *y = &initial;
|
|
|
+
|
|
|
+
|
|
|
+ solution = -brent_eigenvalue_minim(a, b, e, t, matA, matB, early_stop, neg_min_eig, t_point, feval);
|
|
|
+
|
|
|
+
|
|
|
+ return solution;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double min_med_eig_brent(double* matA, double* matB, double* t_point, int* feval){
|
|
|
+
|
|
|
+ double a = r8_epsilon();
|
|
|
+ double b = 1. - r8_epsilon();
|
|
|
+ double e = r8_epsilon(); // relative error
|
|
|
+ double t = 1.0E-7; // absolute error
|
|
|
+ int early_stop = 0;
|
|
|
+ double solution;
|
|
|
+ double initial = 0.;
|
|
|
+ double *y = &initial;
|
|
|
+
|
|
|
+ solution = brent_eigenvalue_minim(a, b, e, t, matA, matB, early_stop, med_eig, t_point, feval);
|
|
|
+
|
|
|
+ return solution;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double max_min_eig_gss(double* matA, double* matB, int early_stop, double* t_point, int* feval){
|
|
|
+
|
|
|
+ double a = r8_epsilon();
|
|
|
+ double b = 1. - r8_epsilon();
|
|
|
+ double c = 0.5;
|
|
|
+ double m = 100.;
|
|
|
+ double mechep = r8_epsilon();
|
|
|
+ double e = r8_epsilon(); // relative error
|
|
|
+ double t = 1.0E-7; // absolute error
|
|
|
+ double solution;
|
|
|
+
|
|
|
+ solution = -gss_eigenvalue_minim(a, b, e, t, matA, matB, early_stop, neg_min_eig, t_point, feval);
|
|
|
+
|
|
|
+ return solution;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+double min_med_eig_gss(double* matA, double* matB, double* t_point, int* feval){
|
|
|
+
|
|
|
+ double a = r8_epsilon();
|
|
|
+ double b = 1. - r8_epsilon();
|
|
|
+ double c = 0.5;
|
|
|
+ double m = 100.;
|
|
|
+ double mechep = r8_epsilon();
|
|
|
+ double e = r8_epsilon(); // relative error
|
|
|
+ double t = 1.0E-7; // absolute error
|
|
|
+ int early_stop = 0;
|
|
|
+ double solution;
|
|
|
+
|
|
|
+ solution = gss_eigenvalue_minim(a, b, e, t, matA, matB, early_stop, med_eig, t_point, feval);
|
|
|
+
|
|
|
+ return solution;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//.......................................CONTACT FUNCTION...................................................
|
|
|
+
|
|
|
+
|
|
|
+void cross_product(double* coord, double* orient, double* cross) {
|
|
|
+
|
|
|
+ cross[0] = coord[1]*orient[2] - coord[2]*orient[1];
|
|
|
+ cross[1] = coord[2]*orient[0] - coord[0]*orient[2];
|
|
|
+ cross[2] = coord[0]*orient[1] - coord[1]*orient[0];
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double contact_function(double a0, double a1, double b0, double b1,
|
|
|
+ double* coord0, double* orient0, double* coord1, double* orient1, int minimizer,
|
|
|
+ double* other_results, int* fevals, int* branch) {
|
|
|
+
|
|
|
+ /*
|
|
|
+ Other results array: [min_eig_T, med_eig_T, min_vec_scalar, med_vec_scalar]
|
|
|
+ fevals array: [feval_min, feval_med]
|
|
|
+ */
|
|
|
+
|
|
|
+ double a_inter;
|
|
|
+ if (a0 < a1) {
|
|
|
+ a_inter = a0;
|
|
|
+ a0 = a1;
|
|
|
+ a1 = a_inter;
|
|
|
+ }
|
|
|
+
|
|
|
+ double cross0[3];
|
|
|
+ double cross1[3];
|
|
|
+ double lin_interpolation[9];
|
|
|
+ double qf0[9];
|
|
|
+ double qf1[9];
|
|
|
+ double min_eig;
|
|
|
+ double med_eig;
|
|
|
+ double eigvec_mineig[3];
|
|
|
+ double mineig_coord0=0.;
|
|
|
+ double mineig_coord1=0.;
|
|
|
+ double inv2a0=1/(a0 * a0);
|
|
|
+ double inv2a1=1/(a1 * a1);
|
|
|
+ double inv2b0=1/(b0 * b0);
|
|
|
+ double inv2b1=1/(b1 * b1);
|
|
|
+ double limit=inv2a0;
|
|
|
+
|
|
|
+ cross_product(coord0, orient0, cross0);
|
|
|
+ cross_product(coord1, orient1, cross1);
|
|
|
+
|
|
|
+ /* Trasformation of quadratic forms */
|
|
|
+ for (int i=0; i<3; i++){
|
|
|
+ for (int l=0; l<3; l++){
|
|
|
+ qf0[3*i+l] = orient0[i]*inv2a0*orient0[l] + cross0[i]*inv2b0*cross0[l];
|
|
|
+ qf1[3*i+l] = orient1[i]*inv2a1*orient1[l] + cross1[i]*inv2b1*cross1[l];
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (minimizer == 0) {
|
|
|
+ min_eig = max_min_eig_brent(qf0, qf1, 0, &other_results[0], &fevals[0]);
|
|
|
+ }
|
|
|
+ else if (minimizer == 1) {
|
|
|
+ min_eig = max_min_eig_brent(qf0, qf1, 1, &other_results[0], &fevals[0]);
|
|
|
+ }
|
|
|
+ else if (minimizer == 2) {
|
|
|
+ min_eig = max_min_eig_gss(qf0, qf1, 0, &other_results[0], &fevals[0]);
|
|
|
+ }
|
|
|
+ else if (minimizer == 3) {
|
|
|
+ min_eig = max_min_eig_gss(qf0, qf1, 1, &other_results[0], &fevals[0]);
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ return -1.;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ /* eigenvector corresponding to maximal 1st eigenvalue (1st intersection coordinate)*/
|
|
|
+ for (int i=0; i<9; i++) {
|
|
|
+ lin_interpolation[i] = other_results[0] * qf0[i] + (1 - other_results[0]) * qf1[i];
|
|
|
+ }
|
|
|
+ eigenvector(lin_interpolation, 1, eigvec_mineig);
|
|
|
+
|
|
|
+ /* scalar product between eigenvectors and position vectors for both ellipses in the pair */
|
|
|
+ for (int i=0; i<3; i++) {
|
|
|
+ mineig_coord0 += eigvec_mineig[i]*coord0[i];
|
|
|
+ mineig_coord1 += eigvec_mineig[i]*coord1[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ other_results[2] = mineig_coord0 * mineig_coord1;
|
|
|
+ other_results[3] = 0;
|
|
|
+
|
|
|
+
|
|
|
+ if (mineig_coord0 * mineig_coord1 > 0) {
|
|
|
+ other_results[1] = -1; // meaningless result for med_eig_T
|
|
|
+ fevals[1] = 0;
|
|
|
+ if (min_eig < limit) {
|
|
|
+ *branch = 0;
|
|
|
+ return min_eig;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ *branch = 2;
|
|
|
+ return limit;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ else {
|
|
|
+
|
|
|
+ if (minimizer == 0 || minimizer == 1 || minimizer == 4 || minimizer == 5) {
|
|
|
+ med_eig = min_med_eig_brent(qf0, qf1, &other_results[1], &fevals[1]);
|
|
|
+ }
|
|
|
+ else if (minimizer == 2 || minimizer == 3) {
|
|
|
+ med_eig = min_med_eig_gss(qf0, qf1, &other_results[1], &fevals[1]);
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ return -1.;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (1.0E-4 < other_results[1] && other_results[1] < 1 - 1.0E-4 && med_eig < limit) {
|
|
|
+ *branch = 1;
|
|
|
+ return med_eig;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ *branch = 2;
|
|
|
+ other_results[1] = 1;
|
|
|
+ return limit;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void contact_function_multi(int n, double a0, double a1, double b0, double b1,
|
|
|
+ double* coords0, double* orients0, double* coords1, double* orients1, int minimizer,
|
|
|
+ int* all_evals, double* results) {
|
|
|
+
|
|
|
+ double other_results[4];
|
|
|
+ int branch=0;
|
|
|
+ int* branch_point = &branch;
|
|
|
+ int fevals[2];
|
|
|
+ fevals[0] = 0;
|
|
|
+ fevals[1] = 0;
|
|
|
+
|
|
|
+ for (int i=0; i<n; i++){
|
|
|
+ results[i] = contact_function(a0, a1, b0, b1, &coords0[3*i], &orients0[3*i], &coords1[3*i], &orients1[3*i],
|
|
|
+ minimizer, &other_results[0], fevals, &branch);
|
|
|
+ all_evals[0] += fevals[0];
|
|
|
+ all_evals[1] += fevals[1];
|
|
|
+ fevals[0] = 0;
|
|
|
+ fevals[1] = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double contact_mono_packing(double inv2a, double inv2b,
|
|
|
+ double* coord0, double* orient0,
|
|
|
+ double* coord1, double* orient1,
|
|
|
+ int try_second, int early_stop) {
|
|
|
+
|
|
|
+ /*
|
|
|
+ Simplified contact function calculation for monodispersed ellipse packing.
|
|
|
+ */
|
|
|
+
|
|
|
+ double cross0[3];
|
|
|
+ double cross1[3];
|
|
|
+ double lin_interpolation[9];
|
|
|
+ double qf0[9];
|
|
|
+ double qf1[9];
|
|
|
+ double min_eig;
|
|
|
+ double min_eig_T = 0.;
|
|
|
+ double* meT_point = &min_eig_T;
|
|
|
+ int fevals = 0;
|
|
|
+ int* fevals_point = &fevals;
|
|
|
+
|
|
|
+ cross_product(coord0, orient0, cross0);
|
|
|
+ cross_product(coord1, orient1, cross1);
|
|
|
+
|
|
|
+ /* Trasformation of quadratic forms */
|
|
|
+ for (int i=0; i<3; i++){
|
|
|
+ for (int l=0; l<3; l++){
|
|
|
+ qf0[3*i+l] = orient0[i]*inv2a*orient0[l] + cross0[i]*inv2b*cross0[l];
|
|
|
+ qf1[3*i+l] = orient1[i]*inv2a*orient1[l] + cross1[i]*inv2b*cross1[l];
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ min_eig = max_min_eig_brent(qf0, qf1, early_stop, meT_point, fevals_point);
|
|
|
+
|
|
|
+ if (try_second == 0 ){
|
|
|
+ return min_eig;
|
|
|
+ }
|
|
|
+
|
|
|
+ else { // The case where the 2nd eigenvalue must also be calculated
|
|
|
+
|
|
|
+ double lin_interpolation[9];
|
|
|
+ double eigvec_mineig[3];
|
|
|
+ double mineig_coord0 = 0;
|
|
|
+ double mineig_coord1 = 0;
|
|
|
+
|
|
|
+ /* eigenvector corresponding to maximal 1st eigenvalue (1st intersection coordinate)*/
|
|
|
+ for (int i=0; i<9; i++) {
|
|
|
+ lin_interpolation[i] = min_eig_T * qf0[i] + (1 - min_eig_T) * qf1[i];
|
|
|
+ }
|
|
|
+ eigenvector(lin_interpolation, 1, eigvec_mineig);
|
|
|
+
|
|
|
+ /* scalar product between eigenvectors and position vectors for both ellipses in the pair */
|
|
|
+ for (int i=0; i<3; i++) {
|
|
|
+ mineig_coord0 += eigvec_mineig[i]*coord0[i];
|
|
|
+ mineig_coord1 += eigvec_mineig[i]*coord1[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ if (mineig_coord0 * mineig_coord1 > 0) {
|
|
|
+ return min_eig;
|
|
|
+ }
|
|
|
+
|
|
|
+ else {
|
|
|
+ min_eig_T = 0.;
|
|
|
+ fevals = 0;
|
|
|
+ return min_med_eig_brent(qf0, qf1, meT_point, fevals_point);
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double contact_mono_pack_multi(int n, double inv2a, double inv2b, double* coords, double* orients, double dist_lim) {
|
|
|
+
|
|
|
+ double energy=0.;
|
|
|
+ double cos_dist;
|
|
|
+ double contact_f;
|
|
|
+ int try_second=0;
|
|
|
+
|
|
|
+ if (inv2a < 2) {
|
|
|
+ try_second = 1; // for large ellipses, 2nd eigenvalue might be the correct one
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int i=0; i<n; i++) {
|
|
|
+ for (int j=i+1; j<n; j++) {
|
|
|
+ cos_dist = coords[3*i]*coords[3*j] + coords[3*i+1]*coords[3*j+1] + coords[3*i+2]*coords[3*j+2];
|
|
|
+ if (cos_dist > dist_lim) {
|
|
|
+ contact_f = contact_mono_packing(inv2a, inv2b, &coords[3*i], &orients[3*i], &coords[3*j], &orients[3*j],
|
|
|
+ try_second, 1);
|
|
|
+ if (contact_f < 1) {
|
|
|
+ energy += contact_f + 1 / contact_f - 2;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return energy;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double contact_bidisp_packing(double inv2a_0, double inv2b_0,
|
|
|
+ double inv2a_1, double inv2b_1,
|
|
|
+ double* coord0, double* orient0,
|
|
|
+ double* coord1, double* orient1,
|
|
|
+ int try_second, double omega) {
|
|
|
+
|
|
|
+ /*
|
|
|
+ Simplified contact function calculation for bidispersed ellipse packing.
|
|
|
+ */
|
|
|
+
|
|
|
+ double cross0[3];
|
|
|
+ double cross1[3];
|
|
|
+ double lin_interpolation[9];
|
|
|
+ double qf0[9];
|
|
|
+ double qf1[9];
|
|
|
+ double min_eig;
|
|
|
+ double med_eig;
|
|
|
+ double min_eig_T = 0.;
|
|
|
+ double* meT_point = &min_eig_T;
|
|
|
+ int fevals = 0;
|
|
|
+ int* fevals_point = &fevals;
|
|
|
+
|
|
|
+ cross_product(coord0, orient0, cross0);
|
|
|
+ cross_product(coord1, orient1, cross1);
|
|
|
+
|
|
|
+ /* Trasformation of quadratic forms */
|
|
|
+ for (int i=0; i<3; i++){
|
|
|
+ for (int l=0; l<3; l++){
|
|
|
+ qf0[3*i+l] = orient0[i]*inv2a_0*orient0[l] + cross0[i]*inv2b_0*cross0[l];
|
|
|
+ qf1[3*i+l] = orient1[i]*inv2a_1*orient1[l] + cross1[i]*inv2b_1*cross1[l];
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ min_eig = max_min_eig_brent(qf0, qf1, 1, meT_point, fevals_point);
|
|
|
+
|
|
|
+ if (try_second == 0 ){
|
|
|
+ if (min_eig < omega) {
|
|
|
+ return min_eig;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ return omega;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ else { // The case where the 2nd eigenvalue must also be calculated
|
|
|
+
|
|
|
+ double lin_interpolation[9];
|
|
|
+ double eigvec_mineig[3];
|
|
|
+ double mineig_coord0 = 0;
|
|
|
+ double mineig_coord1 = 0;
|
|
|
+
|
|
|
+ /* eigenvector corresponding to maximal 1st eigenvalue (1st intersection coordinate)*/
|
|
|
+ for (int i=0; i<9; i++) {
|
|
|
+ lin_interpolation[i] = min_eig_T * qf0[i] + (1 - min_eig_T) * qf1[i];
|
|
|
+ }
|
|
|
+ eigenvector(lin_interpolation, 1, eigvec_mineig);
|
|
|
+
|
|
|
+ /* scalar product between eigenvectors and position vectors for both ellipses in the pair */
|
|
|
+ for (int i=0; i<3; i++) {
|
|
|
+ mineig_coord0 += eigvec_mineig[i]*coord0[i];
|
|
|
+ mineig_coord1 += eigvec_mineig[i]*coord1[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ if (mineig_coord0 * mineig_coord1 > 0) {
|
|
|
+ if (min_eig < omega) {
|
|
|
+ return min_eig;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ return omega;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ else {
|
|
|
+ min_eig_T = 0.;
|
|
|
+ fevals = 0;
|
|
|
+ med_eig = min_med_eig_brent(qf0, qf1, meT_point, fevals_point);
|
|
|
+
|
|
|
+ if (med_eig < omega) {
|
|
|
+ return med_eig;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ return omega;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double contact_bidisp_pack_multi(int n, double* inv2a, double* inv2b,
|
|
|
+ double* coords, double* orients, double dist_lim, double omega) {
|
|
|
+
|
|
|
+ double energy=0.;
|
|
|
+ double cos_dist;
|
|
|
+ double contact_f;
|
|
|
+ int try_second;
|
|
|
+
|
|
|
+ for (int i=0; i<n; i++) {
|
|
|
+ for (int j=i+1; j<n; j++) {
|
|
|
+
|
|
|
+ cos_dist = coords[3*i]*coords[3*j] + coords[3*i+1]*coords[3*j+1] + coords[3*i+2]*coords[3*j+2];
|
|
|
+
|
|
|
+ if (cos_dist > dist_lim) {
|
|
|
+
|
|
|
+ if (inv2a[i] * inv2a[j] < inv2a[i] + inv2a[j]) { // derived from the condition alpha0 + alpha1 > pi / 2
|
|
|
+ try_second = 1; // for large ellipses, 2nd eigenvalue might be the correct one
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ try_second = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ contact_f = contact_bidisp_packing(inv2a[i], inv2b[i], inv2a[j], inv2b[j],
|
|
|
+ &coords[3*i], &orients[3*i], &coords[3*j], &orients[3*j], try_second, omega);
|
|
|
+
|
|
|
+ if (contact_f < 1) {
|
|
|
+ energy += contact_f + 1 / contact_f - 2;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return energy;
|
|
|
+}
|