conmat.c

[Game.Programming].Academic - Graphics Gems (6 books source code)

/* CONMAT.C - Ellipse tranformation and intersection functions */
/* Written by Kenneth J. Hill, June, 24, 1994  */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <values.h>

#ifndef	M_PI
#define M_PI	3.14159265358979323846
#endif

#ifndef	M_PI_2
#define M_PI_2	1.57079632679489661923
#endif

#ifndef max
#define max(a,b) ((a)>=(b) ? (a) : (b) )
#endif

/* Type definitions */

/* Point structure */
typedef struct PointTag
   {
   double X,Y;
   } Point;

/* Line structure */
typedef struct LineTag
   {
   Point P1,P2;
   } Line;

/* Circle structure */
typedef struct CircleTag
   {
   Point Center;
   double Radius;
   } Circle;

/* Ellipse structure */
typedef struct EllipseTag
   {
   Point Center;		 /* ellipse center	 */
   double MaxRad,MinRad;	 /* major and minor axis */
   double Phi;			 /* major axis rotation  */
   } Ellipse;

/* Conic coefficients structure */
typedef struct ConicTag
   {
   double A,B,C,D,E,F;
   } Conic;

/* transformation matrix type */
typedef struct TMatTag
   {
   double a,b,c,d;		 /* tranformation coefficients */
   double m,n;			 /* translation coefficients   */
   } TMat;

/* prototypes */

/* Functions with names beginning with a O:
 *
 * 1. Ignore ellipse center coordinates.
 *
 * 2. Ignore any translation components in tranformation matrices.
 *
 * 3. Assume that conic coeficients were generated by "O-name" functions.
 *
 * This keeps the computations relatively simple. The ellipse centers
 * can then be transformed separately as points.
 */

/* OTransformConic - Transform conic coefficients about the origin */
void OTransformConic(Conic *ConicP,TMat *TMatP);

/* OGenEllipseCoefs - Generate conic coefficients of an ellipse */
void OGenEllipseCoefs(Ellipse *EllipseP,Conic *ConicP);

/* OGenEllipseGeom - Generates ellipse geometry from conic coefficients */
void OGenEllipseGeom(Conic *ConicP,Ellipse *EllipseP);

/* TransformPoint - Transform a point using a tranformation matrix */
void TransformPoint(Point *PointP,TMat *TMatP);

/* TransformEllipse - Transform an ellipse using a tranformation matrix */
void TransformEllipse(Ellipse *EllipseP,TMat *TMatP);

/* The following functions are defined in cubicand.c, on
   the Graphic Gems I diskette. */
int SolveCubic(double c[4],double s[3]);
int SolveQuadic(double c[3],double dest[2]);


/* Identity matrix */
static const TMat IdentMat={1.0,0.0,0.0,1.0,0.0,0.0};

/* Transformation matrix routines */

/* Translate a matrix by m,n */
void TranslateMat(TMat *Mat,double m,double n)
   {
   Mat->m += m;
   Mat->n += n;
   }

/* Rotate a matrix by Phi */
void RotateMat(TMat *Mat,double Phi)
   {
   double SinPhi=sin(Phi);
   double CosPhi=cos(Phi);
   TMat temp=*Mat;		/* temporary copy of Mat */

   /* These are just the matrix operations written out long hand */
   Mat->a = temp.a*CosPhi - temp.b*SinPhi;
   Mat->b = temp.b*CosPhi + temp.a*SinPhi;
   Mat->c = temp.c*CosPhi - temp.d*SinPhi;
   Mat->d = temp.d*CosPhi + temp.c*SinPhi;
   Mat->m = temp.m*CosPhi - temp.n*SinPhi;
   Mat->n = temp.n*CosPhi + temp.m*SinPhi;
   }

/* Scale a matrix by sx, sy */
void ScaleMat(TMat *Mat,double sx,double sy)
   {
   Mat->a *= sx;
   Mat->b *= sy;
   Mat->c *= sx;
   Mat->d *= sy;
   Mat->m *= sx;
   Mat->n *= sy;
   }

/* TransformPoint - Transform a point using a tranformation matrix */
void TransformPoint(Point *PointP,TMat *TMatP)
   {
   Point TempPoint;

   TempPoint.X = PointP->X*TMatP->a + PointP->Y*TMatP->c + TMatP->m;
   TempPoint.Y = PointP->X*TMatP->b + PointP->Y*TMatP->d + TMatP->n;
   *PointP=TempPoint;
   }

/* Conic routines */

/* near zero test */
#define EPSILON 1e-9
#define IsZero(x) (x > -EPSILON && x < EPSILON)

