/* * Ray2mesh : software for geophysicists. * Compute various scores attached to the mesh cells, based on geometric   information that rays bring when they traverse cells. * * Copyright (C) 2003, St閜hane Genaud and Marc Grunberg * * This tool is free software; you can redistribute it and/or * modify it under the terms of the GNU Library General Public * License as published by the Free Software Foundation; either * version 2 of the License, or (at your option) any later version. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU * Library General Public License for more details. * * You should have received a copy of the GNU Library General Public * License along with this library; if not, write to the Free * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. *//* Usefull maths used for ray2mesh */#include <mesh/cellinfo.h>#include <mesh/convert_coord.h>#include <ray/raydescartes.h>#include "ray2mesh.h"		/* for blocnum_t */#include "const_ray2mesh.h"/**\brief  given points c1 and c2 expressed as cartesian coordinates,  * return the distance between c1 and c2                  **/double cart_distance(struct coord_cart_t *c1, struct coord_cart_t *c2){    double dx = c2->x - c1->x, dy = c2->y - c1->y, dz = c2->z - c1->z;    return (sqrt(dx * dx + dy * dy + dz * dz));}/**\brief given points g1 and g2 expressed as geographic coordinates  * (angles are in radians), return the distance between * g1 and g2 (units identical as the one used for depth) **/double geo_distance(struct coord_geo_t *g1, struct coord_geo_t *g2){    double d;			/* distance */    struct coord_cart_t *c1, *c2;    c1 = geo2cart(g1);    c2 = geo2cart(g2);    d = sqrt((c2->x - c1->x) * (c2->x - c1->x) +	     (c2->y - c1->y) * (c2->y - c1->y) +	     (c2->z - c1->z) * (c2->z - c1->z));    free(c1);    free(c2);    return (d);}/**\brief given points g1 and g2 expressed as geographic      * coordinates (angles are in radians), return the middle point of * the segment [g1,g2] (units identical as the one used for depth)**/struct coord_geo_t *geo_middle(struct coord_geo_t *g1,			       struct coord_geo_t *g2){    struct coord_geo_t *m;    m = (struct coord_geo_t *) malloc(sizeof(struct coord_geo_t));    assert(m);    m->lat = (g2->lat + g1->lat) / 2.;    m->lon = (g2->lon + g1->lon) / 2.;    m->prof = (g2->prof + g1->prof) / 2.;    return (m);}/* to check **** to be removed */struct coord_geo_t *geo_middleb(struct coord_geo_t *g1,				struct coord_geo_t *g2){    struct coord_geo_t *m;    struct coord_cart_t *c1, *c2, c;    c1 = geo2cart(g1);    c2 = geo2cart(g2);    c.x = (c1->x + c2->x) / 2.;    c.y = (c1->y + c2->y) / 2.;    c.z = (c1->z + c2->z) / 2.;    free(c1);    free(c2);    m = cart2geo(&c);    return (m);}/** \brief given cartesian coordinates x,y,z, return a unique id  **/blocknum_t cart2lin3D(blocknum_t x,		      blocknum_t y,		      blocknum_t z, blocknum_t max_x, blocknum_t max_y){    return ((blocknum_t) ((max_y * max_x * z) + (y * max_x) + x));}/**\brief given a unique id dest, updates the coordinates x,y,z  **/void lin2cart3D(blocknum_t dest,		blocknum_t * x, blocknum_t * y, blocknum_t * z,		blocknum_t max_x, blocknum_t max_y){    *x = dest % max_x;    *z = dest / (max_x * max_y);    *y = (dest % (max_x * max_y)) / max_x;}/** * \brief return true if v=u , where v,u are in Z^3 **/int equal_z3(struct coord_z3_t p1, struct coord_z3_t p2){    if (p1.x == p2.x && p1.y == p2.y && p1.z == p2.z)	return (1);    else	return (0);}/** * \brief  u.v , where u and v are vectors in R^3 **/double scal_prod(struct coord_cart_t *v0, struct coord_cart_t *v1){    return (v0->x * v1->x + v0->y * v1->y + v0->z * v1->z);}/** * \brief  u - v , where u and v are vectors in R^3 **/struct coord_cart_t *vector_minus_vector(struct coord_cart_t *v0,					 struct coord_cart_t *v1){    struct coord_cart_t *r;    r = (struct coord_cart_t *) malloc(sizeof(struct coord_cart_t));    assert(r);    r->x = v0->x - v1->x;    r->y = v0->y - v1->y;    r->z = v0->z - v1->z;    return (r);}/** * \brief  u + v , where u and v are vectors in R^3 **/struct coord_cart_t *vector_plus_vector(struct coord_cart_t *v0,					struct coord_cart_t *v1){    struct coord_cart_t *r;    r = (struct coord_cart_t *) malloc(sizeof(struct coord_cart_t));    assert(r);    r->x = v0->x + v1->x;    r->y = v0->y + v1->y;    r->z = v0->z + v1->z;    return (r);}/** * \brief scalar times vector k.v , where k is a scalar, v a vector in R^3 **/struct coord_cart_t *times_scalar(float k, struct coord_cart_t *v){    struct coord_cart_t *r;    r = (struct coord_cart_t *) malloc(sizeof(struct coord_cart_t));    assert(r);    r->x = k * v->x;    r->y = k * v->y;    r->z = k * v->z;    return (r);}/** * \brief compute vectorial produt u^v * * compute vectorial produt u^v * u^v , where u and v are vector in R^3 **/struct coord_cart_t *vect_prod(struct coord_cart_t *c1,			       struct coord_cart_t *c2){    struct coord_cart_t *c3;    c3 = (struct coord_cart_t *) malloc(sizeof(struct coord_cart_t));    assert(c3);    c3->x = c1->y * c2->z - c2->y * c1->z;    c3->y = c2->x * c1->z - c1->x * c2->z;    c3->z = c1->x * c2->y - c2->x * c1->y;    return (c3);}/** * \brief segment face intersection * *    Determine whether or not the segment s1,s2 *    Intersects a plane P defined by point p0 and and normal vector n *    Output: i = the intersect point (when it exists). Beware that *            i's value  has no meaning if function returns > 0. *    Return: 0 = intersection in the unique point i *            1 = disjoint (no intersection) *            2 = segment colinear to plane *    source : http://geometryalgorithms.com/Archive/algorithm_0104/algorithm_0104B.htm **/int face_segment_intersect(struct coord_cart_t *s0,			   struct coord_cart_t *s1,			   struct coord_cart_t *p0,			   struct coord_cart_t *n, struct coord_cart_t **i){    struct coord_cart_t *u, *w, *z;	/* vectors */    float denom, numer, sI;    u = vector_minus_vector(s1, s0);    w = vector_minus_vector(s0, p0);    normalize_coord_cart(&u);    normalize_coord_cart(&w);    normalize_coord_cart(&n);#ifdef DEBUG_INTERSECT    dump_coord_cart("u", u);    dump_coord_cart("w", w);    dump_coord_cart("n", n);#endif    denom = scal_prod(n, u);    numer = -scal_prod(n, w);#ifdef DEBUG_INTERSECT    fprintf(stderr, "%s:face_segment_intersect():%d  numer=-scal_prod(n,w)=%.3f abs(denom)=%.3f\n", 			  __FILE__,__LINE__,numer, fabs(denom));#endif    if (fabs(denom) < EPS_10_6) {	/* segment is parallel to plane */    	free(u);        free(w);	if (fabs(numer) < EPS_10_6)		/* segment lies in plane */	    return (2);	else {	    return (3);		/* no intersection */	}    }    /* they are not parallel : compute intersect param */    sI = numer / denom;    if (sI < 0 || sI > 1) {    	free(u);        free(w);	return (1);		/* no intersection */    }    z = times_scalar(sI, u);    *i = vector_plus_vector(s0, z);	/* compute segment intersect point */    dump_coord_geo("[face_segment_intersect]", cart2geo(*i));    free(u);    free(w);    free(z);    return (0);}/** * \brief line facet intersection * *    Determine whether or not the line segment p1,p2 *    Intersects the plane pa,pb,pc *    Return true/false and the intersection point p * *    The equation of the line is p = p1 + mu (p2 - p1) *    The equation of the plane is a x + b y + c z + d = 0 *                                 n.x x + n.y y + n.z z + d = 0 * *    http://astronomy.swin.edu.au/~pbourke/geometry/linefacet/ **/int face_segment_intersect2(struct coord_cart_t *p1,			    struct coord_cart_t *p2,			    struct coord_cart_t *pa,			    struct coord_cart_t *pb,			    struct coord_cart_t *pc,			    struct coord_cart_t **p){double d;double denom, mu;struct coord_cart_t *n;    n = (struct coord_cart_t *) malloc(sizeof(struct coord_cart_t));    assert(n);    /* Calculate the parameters for the plane */    n->x = (pb->y - pa->y) * (pc->z - pa->z) - (pb->z - pa->z) * (pc->y - pa->y);    n->y = (pb->z - pa->z) * (pc->x - pa->x) - (pb->x - pa->x) * (pc->z - pa->z);    n->z = (pb->x - pa->x) * (pc->y - pa->y) - (pb->y - pa->y) * (pc->x - pa->x);    normalize_coord_cart(&n);    d = -n->x * pa->x - n->y * pa->y - n->z * pa->z;    /* Calculate the position on the line that intersects the plane */    denom = n->x * (p2->x - p1->x) + n->y * (p2->y - p1->y) + n->z * (p2->z - p1->z);    if (fabs(denom) < EPS_10_6) {		/* Line and plane don't intersect */	free(n);	return (1);    }	    mu = -(d + n->x * p1->x + n->y * p1->y + n->z * p1->z) / denom;    (*p)->x = p1->x + mu * (p2->x - p1->x);    (*p)->y = p1->y + mu * (p2->y - p1->y);    (*p)->z = p1->z + mu * (p2->z - p1->z);    free(n);#ifdef DEBUG_EXACT_LENGTH    dump_coord_geo("face_segment_intersect2()", cart2geo(*p));    fprintf(stderr,"%s:face_segment_intersect2():%d  mu=%f\n",			  __FILE__,__LINE__,mu);#endif    if ((mu < (- EPS_10_6)) || (mu > (1.+ EPS_10_6))) {	 /*   mu < 0 || mu > 1   */        /* Intersection not along line segment */	return (2);    }    return (0);			/* instersects in p */}/** * \brief face segment intersect3 * *    Intersection sphere - segment *    Return true/false and the intersection point p * *    The equation of the line is p = p2 + mu (p1 - p2) *    The equation of the sphere is  x^2 + y^2 + z^2 = r^2 * *     **/int face_segment_intersect3(struct coord_cart_t *p1,			    struct coord_cart_t *p2,			    double r, struct coord_cart_t **p){    double a, b, c, delta;    double mu1, mu2, mu;    struct coord_cart_t *v;    v = (struct coord_cart_t *) malloc(sizeof(struct coord_cart_t));    assert(v);    /*  calculate the vector p2p1  */    v->x = p1->x - p2->x;    v->y = p1->y - p2->y;    v->z = p1->z - p2->z;    mu = 2.;    /*  equation of  2nd degrees :  a*mu^2 +2*b*mu +c =0  */    /*  calculate  a, b, c, delta  */    /*  delta = b^2 -a*c  */    a = v->x * v->x + v->y * v->y + v->z * v->z;    b = v->x * p2->x + v->y * p2->y + v->z * p2->z;    c = p2->x * p2->x + p2->y * p2->y + p2->z * p2->z - r * r;    delta = b * b - a * c;    if (delta < EPS_10_6) {    /*  delta <= 0.  */	free(v);      	return (1);    }		/* if  delta == 0., the line (prev_p,p) is tangent to the face	*/	    if (fabs(a) < EPS_10_6) {           /*  a == 0.  */	free(v);        return (2);    }    /* the solution of the equation are :  */    mu1 = (-b + sqrt(delta)) / a;    mu2 = (-b - sqrt(delta)) / a;        /* we want  0 < mu < 1, P is in [p1,p2] */    /* the face and the segment have only one intersection point */    if ((mu1 > (- EPS_10_6)) && (mu1 < (1.+ EPS_10_6))) {        /*    0 <= mu1 <= 1  */	mu = mu1;	    }	    if ((mu2 > (- EPS_10_6)) && (mu2 < (1.+ EPS_10_6))) {	/*   0 <= mu2 <= 1  */	mu = mu2;		    }    if ((mu > (2.- EPS_10_6)) && (mu < (2.+ EPS_10_6))) {    	/* mu == 2 */	free(v);	return (3);    }    (*p)->x = p2->x + mu * v->x;    (*p)->y = p2->y + mu * v->y;    (*p)->z = p2->z + mu * v->z;    free(v);    return (0);			/* intersection */}/** * \brief face segment intersect4 * *    Intersection cone (Oz) - segment *    Return true/false and the intersection point p * *    The equation of the line is p = p2 + mu (p1 - p2) *    The equation of the cone is  x^2 + y^2 = (tan alpha)^2 * z^2 *    A, a point of the cone   (tan alpha)^2 = (Ax^2 + Ay^2) / Az^2 *    Beta = (tan alpha)^2 * *     **/int face_segment_intersect4(struct coord_cart_t *p1,			    struct coord_cart_t *p2,			    double Beta, struct coord_cart_t **p){    double a, b, c, delta;    double mu1, mu2, mu;    struct coord_cart_t *v;    v = (struct coord_cart_t *) malloc(sizeof(struct coord_cart_t));    assert(v);    /*  calculate the vector p2p1  */    v->x = p1->x - p2->x;    v->y = p1->y - p2->y;    v->z = p1->z - p2->z;    mu = 2.;    /*  equation of 2nd degrees : a*mu^2 +2*b*mu +c =0  */    /*  calcul of  a, b, c, delta  */    /*  delta = b^2 -a*c  */    a = v->x * v->x + v->y * v->y - Beta * v->z * v->z;    b = v->x * p2->x + v->y * p2->y - Beta * v->z * p2->z;    c = p2->x * p2->x + p2->y * p2->y - Beta * p2->z * p2->z;    delta = b * b - a * c;    if (delta < EPS_10_6) {                          /*   delta <= 0.  */	free(v);	return (1);    }    if (fabs(a) < EPS_10_6) {                   /*   a == 0.    */	free(v);        return (2);    }    /*  solutions of the equation are : */    mu1 = (-b + sqrt(delta)) / a;    mu2 = (-b - sqrt(delta)) / a;    /* we want  0 < mu < 1, P is in [p1,p2] */    /* the face and the segment have only one intersection point  */    if ((mu1 > (- EPS_10_6)) && (mu1 < (1.+ EPS_10_6))) {	/*   0 <= mu1 <= 1  */	mu = mu1;    }        if ((mu2 > (- EPS_10_6)) && (mu2 < (1.+ EPS_10_6))) {	/*   0 <= mu2 <= 1  */	mu = mu2;    }    if ((mu > (2.- EPS_10_6)) && (mu < (2.+ EPS_10_6))) {	/* mu == 2 */	free(v);        return (3);    }        (*p)->x = p2->x + mu * v->x;    (*p)->y = p2->y + mu * v->y;    (*p)->z = p2->z + mu * v->z;    free(v);    return (0);			/* intersection */}/** * \brief face segment intersect5 * *    Intersection cone (Oz) - segment *    Return true/false and the intersection point p * *    The equation of the line is p = p2 + mu (p1 - p2) *    Particular case  Az = 0. (et Bz, Cz, Dz) *    ABCD is a plane define by  z=0. * *     **/int face_segment_intersect5(struct coord_cart_t *p1,			    struct coord_cart_t *p2,			    struct coord_cart_t **p){    double mu;    struct coord_cart_t *v;    v = (struct coord_cart_t *) malloc(sizeof(struct coord_cart_t));    assert(v);    /*  the vector p2p1  */    v->x = p1->x - p2->x;    v->y = p1->y - p2->y;    v->z = p1->z - p2->z;    mu = -p2->z / v->z;		/* we want  p->x = p2->z + mu * v->z  et  p->x = 0. */    if ((mu < (- EPS_10_6)) || (mu > (1.+ EPS_10_6))) {             /* (mu < 0) || (mu > 1) */	free(v);        return (3);    }    (*p)->x = p2->x + mu * v->x;    (*p)->y = p2->y + mu * v->y;    (*p)->z = 0.;    free(v);    return (0);			/* intersection */}