/******************************************************************************
* MshPlanr.c - Test colinearity of control meshes/polygons.		      *
*******************************************************************************
* Written by Gershon Elber, Sep. 91.					      *
******************************************************************************/

#include "cagd_loc.h"

/*****************************************************************************
* Computes a distance between two control points at given indices Index?.    *
* Only E2 or E3 point types are supported.				     *
*****************************************************************************/
CagdRType CagdDistanceTwoCtlPts(CagdRType **Points, int Index1, int Index2,
							CagdPointType PType)
{
    switch (PType) {
	case CAGD_PT_E2_TYPE:
	    return sqrt(SQR(Points[1][Index1] - Points[1][Index2]) +
			SQR(Points[2][Index1] - Points[2][Index2]));
	case CAGD_PT_E3_TYPE:
	    return sqrt(SQR(Points[1][Index1] - Points[1][Index2]) +
			SQR(Points[2][Index1] - Points[2][Index2]) +
			SQR(Points[3][Index1] - Points[3][Index2]));
	default:
	    FATAL_ERROR(CAGD_ERR_UNSUPPORT_PT);
	    return 0.0;
    }
}

/*****************************************************************************
* Fits a plane into the four points from Points indices Index?. Points may   *
* be either E2 or E3 only.						     *
*   Returns 0.0 if failed to fit a plane, otherwise a measure on the size of *
* the mesh data (distance between points) isreturned.			     *
*****************************************************************************/
CagdRType CagdFitPlaneThruCtlPts(CagdPlaneStruct *Plane, CagdPointType PType,
			         CagdRType **Points,
			         int Index1, int Index2, int Index3, int Index4)
{
    int i, j, Indices[4];
    CagdRType SizeMeasure,
	MaxDist = 0.0;
    CagdVecStruct V1, V2, V3;

    Indices[0] = Index1;
    Indices[1] = Index2;
    Indices[2] = Index3;
    Indices[3] = Index4;

    /* Find out the pair of vertices most seperated: */
    for (i = 0; i < 4; i++)
	for (j = i + 1; j < 4; j++) {
	    CagdRType
		Dist = CagdDistanceTwoCtlPts(Points, Indices[i], Indices[j],
								     PType);
	    if (Dist > MaxDist) {
		MaxDist = Dist;
		Index1 = i;
		Index2 = j;
	    }
	}

    if (MaxDist < EPSILON) return 0.0;
    SizeMeasure = MaxDist;
    MaxDist = 0.0;

    /* Find a third most seperated than the selected two. */
    for (i = 0; i < 4; i++)
	if (i != Index1 && i != Index2) {
	    CagdRType
		Dist1 = CagdDistanceTwoCtlPts(Points, Indices[Index1],
					      Indices[j], PType),
		Dist2 = CagdDistanceTwoCtlPts(Points, Indices[Index2],
					      Indices[j], PType),
		Dist = MIN(Dist1, Dist2);

	    if (Dist > MaxDist) {
		MaxDist = Dist;
		Index3 = i;
	    }
	}
    if (MaxDist < EPSILON) return 0.0;

    /* Now we have the 3 most seperated vertices to fit a plane thru. */

    switch (PType) {
	case CAGD_PT_E2_TYPE:
	    /* This is trivial since the plane must be Z = 0. */
	    Plane -> Plane[0] = 0.0;
	    Plane -> Plane[1] = 0.0;
	    Plane -> Plane[2] = 1.0;
	    Plane -> Plane[3] = 0.0;
	    break;
	case CAGD_PT_E3_TYPE:
	    /* Compute normal as a cross product of two vectors thru pts. */
	    for (i = 0; i < 3; i++) {
		j = i + 1;
		V1.Vec[i] = Points[j][Index2] - Points[j][Index1];
		V2.Vec[i] = Points[j][Index3] - Points[j][Index2];
	    }
	    CROSS_PROD(V3.Vec, V1.Vec, V2.Vec);
	    CAGD_NORMALIZE_VECTOR(V3);
	    for (i = 0; i < 3; i++)
		Plane -> Plane[i] = V3.Vec[i];

	    Plane -> Plane[3] = (-(V3.Vec[0] * Points[1][Index1] +
				   V3.Vec[1] * Points[2][Index1] +
				   V3.Vec[2] * Points[3][Index1]));
	    break;
	default:
	    FATAL_ERROR(CAGD_ERR_UNSUPPORT_PT);
	    break;
    }

    return SizeMeasure;
}

/*****************************************************************************
* Computes and returns distance between point Index and given plane which is *
* assumed to be normalized, so that the A B C D plane normal has unit len..  *
* Also assumed the Points are non rational with MaxDim dimension.	     *
*****************************************************************************/
CagdRType CagdDistPtPlane(CagdPlaneStruct *Plane, CagdRType **Points,
						      int Index, int MaxDim)
{
    int i;
    CagdRType
	R = Plane -> Plane[3];

    for (i = 0; i < MaxDim; i++)
	R += Plane -> Plane[i] * Points[i + 1][Index];

    return fabs(R);
}

/*****************************************************************************
* Computes and returns distance between point Index and the line from first  *
* point in direction LineDir (usually the line direction to last the point). *
* LineDir is assumed to be unit length normalized vector.		     *
*****************************************************************************/
static CagdRType CagdDistPtLine(CagdVecStruct *LineDir, CagdRType **Points,
						int Index, int MaxDim)
{
    int i;
    CagdRType R;
    CagdVecStruct V1, V2;

    for (i = 0; i < MaxDim; i++)
	V1.Vec[i] = Points[i+1][Index] - Points[i+1][0];
    if (MaxDim < 3)
	V1.Vec[2] = 0.0;

    /* Let V1 be the vector from point zero to point index. Then the distance */
    /* from point Index the the line is: | (LineDir . V1) LineDir - V1 |.     */
    V2 = *LineDir;
    R = DOT_PROD(V1.Vec, V2.Vec);
    CAGD_MULT_VECTOR(V2, R);
    CAGD_SUB_VECTOR(V2, V1);

    return CAGD_LEN_VECTOR(V2);
}

