void cMathUtil::DeltaRot(const tVector& axis0, double theta0, const tVector& axis1, double theta1, tVector& out_axis, double& out_theta) { tMatrix R0 = RotateMat(axis0, theta0); tMatrix R1 = RotateMat(axis1, theta1); tMatrix M = DeltaRot(R0, R1); RotMatToAxisAngle(M, out_axis, out_theta); }
/* 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); }
/* 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); }
/* 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.Y = -(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] = 2*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] = 2*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; }