conmat.c

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

   ConMat[2][2] = ConicP->F;

   /* inverse transformation */
   D = TMatP->a*TMatP->d - TMatP->b*TMatP->c;
   InvMat[0][0] = TMatP->d/D;
   InvMat[0][1] = -TMatP->b/D;
   InvMat[0][2] = 0.0;
   InvMat[1][0] = -TMatP->c/D;
   InvMat[1][1] = TMatP->a/D;
   InvMat[1][2] = 0.0;
   InvMat[2][0] = (TMatP->c*TMatP->n - TMatP->d*TMatP->m)/D;
   InvMat[2][1] = (TMatP->b*TMatP->m - TMatP->a*TMatP->n)/D;
   InvMat[2][2] = 1.0;

   /* compute transpose */
   for (i = 0;i < 3;i++)
      for (j = 0;j < 3;j++)
	 TranInvMat[j][i] = InvMat[i][j];

   /* multiply the matrices */
   MultMat3((double *)InvMat,(double *)ConMat,(double *)Result1);
   MultMat3((double *)Result1,(double *)TranInvMat,(double *)Result2);
   ConicP->A = Result2[0][0];	       /* return to conic form */
   ConicP->B = Result2[0][1];
   ConicP->C = Result2[1][1];
   ConicP->D = Result2[0][2];
   ConicP->E = Result2[1][2];
   ConicP->F = Result2[2][2];
   }

/* Compute the intersection of a circle and a line */
/* See Graphic Gems Volume 1, page 5 for a description of this algorithm */
int IntCirLine(Point *IntPts,Circle *Cir,Line *Ln)
   {
   Point G,V;
   double a,b,c,d,t,sqrt_d;

   G.X = Ln->P1.X - Cir->Center.X;     	/* G = Ln->P1 - Cir->Center */
   G.Y = Ln->P1.Y - Cir->Center.Y;
   V.X = Ln->P2.X - Ln->P1.X;           /* V = Ln->P2 - Ln->P1 */
   V.Y = Ln->P2.Y - Ln->P1.Y;
   a = V.X*V.X + V.Y*V.Y;		/* a = V.V */
   b = V.X*G.X + V.Y*G.Y;b += b; 	/* b = 2(V.G) */
   c = (G.X*G.X + G.Y*G.Y) -    	/* c = G.G + Circle->Radius^2 */
      Cir->Radius*Cir->Radius;
   d = b*b - 4.0*a*c;			/* discriminant */

   if (d <= 0.0)
      return 0;				/* no intersections */

   sqrt_d = sqrt(d);
   t = (-b + sqrt_d)/(a + a);           /* t = (-b +/- sqrt(d))/2a */
   IntPts[0].X = Ln->P1.X + t*V.X;      /* Pt = Ln->P1 + t V */
   IntPts[0].Y = Ln->P1.Y + t*V.Y;
   t = (-b - sqrt_d)/(a + a);
   IntPts[1].X = Ln->P1.X + t*V.X;
   IntPts[1].Y = Ln->P1.Y + t*V.Y;
   return 2;
   }

/* compute all intersections of two ellipses */
/* E1 and E2 are the two ellipses */
/* IntPts points to an array of twelve points
    (some duplicates may be returned) */
