raymaths.c

用于2维的射线追踪

/*
 * 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 */
}