/******************************************************************************
* SrfRay.c - fast surface ray intersetion for interaction.		      *
*******************************************************************************
* (C) Gershon Elber, Technion, Israel Institute of Technology                 *
*******************************************************************************
* Written by Gershon Elber, May. 95.					      *
******************************************************************************/

#include "irit_sm.h"
#include "geomat3d.h"
#include "cagd_lib.h"
#include "bbox.h"
#include "user_loc.h"

typedef struct IntrSrfRayStruct {
    struct IntrSrfRayStruct *Right, *Left;
    BBBboxStruct BBox;
    PlaneType Plane1, Plane2;
    CagdPolygonStruct *TwoPolys;
} IntrSrfRayStruct;

static CagdRType
    GlblMinRayParam = IRIT_INFNTY;
static CagdUVType
    GlblUV;

static VoidPtr IntrSrfRayPreprocessSrfAux(CagdSrfStruct *Srf, int FineNess);
static void IntrSrfRayTestRayAux(IntrSrfRayStruct *Handle,
				 CagdPType RayOrigin,
				 CagdVType RayDir,
				 CagdUVType InterUV);
static CagdBType RayIntersectBBox(CagdPType RayOrigin,
				  CagdVType RayDir,
				  BBBboxStruct *BBox);
static int RayIntersectTriangle(CagdPType RayOrigin,
				CagdVType RayDir,
				CagdPolygonStruct *Tri,
				PlaneType Plane);

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Preprocess a surface for fast computation of ray-surface intersection.   M
* Returns NULL if fails, otherwise a pointer to preprocessed data structure. M
*   The preprocessed data is in fact a hierarchy of bounding boxes extracted M
* while the surface is being polygonized.				     M
*                                                                            *
* PARAMETERS:                                                                M
*   Srf:        To preprocess.                                               M
*   FineNess:   Control on accuracy, the higher the finer.                   M
*		The surface will be subdivided into approximately FineNess   M
*		regions in each of the two parametric directions.	     M
*                                                                            *
* RETURN VALUE:                                                              M
*   VoidPtr:  A handle on the preprocessed data, NULL otherwise.             M
*                                                                            *
* KEYWORDS:                                                                  M
*   IntrSrfRayPreprocessSrf, ray surface intersection                        M
*****************************************************************************/
VoidPtr IntrSrfRayPreprocessSrf(CagdSrfStruct *Srf, int FineNess)
{
    int i;
    CagdBType
	IsNewSrf = FALSE;
    VoidPtr V;

    if (CAGD_IS_BEZIER_SRF(Srf)) {
	Srf = CnvrtBezier2BsplineSrf(Srf);
	IsNewSrf = TRUE;;
    }

    if (!CAGD_IS_BSPLINE_SRF(Srf)) {
	USER_FATAL_ERROR(USER_ERR_WRONG_SRF);
	return NULL;
    }

    for (i = 1; FineNess > 0; i++, FineNess /= 2);
    V = IntrSrfRayPreprocessSrfAux(Srf, i);

    if (IsNewSrf) {
	CagdSrfFree(Srf);
    }

    return V;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Auxiliary function of IntrSrfRayPreprocessSrf                            *
*****************************************************************************/
static VoidPtr IntrSrfRayPreprocessSrfAux(CagdSrfStruct *Srf, int FineNess)
{
    CagdSrfStruct *Srf1, *Srf2;
    CagdRType UMin, UMax, VMin, VMax, t, *KV;
    IntrSrfRayStruct
	*SrfRay = (IntrSrfRayStruct *) IritMalloc(sizeof(IntrSrfRayStruct));
    BBBboxStruct *TmpBBox;

    CagdSrfDomain(Srf, &UMin, &UMax, &VMin, &VMax);

    if (FineNess > 0) {
	IntrSrfRayStruct *TmpSrfRay;

	if (FineNess & 0x01) {
	    KV = Srf -> UKnotVector;
	    if (Srf -> ULength == Srf -> UOrder)
	        t = (UMin + UMax) / 2.0;
	    else
	        t = KV[(Srf -> ULength + Srf -> UOrder) / 2];

	    Srf1 = BspSrfSubdivAtParam(Srf, t, CAGD_CONST_U_DIR);
	}
	else {
	    KV = Srf -> VKnotVector;
	    if (Srf -> VLength == Srf -> VOrder)
	        t = (VMin + VMax) / 2.0;
	    else
	        t = KV[(Srf -> VLength + Srf -> VOrder) / 2];

	    Srf1 = BspSrfSubdivAtParam(Srf, t, CAGD_CONST_V_DIR);
	}

	Srf2 = Srf1 -> Pnext;

	SrfRay -> Left = (IntrSrfRayStruct *)
	    IntrSrfRayPreprocessSrfAux(Srf1, --FineNess);
	SrfRay -> Right = (IntrSrfRayStruct *)
	    IntrSrfRayPreprocessSrfAux(Srf2, FineNess);
	SrfRay -> TwoPolys = NULL;

	CagdSrfFree(Srf1);
	CagdSrfFree(Srf2);

	if (SrfRay -> Left == NULL) {
	    if (SrfRay -> Right == NULL) {
		IritFree((VoidPtr) SrfRay);
		return NULL;
	    }
	    else {
		TmpSrfRay = SrfRay -> Right;
		GEN_COPY(SrfRay, TmpSrfRay, sizeof(IntrSrfRayStruct));
		IritFree((VoidPtr) TmpSrfRay);
	    }
	}
	else if (SrfRay -> Right == NULL) {
	    TmpSrfRay = SrfRay -> Left;
	    GEN_COPY(SrfRay, TmpSrfRay, sizeof(IntrSrfRayStruct));
	    IritFree((VoidPtr) TmpSrfRay);
	}
	else {
	    TmpBBox = BBMergeBbox(&SrfRay -> Left -> BBox,
				  &SrfRay -> Right -> BBox);
	    GEN_COPY(&SrfRay -> BBox, TmpBBox, sizeof(BBBboxStruct));
	}

	return SrfRay;
    }
    else {
	int i, j;
	CagdRType *Pt;
	CagdVType V;
	CagdPolygonStruct *Poly1, *Poly2;

	/* Approximate this surface region using two polygons. */
	SrfRay -> Left = SrfRay -> Right = NULL;
	SrfRay -> TwoPolys = Poly1 = CagdPolygonNew();
	Poly1 -> Pnext = Poly2 = CagdPolygonNew();

	Pt = CagdSrfEval(Srf, UMin, VMin);
	CagdCoerceToE3(Poly1 -> Polygon[0].Pt, &Pt, -1, Srf -> PType);
	Poly1 -> Polygon[0].UV[0] = UMin;
	Poly1 -> Polygon[0].UV[1] = VMin;

	Pt = CagdSrfEval(Srf, UMax, VMin);
	CagdCoerceToE3(Poly1 -> Polygon[1].Pt, &Pt, -1, Srf -> PType);
	Poly1 -> Polygon[1].UV[0] = UMax;
	Poly1 -> Polygon[1].UV[1] = VMin;

	Pt = CagdSrfEval(Srf, UMax, VMax);
	CagdCoerceToE3(Poly1 -> Polygon[2].Pt, &Pt, -1, Srf -> PType);
	Poly1 -> Polygon[2].UV[0] = UMax;
	Poly1 -> Polygon[2].UV[1] = VMax;


	PT_COPY(Poly2 -> Polygon[0].Pt, Poly1 -> Polygon[0].Pt);
	Poly2 -> Polygon[0].UV[0] = UMin;
	Poly2 -> Polygon[0].UV[1] = VMin;

	PT_COPY(Poly2 -> Polygon[1].Pt, Poly1 -> Polygon[2].Pt);
	Poly2 -> Polygon[1].UV[0] = UMax;
	Poly2 -> Polygon[1].UV[1] = VMax;

	Pt = CagdSrfEval(Srf, UMin, VMax);
	CagdCoerceToE3(Poly2 -> Polygon[2].Pt, &Pt, -1, Srf -> PType);
	Poly2 -> Polygon[2].UV[0] = UMin;
	Poly2 -> Polygon[2].UV[1] = VMax;

	if (!CGPlaneFrom3Points(SrfRay -> Plane1,
				Poly1 -> Polygon[0].Pt,
				Poly1 -> Polygon[1].Pt,
				Poly1 -> Polygon[2].Pt) ||
	    !CGPlaneFrom3Points(SrfRay -> Plane2,
				Poly2 -> Polygon[0].Pt,
				Poly2 -> Polygon[1].Pt,
				Poly2 -> Polygon[2].Pt)) {
	    IritFree((VoidPtr) SrfRay);
	    CagdPolygonFree(Poly1);
	    CagdPolygonFree(Poly2);
	    return NULL;
	}

	/* Update normals of triangle to hold the vectors from Pi to P(i+1). */
	for (i = 0; i < 3; i++) {
	    PT_SUB(Poly1 -> Polygon[i].Nrml,
		   Poly1 -> Polygon[i].Pt,
		   Poly1 -> Polygon[(i + 1) % 3].Pt);
	    PT_SUB(Poly2 -> Polygon[i].Nrml,
		   Poly2 -> Polygon[i].Pt,
		   Poly2 -> Polygon[(i + 1) % 3].Pt);
	}

	/* Make sure cross product will be consistent. */
	CROSS_PROD(V, Poly1 -> Polygon[0].Nrml, Poly1 -> Polygon[1].Nrml);
	if (DOT_PROD(V, SrfRay -> Plane1) < 0) {
	    for (i = 0; i < 3; i++)
		PT_SCALE(Poly1 -> Polygon[i].Nrml, -1);
	}
	CROSS_PROD(V, Poly2 -> Polygon[0].Nrml, Poly2 -> Polygon[1].Nrml);
	if (DOT_PROD(V, SrfRay -> Plane2) < 0) {
	    for (i = 0; i < 3; i++)
		PT_SCALE(Poly2 -> Polygon[i].Nrml, -1);
	}

	/* Update bounding boxes. */
	for (i = 0; i < 3; i++) {
	    SrfRay -> BBox.Min[i] = IRIT_INFNTY;
	    SrfRay -> BBox.Max[i] = -IRIT_INFNTY;
	}
	for (j = 0; j < 3; j++) {
	    for (i = 0; i < 3; i++) {
		if (SrfRay -> BBox.Min[i] > Poly1 -> Polygon[j].Pt[i])
		    SrfRay -> BBox.Min[i] = Poly1 -> Polygon[j].Pt[i];
		if (SrfRay -> BBox.Max[i] < Poly1 -> Polygon[j].Pt[i])
		    SrfRay -> BBox.Max[i] = Poly1 -> Polygon[j].Pt[i];
	    }
	}
	for (i = 0; i < 3; i++) {
	    if (SrfRay -> BBox.Min[i] > Poly2 -> Polygon[2].Pt[i])
	        SrfRay -> BBox.Min[i] = Poly2 -> Polygon[2].Pt[i];
	    if (SrfRay -> BBox.Max[i] < Poly2 -> Polygon[2].Pt[i])
	        SrfRay -> BBox.Max[i] = Poly2 -> Polygon[2].Pt[i];
	}

	return SrfRay;
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Releases the pre processed data structed created by the function         M
* IntrSrfRayPreprocessSrf.                                                   M
*                                                                            *
* PARAMETERS:                                                                M
*   Handle:    As returned by IntrSrfRayPreprocessSrf to release             M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   IntrSrfRayFreePreprocess, ray surface intersection                       M
*****************************************************************************/
void IntrSrfRayFreePreprocess(VoidPtr Handle)
{
    IntrSrfRayStruct
	*RaySrf = (IntrSrfRayStruct *) Handle;

    if (RaySrf -> Left != NULL)
	IntrSrfRayFreePreprocess(RaySrf -> Left);
    if (RaySrf -> Right != NULL)
	IntrSrfRayFreePreprocess(RaySrf -> Right);
    if (RaySrf -> TwoPolys)
	CagdPolygonFreeList(RaySrf -> TwoPolys);
    IritFree((VoidPtr) RaySrf);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Computes the first intersection of a given ray with the given surface,   M
* if any. If TRUE is returned, the InterUV is updated to the interesection.  M
*                                                                            *
* PARAMETERS:                                                                M
*   Handle:    As returned by IntrSrfRayPreprocessSrf to release             M
*   RayOrigin: Starting point of ray.                                        M
*   RayDir:    Direction of ray.                                             M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdBType: TRUE if found intersection, FALSE otherwise.                  M
*                                                                            *
* KEYWORDS:                                                                  M
*   IntrSrfRayTestRay, ray surface intersection                              M
*****************************************************************************/
CagdBType IntrSrfRayTestRay(VoidPtr Handle,
			    CagdPType RayOrigin,
			    CagdVType RayDir,
			    CagdUVType InterUV)
{
    IntrSrfRayStruct
	*RaySrf = (IntrSrfRayStruct *) Handle;

    GlblMinRayParam = IRIT_INFNTY;
    GlblUV[0] = GlblUV[1] = -IRIT_INFNTY;

    IntrSrfRayTestRayAux(RaySrf, RayOrigin, RayDir, InterUV);

    InterUV[0] = GlblUV[0];
    InterUV[1] = GlblUV[1];

    return GlblMinRayParam < IRIT_INFNTY;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Auxiliary function of IntrSrfRayTestRayAux.	                             *
*****************************************************************************/
static void IntrSrfRayTestRayAux(IntrSrfRayStruct *RaySrf,
				 CagdPType RayOrigin,
				 CagdVType RayDir,
				 CagdUVType InterUV)
{
    if (RayIntersectBBox(RayOrigin, RayDir, &RaySrf -> BBox)) {
	if (RaySrf -> TwoPolys != NULL) {
	    RayIntersectTriangle(RayOrigin, RayDir,
				 RaySrf -> TwoPolys, RaySrf -> Plane1);
	    RayIntersectTriangle(RayOrigin, RayDir,
				 RaySrf -> TwoPolys -> Pnext,
				 RaySrf -> Plane2);
	}
	else {
	    if (RaySrf -> Left != NULL)
		IntrSrfRayTestRayAux(RaySrf -> Left, RayOrigin,
				     RayDir, InterUV);
	    if (RaySrf -> Right != NULL)
		IntrSrfRayTestRayAux(RaySrf -> Right, RayOrigin,
				     RayDir, InterUV);
	}
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Tests if the given ray intersects the given bounding box.                *
*                                                                            *
* PARAMETERS:                                                                *
*   RayOrigin: Starting point of ray.                                        *
*   RayDir:    Direction of ray.                                             *
*   BBox:      To test against ray.                                          *
*                                                                            *
* RETURN VALUE:                                                              *
*   CagdBType:  TRUE if intersects, FALSE otherwise.                         *
*****************************************************************************/
static CagdBType RayIntersectBBox(CagdPType RayOrigin,
				  CagdVType RayDir,
				  BBBboxStruct *BBox)
{
    int i, j;

    for (i = 0; i < 3; i++) {
	CagdRType T1, T2;
	CagdPType P1, P2;

	if (RayDir[i] != 0) {
	    /* Computes the t values of the intersection locations of the  */
	    /* ray with the six faces of the bounding box.		   */
	    T1 = (BBox -> Min[i] - RayOrigin[i]) / RayDir[i];
	    T2 = (BBox -> Max[i] - RayOrigin[i]) / RayDir[i];

	    /* Compute intersection points. */ 
	    for (j = 0; j < 3; j++) {
		if (j == i) {
		    /* Be exact, so we can compare for equality. */
		    P1[j] = BBox -> Min[i];
		    P2[j] = BBox -> Max[i];
		}
		else {
		    P1[j] = RayOrigin[j] + T1 * RayDir[j];
		    P2[j] = RayOrigin[j] + T2 * RayDir[j];
		}
	    }

	    /* Is the intersction point of either P1 or P2 inside BBox? */
	    if ((P1[0] >= BBox -> Min[0] && P1[0] <= BBox -> Max[0] &&
		 P1[0] >= BBox -> Min[0] && P1[0] <= BBox -> Max[0] &&
		 P1[1] >= BBox -> Min[1] && P1[1] <= BBox -> Max[1] &&
		 P1[1] >= BBox -> Min[1] && P1[1] <= BBox -> Max[1] &&
		 P1[2] >= BBox -> Min[2] && P1[2] <= BBox -> Max[2] &&
		 P1[2] >= BBox -> Min[2] && P1[2] <= BBox -> Max[2]) ||
		(P2[0] >= BBox -> Min[0] && P2[0] <= BBox -> Max[0] &&
		 P2[0] >= BBox -> Min[0] && P2[0] <= BBox -> Max[0] &&
		 P2[1] >= BBox -> Min[1] && P2[1] <= BBox -> Max[1] &&
		 P2[1] >= BBox -> Min[1] && P2[1] <= BBox -> Max[1] &&
		 P2[2] >= BBox -> Min[2] && P2[2] <= BBox -> Max[2] &&
		 P2[2] >= BBox -> Min[2] && P2[2] <= BBox -> Max[2]))
	        return TRUE;
	}
    }

    return FALSE;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Tests if the given ray intersects the given triangle. Updates the global *
* GlblMinRayParam and GlblUV if closest intersection so far.                 *
*                                                                            *
* PARAMETERS:                                                                *
*   RayOrigin: Starting point of ray.                                        *
*   RayDir:    Direction of ray.                                             *
*   Tri:       Triangle to test against ray for intersection.                *
*   Plane:     of Triangle Tri.                                              *
*                                                                            *
* RETURN VALUE:                                                              *
*   int:       TRUE if succesful, FALSE otherwise.                           *
*****************************************************************************/
static int RayIntersectTriangle(CagdPType RayOrigin,
				CagdVType RayDir,
				CagdPolygonStruct *Tri,
				PlaneType Plane)
{
    int i;
    CagdRType t, w, w0, w1, w2;
    CagdPType InterPoint;

    if (!CGPointFromLinePlane(RayOrigin, RayDir, Plane, InterPoint, &t))
	return FALSE;

    /* Test if new intersection is not closer than previous one. */
    if (t > GlblMinRayParam)
	return TRUE;

    for (i = 0; i < 3; i++) {
	CagdVType V1, V2;

	PT_SUB(V1, InterPoint, Tri -> Polygon[i].Pt);
	CROSS_PROD(V2, V1, Tri -> Polygon[i].Nrml);
	if (DOT_PROD(V2, Plane) < 0.0)
	    return FALSE;
    }

    /* A new intersection. Update global state. */
    GlblMinRayParam = t;
    w0 = CGDistPointLine(InterPoint,
			 Tri -> Polygon[1].Pt, Tri -> Polygon[1].Nrml) *
				     PT_LENGTH(Tri -> Polygon[1].Nrml);
    w1 = CGDistPointLine(InterPoint,
			 Tri -> Polygon[2].Pt, Tri -> Polygon[2].Nrml) *
				     PT_LENGTH(Tri -> Polygon[2].Nrml);
    w2 = CGDistPointLine(InterPoint,
			 Tri -> Polygon[0].Pt, Tri -> Polygon[0].Nrml) *
				     PT_LENGTH(Tri -> Polygon[0].Nrml);
    w = w0 + w1 + w2;

    for (i = 0; i < 2; i++)
	GlblUV[i] = (Tri -> Polygon[0].UV[i] * w0 +
		     Tri -> Polygon[1].UV[i] * w1 +
		     Tri -> Polygon[2].UV[i] * w2) / w;

    return TRUE;
}