/* GenEllipseCoefs - Generate conic coefficients of an ellipse */
static void GenEllipseCoefs(Ellipse *elip,Conic *M)
   {
   double sqr_r1,sqr_r2;
   double sint,cost,sin2t,sqr_sint,sqr_cost;
   double cenx,ceny,sqr_cenx,sqr_ceny,invsqr_r1,invsqr_r2;

   /* common coeficients */
   sqr_r1 = elip->MaxRad;
   sqr_r2 = elip->MinRad;
   sqr_r1 *= sqr_r1;
   sqr_r2 *= sqr_r2;
   sint = sin(elip->Phi);
   cost = cos(elip->Phi);
   sin2t = 2.0*sint*cost;
   sqr_sint = sint*sint;
   sqr_cost = cost*cost;
   cenx = elip->Center.X;
   sqr_cenx = cenx*cenx;
   ceny = elip->Center.Y;
   sqr_ceny = ceny*ceny;
   invsqr_r1 = 1.0/sqr_r1;
   invsqr_r2 = 1.0/sqr_r2;

   /* Compute the coefficients. These formulae are the transformations
      on the unit circle written out long hand */
   M->A = sqr_cost/sqr_r1 + sqr_sint/sqr_r2;
   M->B = (sqr_r2-sqr_r1)*sin2t/(2.0*sqr_r1*sqr_r2);
   M->C = sqr_cost/sqr_r2 + sqr_sint/sqr_r1;
   M->D = -ceny*M->B-cenx*M->A;
   M->E = -cenx*M->B-ceny*M->C;
   M->F = -1.0 + (sqr_cenx + sqr_ceny)*(invsqr_r1 + invsqr_r2)/2.0 +
      (sqr_cost - sqr_sint)*(sqr_cenx - sqr_ceny)*(invsqr_r1 - invsqr_r2)/2.0 +
      cenx*ceny*(invsqr_r1 - invsqr_r2)*sin2t;
   }

/* Compute the transformation which turns an ellipse into a circle */
void Elp2Cir(Ellipse *Elp,TMat *CirMat)
   {
   /* Start with identity matrix */
   *CirMat = IdentMat;
   /* Translate to origin */
   TranslateMat(CirMat,-Elp->Center.X,-Elp->Center.Y);
   /* Rotate into standard position */
   RotateMat(CirMat,-Elp->Phi);
   /* Scale into a circle. */
   ScaleMat(CirMat,1.0/Elp->MaxRad,1.0/Elp->MinRad);
   }

/* Compute the inverse of the transformation
   which turns an ellipse into a circle */
void InvElp2Cir(Ellipse *Elp,TMat *InvMat)
   {
   /* Start with identity matrix */
   *InvMat = IdentMat;
   /* Scale back into an ellipse. */
   ScaleMat(InvMat,Elp->MaxRad,Elp->MinRad);
   /* Rotate */
   RotateMat(InvMat,Elp->Phi);
   /* Translate from origin */
   TranslateMat(InvMat,Elp->Center.X,Elp->Center.Y);
   }

/* OTransformConic - Transform conic coefficients about the origin	 */
/* This routine ignores the translation components of *TMatP and
   assumes the conic is "centered" at the origin (i.e. D,E=0, F=-1)	 */
/* The computations are just the matrix operations written out long hand */
/* This code assumes that the transformation is not degenerate		 */
void OTransformConic(Conic *ConicP,TMat *TMatP)
   {
   double A,B,C,Denom;

   /* common denominator for transformed cooefficients */
   Denom = TMatP->a*TMatP->d - TMatP->b*TMatP->c;
   Denom *= Denom;

   A = (ConicP->C*TMatP->b*TMatP->b - 2.0*ConicP->B*TMatP->b*TMatP->d +
      ConicP->A*TMatP->d*TMatP->d)/Denom;

   B = (-ConicP->C*TMatP->a*TMatP->b + ConicP->B*TMatP->b*TMatP->c +
      ConicP->B*TMatP->a*TMatP->d - ConicP->A*TMatP->c*TMatP->d)/Denom;

   C = (ConicP->C*TMatP->a*TMatP->a - 2.0*ConicP->B*TMatP->a*TMatP->c +
      ConicP->A*TMatP->c*TMatP->c)/Denom;

   ConicP->A=A;
   ConicP->B=B;
   ConicP->C=C;
   }

