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

#include "cagd_loc.h"

/*****************************************************************************
* DESCRIPTION:                                                               M
* Computes the distance between two control points at given indices Index?.  M
* Only E2 or E3 point types are supported.				     M
*                                                                            *
* PARAMETERS:                                                                M
*   Points:    To compute the distance between.                              M
*   Index1:    Index of first point to consider.                             M
*   Index2:    Index of second point to consider.                            M
*   PType:     Type of points. Must be E2 or E3                              M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:    Euclidean distance between the two points.                 M
*                                                                            *
* KEYWORDS:                                                                  M
*   CagdDistanceTwoCtlPts, distance                                          M
*****************************************************************************/
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:
	    CAGD_FATAL_ERROR(CAGD_ERR_UNSUPPORT_PT);
	    return 0.0;
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Fits a plane through the four points from Points indices Index?. Points    M
* may be either E2 or E3 only.						     M
*   Returns 0.0 if failed to fit a plane, otherwise a measure on the size of M
* the mesh data (distance between points) is returned.			     M
*                                                                            *
* PARAMETERS:                                                                M
*   Plane:       To compute and save here.                                   M
*   PType:       Point type expected of four points. Must be E2 or E3.       M
*   Points:      Point array where to look for the four points.              M
*   Index?:      Four indices of the points.                                 M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:     Measure  on the distance between the data points, 0.0 if  M
*                  fitting failed.                                           M
*                                                                            *
* KEYWORDS:                                                                  M
*   CagdFitPlaneThruCtlPts, plane fit                                        M
*****************************************************************************/
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 < IRIT_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 < IRIT_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:
	    CAGD_FATAL_ERROR(CAGD_ERR_UNSUPPORT_PT);
	    break;
    }

    return SizeMeasure;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Computes and returns distance between point Index and given plane which is M
* assumed to be normalized, so that the A B C plane;s normal has a unit      M
* length.		                                                     M
*   Also assumes the Points are non rational with MaxDim dimension.	     M
*                                                                            *
* PARAMETERS:                                                                M
*   Plane:      To compute the distance to.                                  M
*   Points:     To compute the distance from.                                M
*   Index:      Index in Points for the point to consider.                   M
*   MaxDim:     Number of dimensions to consider. Less or equal to three.    M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:   Resulting distance.                                         M
*                                                                            *
* KEYWORDS:                                                                  M
*   CagdDistPtPlane, point plane distance                                    M
*****************************************************************************/
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);
}

/*****************************************************************************
* DESCRIPTION:                                                               *
* Computes and returns distance between point Index and the line from point  *
* index 0 in direction LineDir (usually the line direction to last the       *
* point).                                                                    *
*   LineDir is assumed to be unit length normalized vector.		     *
*                                                                            *
* PARAMETERS:                                                                *
*   LineDir:    To compute distance to.                                      *
*   Plane:      To compute the distance to.                                  *
*   Points:     To compute the distance from.                                *
*   Index:      Index in Points for the point to consider.                   *
*   MaxDim:     Number of dimensions to consider. Less or equal to three.    *
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:   Resulting distance.                                         M
*****************************************************************************/
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);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Tests polygonal colinearity by testing the distance of interior control    M  
* points from the line connecting the two control polygon end points.	     M
*   Returns a relative ratio of deviation from line relative to its length.  M
*   Zero means all points are colinear.					     M
*   If two end points are same ( no line can be fit ) INFINITY is returned.  M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:       To measure its colinearity.                                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:    Colinearity relative measure.                              M
*                                                                            *
* KEYWORDS:                                                                  M
*   CagdEstimateCrvColinearity, conversion, colinearity                      M
*****************************************************************************/
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;
	default:
	    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:
	    CAGD_FATAL_ERROR(CAGD_ERR_UNSUPPORT_PT);
	    break;
    }

    LineLen = CAGD_LEN_VECTOR(LineDir);
    if (LineLen < IRIT_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;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
 Tests mesh colinearity by testing the distance of interior points from the  M
* plane thru 3 corner points.						     M
*   Returns a relative ratio of deviation from plane relative to its size.   M
*   Zero means all points are coplanar.					     M
*   If end points are same ( no plane can be fit ) INFINITY is returned.     M
*                                                                            M
* PARAMETERS:                                                                M
*   Srf:        To measure its coplanarity.                                  M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:    Coplanarity measure.                                       M
*                                                                            *
* KEYWORDS:                                                                  M
*   CagdEstimateSrfPlanarity, conversion, coplanarity                        M
*****************************************************************************/
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;
	default:
	    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:
	    CAGD_FATAL_ERROR(CAGD_ERR_UNSUPPORT_PT);
	    break;
    }

    if (PlaneSize < IRIT_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;
}