/* The number of intersections found is returned */
/* Both ellipses are assumed to have non-zero radii */
int Int2Elip(Point *IntPts,Ellipse *E1,Ellipse *E2)
   {
   TMat ElpCirMat1,ElpCirMat2,InvMat,TempMat;
   Conic Conic1,Conic2,Conic3,TempConic;
   double Roots[3],qRoots[2];
   static Circle TestCir = {{0.0,0.0},1.0};
   Line TestLine[2];
   Point TestPoint;
   double PolyCoef[4];		/* coefficients of the polynomial */
   double D;			/* discriminant: B^2 - AC */
   double Phi;			/* ellipse rotation */
   double m,n;			/* ellipse translation */
   double Scl;			/* scaling factor */
   int NumRoots,NumLines;
   int CircleInts;		/* intersections between line and circle */
   int IntCount = 0;		/* number of intersections found */
   int i,j,k;

   /* compute the transformations which turn E1 and E2 into circles */
   Elp2Cir(E1,&ElpCirMat1);
   Elp2Cir(E2,&ElpCirMat2);

   /* compute the inverse transformation of ElpCirMat1 */
   InvElp2Cir(E1,&InvMat);

   /* Compute the characteristic matrices */
   GenEllipseCoefs(E1,&Conic1);
   GenEllipseCoefs(E2,&Conic2);

   /* Find x such that Det(Conic1 + x Conic2) = 0 */
   PolyCoef[0] = -Conic1.C*Conic1.D*Conic1.D + 2.0*Conic1.B*Conic1.D*Conic1.E -
      Conic1.A*Conic1.E*Conic1.E - Conic1.B*Conic1.B*Conic1.F +
      Conic1.A*Conic1.C*Conic1.F;
   PolyCoef[1] = -(Conic2.C*Conic1.D*Conic1.D) -
      2.0*Conic1.C*Conic1.D*Conic2.D + 2.0*Conic2.B*Conic1.D*Conic1.E +
      2.0*Conic1.B*Conic2.D*Conic1.E - Conic2.A*Conic1.E*Conic1.E +
      2.0*Conic1.B*Conic1.D*Conic2.E - 2.0*Conic1.A*Conic1.E*Conic2.E -
      2.0*Conic1.B*Conic2.B*Conic1.F + Conic2.A*Conic1.C*Conic1.F +
      Conic1.A*Conic2.C*Conic1.F - Conic1.B*Conic1.B*Conic2.F +
      Conic1.A*Conic1.C*Conic2.F;
   PolyCoef[2] = -2.0*Conic2.C*Conic1.D*Conic2.D - Conic1.C*Conic2.D*Conic2.D +
      2.0*Conic2.B*Conic2.D*Conic1.E + 2.0*Conic2.B*Conic1.D*Conic2.E +
      2.0*Conic1.B*Conic2.D*Conic2.E - 2.0*Conic2.A*Conic1.E*Conic2.E -
      Conic1.A*Conic2.E*Conic2.E - Conic2.B*Conic2.B*Conic1.F +
      Conic2.A*Conic2.C*Conic1.F - 2.0*Conic1.B*Conic2.B*Conic2.F +
      Conic2.A*Conic1.C*Conic2.F + Conic1.A*Conic2.C*Conic2.F;
   PolyCoef[3] = -Conic2.C*Conic2.D*Conic2.D + 2.0*Conic2.B*Conic2.D*Conic2.E -
      Conic2.A*Conic2.E*Conic2.E - Conic2.B*Conic2.B*Conic2.F +
      Conic2.A*Conic2.C*Conic2.F;
   NumRoots = SolveCubic(PolyCoef,Roots);

   if (NumRoots == 0)
      return 0;

   /* we try all the roots, even though it's redundant, so that we
      avoid some pathological situations */
   for (i=0;i<NumRoots;i++)
      {
      NumLines = 0;

      /* Conic3 = Conic1 + mu Conic2 */
      Conic3.A = Conic1.A + Roots[i]*Conic2.A;
      Conic3.B = Conic1.B + Roots[i]*Conic2.B;
      Conic3.C = Conic1.C + Roots[i]*Conic2.C;
      Conic3.D = Conic1.D + Roots[i]*Conic2.D;
      Conic3.E = Conic1.E + Roots[i]*Conic2.E;
      Conic3.F = Conic1.F + Roots[i]*Conic2.F;

      D = Conic3.B*Conic3.B - Conic3.A*Conic3.C;
      if (IsZero(Conic3.A) && IsZero(Conic3.B) && IsZero(Conic3.C))
	 {
	 /* Case 1 - Single line */
	 NumLines = 1;
	 /* compute endpoints of the line, avoiding division by zero */
	 if (fabs(Conic3.D) > fabs(Conic3.E))
	    {
	    TestLine[0].P1.Y = 0.0;
	    TestLine[0].P1.X = -Conic3.F/(Conic3.D + Conic3.D);
	    TestLine[0].P2.Y = 1.0;
	    TestLine[0].P2.X = -(Conic3.E + Conic3.E + Conic3.F)/
	       (Conic3.D + Conic3.D);
	    }
	 else
	    {
	    TestLine[0].P1.X = 0.0;
	    TestLine[0].P1.Y = -Conic3.F/(Conic3.E + Conic3.E);
	    TestLine[0].P2.X = 1.0;
	    TestLine[0].P2.X = -(Conic3.D + Conic3.D + Conic3.F)/
	       (Conic3.E + Conic3.E);
	    }
	 }
      else
	 {
	 /* use the espresion for Phi that takes atan of the
	    smallest argument */
	 Phi = (fabs(Conic3.B + Conic3.B) < fabs(Conic3.A-Conic3.C)?
	    atan((Conic3.B + Conic3.B)/(Conic3.A - Conic3.C)):
	    M_PI_2 - atan((Conic3.A - Conic3.C)/(Conic3.B + Conic3.B)))/2.0;
	 if (IsZero(D))
	    {
	    /* Case 2 - Parallel lines */
	    TempConic = Conic3;
	    TempMat = IdentMat;
	    RotateMat(&TempMat,-Phi);
	    TransformConic(&TempConic,&TempMat);
	    if (IsZero(TempConic.C))   /* vertical */
	       {
	       PolyCoef[0] = TempConic.F;
	       PolyCoef[1] = TempConic.D;
	       PolyCoef[2] = TempConic.A;
	       if ((NumLines=SolveQuadic(PolyCoef,qRoots))!=0)
		  {
		  TestLine[0].P1.X = qRoots[0];
		  TestLine[0].P1.Y = -1.0;
		  TestLine[0].P2.X = qRoots[0];
		  TestLine[0].P2.Y = 1.0;
		  if (NumLines==2)
		     {
		     TestLine[1].P1.X = qRoots[1];
		     TestLine[1].P1.Y = -1.0;
		     TestLine[1].P2.X = qRoots[1];
		     TestLine[1].P2.Y = 1.0;
		     }
		  }
	       }
	    else		    /* horizontal */
	       {
	       PolyCoef[0] = TempConic.F;
	       PolyCoef[1] = TempConic.E;
	       PolyCoef[2] = TempConic.C;
	       if ((NumLines=SolveQuadic(PolyCoef,qRoots))!=0)
		  {
		  TestLine[0].P1.X = -1.0;
		  TestLine[0].P1.Y = qRoots[0];
		  TestLine[0].P2.X = 1.0;
		  TestLine[0].P2.Y = qRoots[0];
		  if (NumLines==2)
		     {
		     TestLine[1].P1.X = -1.0;
		     TestLine[1].P1.Y = qRoots[1];
		     TestLine[1].P2.X = 1.0;
		     TestLine[1].P2.Y = qRoots[1];
		     }
		  }
	       }
	    TempMat = IdentMat;
	    RotateMat(&TempMat,Phi);
	    TransformPoint(&TestLine[0].P1,&TempMat);
	    TransformPoint(&TestLine[0].P2,&TempMat);
	    if (NumLines==2)
	       {
	       TransformPoint(&TestLine[1].P1,&TempMat);
	       TransformPoint(&TestLine[1].P2,&TempMat);
	       }
	    }
	 else
	    {
	    /* Case 3 - Crossing lines */
	    NumLines = 2;

	    /* translate the system so that the intersection of the lines
	       is at the origin */
	    TempConic = Conic3;
	    m = (Conic3.C*Conic3.D - Conic3.B*Conic3.E)/D;
	    n = (Conic3.A*Conic3.E - Conic3.B*Conic3.D)/D;
	    TempMat = IdentMat;
	    TranslateMat(&TempMat,-m,-n);
	    RotateMat(&TempMat,-Phi);
	    TransformConic(&TempConic,&TempMat);

	    /* Compute the line endpoints */
	    TestLine[0].P1.X = sqrt(fabs(1.0/TempConic.A));
	    TestLine[0].P1.Y = sqrt(fabs(1.0/TempConic.C));
	    Scl = max(TestLine[0].P1.X,TestLine[0].P1.Y);  /* adjust range */
	    TestLine[0].P1.X /= Scl;
	    TestLine[0].P1.Y /= Scl;
	    TestLine[0].P2.X = - TestLine[0].P1.X;
	    TestLine[0].P2.Y = - TestLine[0].P1.Y;
	    TestLine[1].P1.X = TestLine[0].P1.X;
	    TestLine[1].P1.Y = - TestLine[0].P1.Y;
	    TestLine[1].P2.X = - TestLine[1].P1.X;
	    TestLine[1].P2.Y = - TestLine[1].P1.Y;

	    /* translate the lines back */
	    TempMat = IdentMat;
	    RotateMat(&TempMat,Phi);
	    TranslateMat(&TempMat,m,n);
	    TransformPoint(&TestLine[0].P1,&TempMat);
	    TransformPoint(&TestLine[0].P2,&TempMat);
	    TransformPoint(&TestLine[1].P1,&TempMat);
	    TransformPoint(&TestLine[1].P2,&TempMat);
	    }
	 }

      /* find the ellipse line intersections */
      for (j = 0;j < NumLines;j++)
	 {
	 /* transform the line endpts into the circle space of the ellipse */
	 TransformPoint(&TestLine[j].P1,&ElpCirMat1);
	 TransformPoint(&TestLine[j].P2,&ElpCirMat1);

	 /* compute the number of intersections of the transformed line
	    and test circle */
	 CircleInts = IntCirLine(&IntPts[IntCount],&TestCir,&TestLine[j]);
	 if (CircleInts>0)
	    {
	    /* transform the intersection points back into ellipse space */
	    for (k = 0;k < CircleInts;k++)
	       TransformPoint(&IntPts[IntCount+k],&InvMat);
	    /* update the number of intersections found */
	    IntCount += CircleInts;
	    }
	 }
      }
   /* validate the points */
   j = IntCount;
   IntCount = 0;
   for (i = 0;i < j;i++)
      {
      TestPoint = IntPts[i];
      TransformPoint(&TestPoint,&ElpCirMat2);
      if (TestPoint.X < 2.0 && TestPoint.Y < 2.0 &&
	 IsZero(1.0 - sqrt(TestPoint.X*TestPoint.X +
	 TestPoint.Y*TestPoint.Y)))
	 IntPts[IntCount++]=IntPts[i];
      }
   return IntCount;
   }

