/*----------------------------------------------------------------------*/
/*  ln_dist_ln.c                                                        */
/*                                                                      */
/*  Compute the distance between two lines or the intersection point    */
/*  if the lines cross.                                                 */
/*                                                                      */
/*  See ln_dist_ln.mac for more details                                 */
/*                                                                      */
/*  Compile cygwin:  gcc -O3 -o ln_dist_ln ln_dist_ln.c                 */
/*  Compile UNIX:    gcc -O3 -o ln_dist_ln ln_dist_ln.c -lm             */
/*  Compile MS Visual C++:  cl ln_dist_ln.c                             */
/*----------------------------------------------------------------------*/

// TO DO: Move fprintf lines into conditionals instead of all at end
// to keep xi_, etc, from getting assigned 1E315 type numbers.

#include <stdio.h>
#include <math.h>

// Read this file for input points (Ansys output)
#define FINPUT "ln_dist_ln.out"

// Write the results to this file (Gets input back into Ansys)
#define FOUTPUT "ln_dist_ln.in"

#define TRUE 1
#define FALSE 0
#define EPS 1.0e-9
#define ABS fabs
#define MAXLINE 95                  // Depends on *vwrite settings

typedef struct {
   double x,y,z;
} XYZ;

/*
  Written by Paul Bourke      April 1998
  http://astronomy.swin.edu.au/pbourke/geometry/lineline3d/

   Calculate the line segment PaPb that is the shortest route between
   two lines P1P2 and P3P4. Calculate also the values of mua and mub where
      Pa = P1 + mua (P2 - P1)
      Pb = P3 + mub (P4 - P3)
   Return FALSE if no solution exists, otherwise return the coordinates
   of Pa and Pb.
*/
int ln_dist_ln (
   XYZ p1,XYZ p2,XYZ p3,XYZ p4,XYZ *pa,XYZ *pb,
   double *mua, double *mub)
{
   XYZ p13,p43,p21;
   double d1343,d4321,d1321,d4343,d2121;
   double numer,denom;

   p13.x = p1.x - p3.x;
   p13.y = p1.y - p3.y;
   p13.z = p1.z - p3.z;
   p43.x = p4.x - p3.x;
   p43.y = p4.y - p3.y;
   p43.z = p4.z - p3.z;
   if (ABS(p43.x)  < EPS && ABS(p43.y)  < EPS && ABS(p43.z)  < EPS)
      return(FALSE);
   p21.x = p2.x - p1.x;
   p21.y = p2.y - p1.y;
   p21.z = p2.z - p1.z;
   if (ABS(p21.x)  < EPS && ABS(p21.y)  < EPS && ABS(p21.z)  < EPS)
      return(FALSE);

   d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
   d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
   d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
   d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
   d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;

   denom = d2121 * d4343 - d4321 * d4321;
   if (ABS(denom) < EPS)
      return(FALSE);
   numer = d1343 * d4321 - d1321 * d4343;

   *mua = numer / denom;
   *mub = (d1343 + d4321 * (*mua)) / d4343;

   pa->x = p1.x + *mua * p21.x;
   pa->y = p1.y + *mua * p21.y;
   pa->z = p1.z + *mua * p21.z;
   pb->x = p3.x + *mub * p43.x;
   pb->y = p3.y + *mub * p43.y;
   pb->z = p3.z + *mub * p43.z;

   return(TRUE);
}

double sq(double x)
{
    return x*x;
}

int main(int argc, char *argv[])
{
    int i, okay;
    double err, dist=0.0, mua, mub;
    XYZ P[4], PA, PB;

    FILE *fin, *fout;
    char str[MAXLINE];


    if ((fin=fopen(FINPUT,"r")) == NULL) {
        printf("\n ln_dist_ln.c:  Error opening file: %s\n", FINPUT);
        return 1;
    }

    for (i=0; i < 4; i++) {

        if (NULL == (fgets(str, MAXLINE, fin))) {
            printf("\n ln_dist_ln.c:  Bad input file: %s\n", FINPUT);
            fclose(fin);
            return 1;
        }

        sscanf(str, " %le %le %le", &P[i].x, &P[i].y, &P[i].z);

    }

    fclose(fin);

    okay = ln_dist_ln(P[0],P[1],P[2],P[3],&PA,&PB,&mua,&mub);

    if (okay) {

        // Compute the distance between lines
        dist = sqrt(sq(PA.x - PB.x) + sq(PA.y - PB.y) + sq(PA.z - PB.z));

        if (dist <= EPS) {

            err=1.0;            // Lines intersect
            printf("Lines intersect at (%.4f, %.4f, %.4f) in global CS\n",
                    PA.x,PA.y,PA.z);
            printf("Results written to parameters XI_, YI_, ZI_ and ERR_\n\n");

        } else {

            err=2.0;            // Lines are skew
            printf("Minimum distance between skew lines %.4f\n",dist);
            printf("Results written to parameters DIST_ and ERR_\n\n");
        }

    } else {

        err=0.0;                // Lines are parallel
        printf("Lines are parallel\n");
        printf("Results written to parameter ERR_\n\n");
    }


    //
    // Write the results to a file for Ansys to pick up
    //
    if ((fout=fopen(FOUTPUT,"w")) == NULL) {
        printf("\n ln_dist_ln.c:  Error opening file: %s\n", FINPUT);
        return 1;
    }

    // Ansys will complain if a parameter is less than 1E-60
    // If the value is less than 1E-60, set it to exactly zero
    dist = (ABS(dist) < 1.0E-60) ? 0.0 : dist;
    PA.x = (ABS(PA.x) < 1.0E-60) ? 0.0 : PA.x;
    PA.y = (ABS(PA.y) < 1.0E-60) ? 0.0 : PA.y;
    PA.z = (ABS(PA.z) < 1.0E-60) ? 0.0 : PA.z;

    fprintf(fout, "/nopr\n");
    fprintf(fout, "*set,ERR_\t, %30.18E\n", err);
    fprintf(fout, "*set,DIST_\t, %30.18E\n", dist);
    fprintf(fout, "*set,XI_\t, %30.18E\n", PA.x);
    fprintf(fout, "*set,YI_\t, %30.18E\n", PA.y);
    fprintf(fout, "*set,ZI_\t, %30.18E\n", PA.z);
    fprintf(fout, "/go\n");

    close(fout);

    return 0;

}