/*****************************************************************************
* Test polygon colinearity by testing the distance of interior control       *
* points from the line connecting the two polygon end points.		     *
*   Returns a relative ratio of deviation from line relative to its length.  *
*   Zero means all points are colinear.					     *
*   If two end points are same ( no line can be fit ) INFINITY is returned.  *
*****************************************************************************/
CagdRType CagdEstimateCrvColinearity(CagdCrvStruct *Crv)
{
    int i,
	MaxDim = 3,
	Length = Crv -> Length,
	Length1 = Length - 1;
    CagdRType LineLen,
	MaxDist = 0.0,
	**Points = Crv -> Points;
    CagdCrvStruct
	*CoercedCrv = NULL;
    CagdPointType
	PType = Crv ->PType;
    CagdVecStruct LineDir;

    switch (PType) {	      /* Convert the rational cases to non rational. */
	case CAGD_PT_P2_TYPE:
	    CoercedCrv = CagdCoerceCrvTo(Crv, CAGD_PT_E2_TYPE);
	    Points = CoercedCrv -> Points;
	    PType = CoercedCrv -> PType;
	    break;
	case CAGD_PT_P3_TYPE:
	    CoercedCrv = CagdCoerceCrvTo(Crv, CAGD_PT_E3_TYPE);
	    Points = CoercedCrv -> Points;
	    PType = CoercedCrv -> PType;
	    break;
    }

    switch (PType) {
	case CAGD_PT_E2_TYPE:
	    LineDir.Vec[0] = Points[1][Length1] - Points[1][0];
	    LineDir.Vec[1] = Points[2][Length1] - Points[2][0];
	    LineDir.Vec[2] = 0.0;
	    MaxDim = 2;
	    break;
	case CAGD_PT_E3_TYPE:
	    LineDir.Vec[0] = Points[1][Length1] - Points[1][0];
	    LineDir.Vec[1] = Points[2][Length1] - Points[2][0];
	    LineDir.Vec[2] = Points[3][Length1] - Points[3][0];
	    break;
	default:
	    FATAL_ERROR(CAGD_ERR_UNSUPPORT_PT);
	    break;
    }

    LineLen = CAGD_LEN_VECTOR(LineDir);
    if (LineLen < EPSILON) {
	if (CoercedCrv != NULL)
	    CagdCrvFree(CoercedCrv);
	return INFINITY;
    }

    CAGD_DIV_VECTOR(LineDir, LineLen);

    for (i = 1; i < Length1; i++) {
	CagdRType
	    Dist = CagdDistPtLine(&LineDir, Points, i, MaxDim);

	if (Dist > MaxDist)
	    MaxDist = Dist;
    }

    if (CoercedCrv != NULL)
	CagdCrvFree(CoercedCrv);

    return MaxDist / LineLen;
}

/*****************************************************************************
* Test mesh colinearity by testing the distance of interior points from the  *
* plane thru 3 corner points.						     *
*   Returns a relative ratio of deviation from plane relative to its size.   *
*   Zero means all points are coplanar.					     *
*   If end points are same ( no plane can be fit ) INFINITY is returned.     *
*****************************************************************************/
CagdRType CagdEstimateSrfPlanarity(CagdSrfStruct *Srf)
{
    int i,
	ULength = Srf -> ULength,
	ULength1 = ULength - 1,
	VLength = Srf -> VLength,
	VLength1 = VLength - 1;
    CagdRType
	PlaneSize = 0.0,
	MaxDist = 0.0,
	**Points = Srf -> Points;
    CagdSrfStruct
	*CoercedSrf = NULL;
    CagdPointType
	PType = Srf ->PType;
    CagdPlaneStruct Plane;

    switch (PType) {	      /* Convert the rational cases to non rational. */
	case CAGD_PT_P2_TYPE:
	case CAGD_PT_E2_TYPE:
	    /* Trivial case - it is planar surface. */
	    return 0.0;
	case CAGD_PT_P3_TYPE:
	    CoercedSrf = CagdCoerceSrfTo(Srf, CAGD_PT_E3_TYPE);
	    Points = CoercedSrf -> Points;
	    PType = CoercedSrf -> PType;
	    break;
    }

    switch (PType) {
	case CAGD_PT_E3_TYPE:
	    PlaneSize = CagdFitPlaneThruCtlPts(&Plane, PType, Points,
					CAGD_MESH_UV(Srf, 0,        0),
					CAGD_MESH_UV(Srf, 0,        VLength1),
					CAGD_MESH_UV(Srf, ULength1, 0),
					CAGD_MESH_UV(Srf, ULength1, VLength1));
	    break;
	default:
	    FATAL_ERROR(CAGD_ERR_UNSUPPORT_PT);
	    break;
    }

    if (PlaneSize < EPSILON) {
	if (CoercedSrf != NULL)
	    CagdSrfFree(CoercedSrf);
	return INFINITY;
    }

    for (i = ULength *VLength; i > 0; i--) {
	CagdRType
	    Dist = CagdDistPtPlane(&Plane, Points, i, 3);

	if (Dist > MaxDist)
	    MaxDist = Dist;
    }

    if (CoercedSrf != NULL)
	CagdSrfFree(CoercedSrf);

    return MaxDist / PlaneSize;
}