/* OGenEllipseCoefs - Generate conic coefficients of an ellipse */
/* The ellipse is assumed to be centered at the origin. */
void OGenEllipseCoefs(Ellipse *EllipseP,Conic *ConicP)
   {
   double SinPhi = sin(EllipseP->Phi); /* sine of ellipse rotation   */
   double CosPhi = cos(EllipseP->Phi); /* cosine of ellipse rotation */
   double SqSinPhi = SinPhi*SinPhi;    /* square of sin(phi)	     */
   double SqCosPhi = CosPhi*CosPhi;    /* square of cos(phi)	     */
   double SqMaxRad = EllipseP->MaxRad*EllipseP->MaxRad;
   double SqMinRad = EllipseP->MinRad*EllipseP->MinRad;

   /* compute coefficients for the ellipse in standard position */
   ConicP->A = SqCosPhi/SqMaxRad + SqSinPhi/SqMinRad;
   ConicP->B = (1.0/SqMaxRad - 1.0/SqMinRad)*SinPhi*CosPhi;
   ConicP->C = SqCosPhi/SqMinRad + SqSinPhi/SqMaxRad;
   ConicP->D = ConicP->E = 0.0;
   ConicP->F = -1.0;
   }

/* OGenEllipseGeom - Generates ellipse geometry from conic coefficients */
/* This routine assumes the conic coefficients D=E=0, F=-1 */
void OGenEllipseGeom(Conic *ConicP,Ellipse *EllipseP)
   {
   double Numer,Denom,Temp;
   TMat DiagTransform;		 /* transform diagonalization */
   Conic ConicT = *ConicP;	 /* temporary copy of conic coefficients */
   double SinPhi,CosPhi;

   /* compute new ellipse rotation */
   Numer = ConicT.B + ConicT.B;
   Denom = ConicT.A - ConicT.C;
   /* Phi = 1/2 atan(Numer/Denom) = 1/2 (pi/2 - atan(Denom/Numer)
      We use the form that keeps the argument to atan between -1 and 1 */

   EllipseP->Phi = 0.5*(fabs(Numer) < fabs(Denom)?
      atan(Numer/Denom):M_PI_2-atan(Denom/Numer));

   /* diagonalize the conic */
   SinPhi = sin(EllipseP->Phi);
   CosPhi = cos(EllipseP->Phi);
   DiagTransform.a = CosPhi;	 /* rotate by -Phi */
   DiagTransform.b = -SinPhi;
   DiagTransform.c = SinPhi;
   DiagTransform.d = CosPhi;
   DiagTransform.m = DiagTransform.n = 0.0;
   OTransformConic(&ConicT,&DiagTransform);

   /* compute new radii from diagonalized coefficients */
   EllipseP->MaxRad = 1.0/sqrt(ConicT.A);
   EllipseP->MinRad = 1.0/sqrt(ConicT.C);

   /* be sure MaxRad >= MinRad */
   if (EllipseP->MaxRad < EllipseP->MinRad)
      {
      Temp = EllipseP->MaxRad;	 /* exchange the radii */
      EllipseP->MaxRad = EllipseP->MinRad;
      EllipseP->MinRad = Temp;
      EllipseP->Phi += M_PI_2;	 /* adjust the rotation */
      }
   }

/* TransformEllipse - Transform an ellipse using a tranformation matrix */
void TransformEllipse(Ellipse *EllipseP,TMat *TMatP)
   {
   Conic EllipseCoefs;

   /* generate the ellipse coefficients (using Center=origin) */
   OGenEllipseCoefs(EllipseP,&EllipseCoefs);

   /* transform the coefficients */
   OTransformConic(&EllipseCoefs,TMatP);

   /* turn the transformed coefficients back into geometry */
   OGenEllipseGeom(&EllipseCoefs,EllipseP);

   /* translate the center */
   TransformPoint(&EllipseP->Center,TMatP);
   }

/* MultMat3 - Multiply two 3x3 matrices */
void MultMat3(double *Mat1,double *Mat2, double *Result)
   {
   int i,j;

   for (i = 0;i < 3;i++)
      for (j = 0;j < 3;j++)
	 Result[i*3+j] = Mat1[i*3+0]*Mat2[0*3+j] +
	    Mat1[i*3+1]*Mat2[1*3+j] +
	    Mat1[i*3+2]*Mat2[2*3+j];
   }

/* Transform a conic by a tranformation matrix */
void TransformConic(Conic *ConicP,TMat *TMatP)
   {
   double InvMat[3][3],ConMat[3][3],TranInvMat[3][3];
   double Result1[3][3],Result2[3][3];
   double D;
   int i,j;

   /* Compute M' = Inv(TMat).M.Transpose(Inv(TMat))

   /* compute the transformation using matrix muliplication */
   ConMat[0][0] = ConicP->A;
   ConMat[0][1] = ConMat[1][0] = ConicP->B;
   ConMat[1][1] = ConicP->C;
   ConMat[0][2] = ConMat[2][0] = ConicP->D;
   ConMat[1][2] = ConMat[2][1] = ConicP->E;