/* Test routines */

/* Ellipse with center at (1,2), major radius 2, minor radius 1,
   and rotation 0. */
Ellipse TestEllipse={{2.0,1.0},2.0,1.0,0.0};

/* Transform matrix for shear of 45 degrees from vertical */
TMat TestTransform={1.0,0.0,1.0,1.0,0.0,0.0};

/* Display an ellipse. This version lists the structure values. */
void DisplayEllipse(Ellipse *EllipseP)
   {
   printf("\tCenter at (%6.3f,%6.3f)\n"
      "\tMajor radius: %6.3f\n"
      "\tMinor radius: %6.3f\n"
      "\tRotation: %6.3f degrees\n\n",
      EllipseP->Center.X,EllipseP->Center.Y,
      EllipseP->MaxRad,EllipseP->MinRad,
      180.0*EllipseP->Phi/M_PI);
   }


/* test ellipses for intersection */
Ellipse Elp1={{5.0,4.0},1.0,0.5,M_PI/3.0};
Ellipse Elp2={{4.0,3.0},2.0,1.0,0.0};
Ellipse Elp3={{1.0,1.0},2.0,1.0,M_PI_2};
Ellipse Elp4={{1.0,1.0},2.0,0.5,0.0};

void main(void)
   {
   Point IntPts[12];
   int IntCount;
   int i;

   /* find ellipse intersections */
   printf("Intersections of ellipses 1 & 2:\n\n");
   IntCount=Int2Elip(IntPts,&Elp1,&Elp2);
   for (i = 0;i < IntCount;i++)
      printf("   %f, %f\n",IntPts[i].X,IntPts[i].Y);
   printf("Intersections of ellipses 3 & 4:\n");
   IntCount=Int2Elip(IntPts,&Elp3,&Elp4);
   for (i = 0;i < IntCount;i++)
      printf("   %f, %f\n",IntPts[i].X,IntPts[i].Y);

   /* transform ellipses */
   printf("\n\nBefore transformation:\n");
   DisplayEllipse(&TestEllipse);

   TransformEllipse(&TestEllipse,&TestTransform);

   printf("After transformation:\n");
   DisplayEllipse(&TestEllipse);
   }