C++ RS_VectorSolutions::rotate方法代码示例

/**
 * @return One or two intersection points between given entities.
 */
RS_VectorSolutions RS_Information::getIntersectionEllipseLine(RS_Line* line,
        RS_Ellipse* ellipse) {

    RS_VectorSolutions ret;

    if (line==NULL || ellipse==NULL) {
        return ret;
    }

    // rotate into normal position:

    double rx = ellipse->getMajorRadius();
    if(rx<RS_TOLERANCE) {
        //zero radius ellipse
        RS_Vector vp(line->getNearestPointOnEntity(ellipse->getCenter(), true));
        if((vp-ellipse->getCenter()).squared() <RS_TOLERANCE2){
            //center on line
            ret.push_back(vp);
        }
        return ret;
    }
    RS_Vector angleVector = ellipse->getMajorP().scale(RS_Vector(1./rx,-1./rx));
    double ry = rx*ellipse->getRatio();
    RS_Vector center = ellipse->getCenter();
    RS_Vector a1 = line->getStartpoint().rotate(center, angleVector);
    RS_Vector a2 = line->getEndpoint().rotate(center, angleVector);
//    RS_Vector origin = a1;
    RS_Vector dir = a2-a1;
    RS_Vector diff = a1 - center;
    RS_Vector mDir = RS_Vector(dir.x/(rx*rx), dir.y/(ry*ry));
    RS_Vector mDiff = RS_Vector(diff.x/(rx*rx), diff.y/(ry*ry));

    double a = RS_Vector::dotP(dir, mDir);
    double b = RS_Vector::dotP(dir, mDiff);
    double c = RS_Vector::dotP(diff, mDiff) - 1.0;
    double d = b*b - a*c;

//    std::cout<<"RS_Information::getIntersectionEllipseLine(): d="<<d<<std::endl;
    if (d < - 1.e3*RS_TOLERANCE*sqrt(RS_TOLERANCE)) {
        RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: outside 0");
        return ret;
    }
    if( d < 0. ) d=0.;
    double root = sqrt(d);
    double t_a = -b/a;
    double t_b = root/a;
    //        double t_b = (-b + root) / a;

    ret.push_back(a1.lerp(a2,t_a+t_b));
    RS_Vector vp(a1.lerp(a2,t_a-t_b));
    if ( (ret.get(0)-vp).squared()>RS_TOLERANCE2) {
        ret.push_back(vp);
    }
    angleVector.y *= -1.;
    ret.rotate(center, angleVector);
//    std::cout<<"found Ellipse-Line intersections: "<<ret.getNumber()<<std::endl;
//    std::cout<<ret<<std::endl;
    RS_DEBUG->print("RS_Information::getIntersectionEllipseLine(): done");
    return ret;
}

/**
  *find the tangential points from a given point, i.e., the tangent lines should pass
  * the given point and tangential points
  *
  *Author: Dongxu Li
  */
RS_VectorSolutions RS_Ellipse::getTangentPoint(const RS_Vector& point) const {
    RS_Vector point2(point);
    point2.move(-getCenter());
    RS_Vector aV(-getAngle());
    point2.rotate(aV);
    RS_VectorSolutions sol;
    double a=getMajorRadius();
    if(a<RS_TOLERANCE || getRatio()<RS_TOLERANCE) return sol;
    RS_Circle c(NULL, RS_CircleData(RS_Vector(0.,0.),a));
    point2.y /=getRatio();
    sol=c.getTangentPoint(point2);
    sol.scale(RS_Vector(1.,getRatio()));
    aV.y *= -1.;
    sol.rotate(aV);
    sol.move(getCenter());
    return sol;
}

/**
 * @return One or two intersection points between given entities.
 */
RS_VectorSolutions RS_Information::getIntersectionLineEllipse(RS_Line* line,
        RS_Ellipse* ellipse) {

    RS_VectorSolutions ret;

    if (line==NULL || ellipse==NULL) {
        return ret;
    }

    // rotate into normal position:
    double ang = ellipse->getAngle();

    double rx = ellipse->getMajorRadius();
    double ry = ellipse->getMinorRadius();
    RS_Vector center = ellipse->getCenter();
    RS_Vector a1 = line->getStartpoint().rotate(center, -ang);
    RS_Vector a2 = line->getEndpoint().rotate(center, -ang);
    RS_Vector origin = a1;
    RS_Vector dir = a2-a1;
    RS_Vector diff = origin - center;
    RS_Vector mDir = RS_Vector(dir.x/(rx*rx), dir.y/(ry*ry));
    RS_Vector mDiff = RS_Vector(diff.x/(rx*rx), diff.y/(ry*ry));

    double a = RS_Vector::dotP(dir, mDir);
    double b = RS_Vector::dotP(dir, mDiff);
    double c = RS_Vector::dotP(diff, mDiff) - 1.0;
    double d = b*b - a*c;

    if (d < 0) {
        RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: outside 0");
    } else if ( d > 0 ) {
        double root = sqrt(d);
        double t_a = (-b - root) / a;
        double t_b = (-b + root) / a;

        /*if ( (t_a < 0 || 1 < t_a) && (t_b < 0 || 1 < t_b) ) {
            if ( (t_a < 0 && t_b < 0) || (t_a > 1 && t_b > 1) ) {
                RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: outside 1");
            }
            else {
                RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: inside 1");
            }
        } else {*/
        RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: intersection 1");
        RS_Vector ret1(false);
        RS_Vector ret2(false);
        //if ( 0 <= t_a && t_a <= 1 ) {
        //RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: 0<=t_a<=1");
        ret1 = a1.lerp(a2, t_a);
        RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: ret1: %f/%f", ret1.x, ret1.y);
        //}
        //if ( 0 <= t_b && t_b <= 1 ) {
        //RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: 0<=t_b<=1");
        ret2 = a1.lerp(a2, t_b);
        RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: ret2: %f/%f", ret2.x, ret2.y);
        //}
        if (ret1.valid && ret2.valid) {
            ret = RS_VectorSolutions(ret1, ret2);
        }
        else {
            if (ret1.valid) {
                ret = RS_VectorSolutions(ret1);
            }
            if (ret2.valid) {
                ret = RS_VectorSolutions(ret2);
            }
        }
        //}
    } else {
        double t = -b/a;
        if ( 0 <= t && t <= 1 ) {
            RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: 0<=t<=1");
            RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: intersection 2");
            ret = RS_VectorSolutions(a1.lerp(a2, t));
            RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: ret1: %f/%f", ret.get(0).x, ret.get(0).y);
        } else {
            RS_DEBUG->print("RS_Information::getIntersectionLineEllipse: outside 2");
        }
    }

    ret.rotate(center, ang);
    return ret;



    /*
    RS_Arc* arc = new RS_Arc(NULL,
                             RS_ArcData(ellipse->getCenter(),
                                        ellipse->getMajorRadius(),
                                        ellipse->getAngle1(),
                                        ellipse->getAngle2(),
                                        false));
    RS_Line* other = (RS_Line*)line->clone();
    double angle = ellipse->getAngle();
    //double ratio = ellipse->getRatio();

    // rotate entities:
//.........这里部分代码省略.........

RS_VectorSolutions RS_Information::getIntersectionEllipseEllipse(RS_Ellipse* e1, RS_Ellipse* e2) {
    RS_VectorSolutions ret;

    if (e1==NULL || e2==NULL ) {
        return ret;
    }
    if (
        (e1->getCenter() - e2 -> getCenter() ).magnitude() < RS_TOLERANCE &&
        ( e1->getMajorP() - e2 ->getMajorP()).magnitude() < RS_TOLERANCE &&
        fabs(e1->getMajorRadius() - e2 ->getMajorRadius()) < RS_TOLERANCE &&
        fabs(e1->getMinorRadius() - e2 ->getMinorRadius()) < RS_TOLERANCE
    ) { // overlapped ellipses, do not do overlap
        return ret;
    }

    RS_Ellipse *e01= ( RS_Ellipse *) e1->clone();
    if( e01->getMajorRadius() < e01->getMinorRadius() ) e01->switchMajorMinor();
    RS_Ellipse *e02= ( RS_Ellipse *) e2->clone();
    if( e02->getMajorRadius() < e02->getMinorRadius() ) e02->switchMajorMinor();
    //transform ellipse2 to ellipse1's coordinates
    RS_Vector shiftc1=- e01->getCenter();
    double shifta1=-e01->getAngle();
    e02->move(shiftc1);
    e02->rotate(shifta1);
    RS_Vector majorP2=e02->getMajorP();
    double a1=e01->getMajorRadius();
    double b1=e01->getMinorRadius();
    double x2=e02->getCenter().x,
           y2=e02->getCenter().y;
    double a2=e02->getMajorRadius();
    double b2=e02->getMinorRadius();

    if( e01->getMinorRadius() < RS_TOLERANCE || e01 -> getRatio()< RS_TOLERANCE) {
        // treate e01 as a line
        RS_LineData ldata0(RS_Vector(-a1,0.),RS_Vector(a1,0.));
        RS_Line *l0=new RS_Line(e1->getParent(),ldata0);
        ret= getIntersectionLineEllipse(l0, e02);
        ret.rotate(-shifta1);
        ret.move(-shiftc1);
        return ret;
    }
    if( e02->getMinorRadius() < RS_TOLERANCE || e02 -> getRatio()< RS_TOLERANCE) {
        // treate e02 as a line
        RS_LineData ldata0(RS_Vector(-a2,0.),RS_Vector(a2,0.));
        RS_Line *l0=new RS_Line(e1->getParent(),ldata0);
        l0->rotate(RS_Vector(0.,0.),e02->getAngle());
        l0->move(e02->getCenter());
        ret= getIntersectionLineEllipse(l0, e01);
        ret.rotate(-shifta1);
        ret.move(-shiftc1);
        return ret;
    }

    //ellipse01 equation:
    //	x^2/(a1^2) + y^2/(b1^2) - 1 =0
    double t2= - e02->getAngle();
    //ellipse2 equation:
    // ( (x - u) cos(t) - (y - v) sin(t))^2/a^2 + ( (x - u) sin(t) + (y-v) cos(t))^2/b^2 =1
    // ( cos^2/a^2 + sin^2/b^2) x^2 +
    // ( sin^2/a^2 + cos^2/b^2) y^2 +
    //  2 sin cos (1/b^2 - 1/a^2) x y +
    //  ( ( 2 v sin cos - 2 u cos^2)/a^2 - ( 2v sin cos + 2 u sin^2)/b^2) x +
    //  ( ( 2 u sin cos - 2 v sin^2)/a^2 - ( 2u sin cos + 2 v cos^2)/b^2) y +
    //  (u cos - v sin)^2/a^2 + (u sin + v cos)^2/b^2 -1 =0
    // detect whether any ellipse radius is zero
    double cs=cos(t2),si=sin(t2);
    double ucs=x2*cs,usi=x2*si,
           vcs=y2*cs,vsi=y2*si;
    double cs2=cs*cs,si2=1-cs2;
    double tcssi=2.*cs*si;
    double ia2=1./(a2*a2),ib2=1./(b2*b2);
//    std::cout<<"e1: x^2/("<<a1<<")^2+y^2/("<<b1<<")^2-1 =0\n";
//    std::cout<<"e2: ( (x-("<<x2<<"))*("<<cs<<")-(y-("<<y2<<"))*("<<si<<"))^2/"<<a2<<"^2+( ( x - ("<<x2<<"))*("<<si<<")+(y-("<<y2<<"))*("<<cs<<"))^2/"<<b2<<"^2 -1 =0\n";
    double mc1=(ucs - vsi)*(ucs-vsi)*ia2+(usi+vcs)*(usi+vcs)*ib2 -1.;
    double mb10= ( y2*tcssi - 2.*x2*cs2)*ia2 - ( y2*tcssi+2*x2*si2)*ib2; //x
    double mb11= ( x2*tcssi - 2.*y2*si2)*ia2 - ( x2*tcssi+2*y2*cs2)*ib2; //y
    double ma100= cs2*ia2 + si2*ib2; // x^2
    double ma101= cs*si*(ib2 - ia2); // xy term is 2*ma101*x*y
    double ma111= si2*ia2 + cs2*ib2; // y^2
    double ma000= 1./(a1*a1),ma011=1./(b1*b1);
//    std::cout<<"simplified e1: "<<ma000<<"*x^2 + "<<ma011<<"*y^2 -1 =0\n";
//    std::cout<<"simplified e2: "<<ma100<<"*x^2 + 2*("<<ma101<<")*x*y + "<<ma111<<"*y^2 "<<" + ("<<mb10<<")*x + ("<<mb11<<")*y + ("<<mc1<<") =0\n";
    // construct the Bezout determinant
    double v0=2.*ma000*ma101;
    double v2=ma000*mb10;
    double v3=ma000*mb11;
    double v4=ma000*mc1+ma100;
    //double v5= 2.*ma101*ma011;
    //double v6= ma000*ma111;
    //double v7= 2.*ma101;
    double v8= 2.*ma011*mb10;
    //double v9= ma100*ma011;
    double v1=ma000*ma111-ma100*ma011;
    //double v1= v6 - v9;
    double u0 = v4*v4-v2*mb10;
    double u1 = 2.*(v3*v4-v0*mb10);
    double u2 = 2.*(v4*v1-ma101*v0)+v3*v3+0.5*v2*v8;
    double u3 = v0*v8+2.*v3*v1;
    double u4 = v1*v1+2.*ma101*ma011*v0;
    //std::cout<<"u0="<<u0<<"\tu1="<<u1<<"\tu2="<<u2<<"\tu3="<<u3<<"\tu4="<<u4<<std::endl;
//.........这里部分代码省略.........

RS_VectorSolutions LC_Quadratic::getIntersection(const LC_Quadratic& l1, const LC_Quadratic& l2)
{
    RS_VectorSolutions ret;
    if( l1.isValid()==false || l2.isValid()==false ) {
//        DEBUG_HEADER
//        std::cout<<l1<<std::endl;
//        std::cout<<l2<<std::endl;
        return ret;
    }
    auto p1=&l1;
    auto p2=&l2;
    if(p1->isQuadratic()==false){
        std::swap(p1,p2);
    }
	if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
		DEBUG_HEADER
		std::cout<<*p1<<std::endl;
		std::cout<<*p2<<std::endl;
	}
    if(p1->isQuadratic()==false){
        //two lines
		std::vector<std::vector<double> > ce(2,std::vector<double>(3,0.));
        ce[0][0]=p1->m_vLinear(0);
        ce[0][1]=p1->m_vLinear(1);
        ce[0][2]=-p1->m_dConst;
        ce[1][0]=p2->m_vLinear(0);
        ce[1][1]=p2->m_vLinear(1);
        ce[1][2]=-p2->m_dConst;
		std::vector<double> sn(2,0.);
        if(RS_Math::linearSolver(ce,sn)){
            ret.push_back(RS_Vector(sn[0],sn[1]));
        }
        return ret;
    }
    if(p2->isQuadratic()==false){
        //one line, one quadratic
        //avoid division by zero
        if(fabs(p2->m_vLinear(0))+DBL_EPSILON<fabs(p2->m_vLinear(1))){
            ret=getIntersection(p1->flipXY(),p2->flipXY()).flipXY();
//            for(size_t j=0;j<ret.size();j++){
//                DEBUG_HEADER
//                std::cout<<j<<": ("<<ret[j].x<<", "<< ret[j].y<<")"<<std::endl;
//            }
            return ret;
        }
        std::vector<std::vector<double> >  ce(0);
		if(fabs(p2->m_vLinear(1))<RS_TOLERANCE){
            const double angle=0.25*M_PI;
            LC_Quadratic p11(*p1);
            LC_Quadratic p22(*p2);
            ce.push_back(p11.rotate(angle).getCoefficients());
            ce.push_back(p22.rotate(angle).getCoefficients());
            ret=RS_Math::simultaneousQuadraticSolverMixed(ce);
            ret.rotate(-angle);
//            for(size_t j=0;j<ret.size();j++){
//                DEBUG_HEADER
//                std::cout<<j<<": ("<<ret[j].x<<", "<< ret[j].y<<")"<<std::endl;
//            }
            return ret;
        }
        ce.push_back(p1->getCoefficients());
        ce.push_back(p2->getCoefficients());
        ret=RS_Math::simultaneousQuadraticSolverMixed(ce);
//        for(size_t j=0;j<ret.size();j++){
//            DEBUG_HEADER
//            std::cout<<j<<": ("<<ret[j].x<<", "<< ret[j].y<<")"<<std::endl;
//        }
        return ret;
    }
    if( fabs(p1->m_mQuad(0,0))<RS_TOLERANCE && fabs(p1->m_mQuad(0,1))<RS_TOLERANCE
            &&
            fabs(p2->m_mQuad(0,0))<RS_TOLERANCE && fabs(p2->m_mQuad(0,1))<RS_TOLERANCE
            ){
        if(fabs(p1->m_mQuad(1,1))<RS_TOLERANCE && fabs(p2->m_mQuad(1,1))<RS_TOLERANCE){
            //linear
            std::vector<double> ce(0);
            ce.push_back(p1->m_vLinear(0));
            ce.push_back(p1->m_vLinear(1));
            ce.push_back(p1->m_dConst);
            LC_Quadratic lc10(ce);
            ce.clear();
            ce.push_back(p2->m_vLinear(0));
            ce.push_back(p2->m_vLinear(1));
            ce.push_back(p2->m_dConst);
            LC_Quadratic lc11(ce);
            return getIntersection(lc10,lc11);
        }
        return getIntersection(p1->flipXY(),p2->flipXY()).flipXY();
    }
    std::vector<std::vector<double> >  ce(0);
    ce.push_back(p1->getCoefficients());
    ce.push_back(p2->getCoefficients());
    if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
        DEBUG_HEADER
        std::cout<<*p1<<std::endl;
        std::cout<<*p2<<std::endl;
    }
	auto sol= RS_Math::simultaneousQuadraticSolverFull(ce);
    bool valid= sol.size()>0;
	for(auto & v: sol){
//.........这里部分代码省略.........

RS_VectorSolutions RS_Information::getIntersectionEllipseEllipse(
		RS_Ellipse const* e1, RS_Ellipse const* e2) {
    RS_VectorSolutions ret;

	if (!(e1 && e2) ) {
        return ret;
    }
    if (
        (e1->getCenter() - e2 ->getCenter()).squared() < RS_TOLERANCE2 &&
        (e1->getMajorP() - e2 ->getMajorP()).squared() < RS_TOLERANCE2 &&
        fabs(e1->getRatio() - e2 ->getRatio()) < RS_TOLERANCE
    ) { // overlapped ellipses, do not do overlap
        return ret;
    }
	RS_Ellipse ellipse01(nullptr,e1->getData());

    RS_Ellipse *e01= & ellipse01;
    if( e01->getMajorRadius() < e01->getMinorRadius() ) e01->switchMajorMinor();
	RS_Ellipse ellipse02(nullptr,e2->getData());
    RS_Ellipse *e02= &ellipse02;
    if( e02->getMajorRadius() < e02->getMinorRadius() ) e02->switchMajorMinor();
    //transform ellipse2 to ellipse1's coordinates
    RS_Vector shiftc1=- e01->getCenter();
    double shifta1=-e01->getAngle();
    e02->move(shiftc1);
    e02->rotate(shifta1);
//    RS_Vector majorP2=e02->getMajorP();
    double a1=e01->getMajorRadius();
    double b1=e01->getMinorRadius();
    double x2=e02->getCenter().x,
           y2=e02->getCenter().y;
    double a2=e02->getMajorRadius();
    double b2=e02->getMinorRadius();

    if( e01->getMinorRadius() < RS_TOLERANCE || e01 -> getRatio()< RS_TOLERANCE) {
        // treate e01 as a line
		RS_Line *l0=new RS_Line{e1->getParent(), {{-a1,0.}, {a1,0.}}};
        ret= getIntersectionEllipseLine(l0, e02);
        ret.rotate(-shifta1);
        ret.move(-shiftc1);
        return ret;
    }
    if( e02->getMinorRadius() < RS_TOLERANCE || e02 -> getRatio()< RS_TOLERANCE) {
        // treate e02 as a line
		RS_Line *l0=new RS_Line{e1->getParent(), {{-a2,0.}, {a2,0.}}};
		l0->rotate({0.,0.}, e02->getAngle());
        l0->move(e02->getCenter());
        ret= getIntersectionEllipseLine(l0, e01);
        ret.rotate(-shifta1);
        ret.move(-shiftc1);
        return ret;
    }

    //ellipse01 equation:
    //	x^2/(a1^2) + y^2/(b1^2) - 1 =0
    double t2= - e02->getAngle();
    //ellipse2 equation:
    // ( (x - u) cos(t) - (y - v) sin(t))^2/a^2 + ( (x - u) sin(t) + (y-v) cos(t))^2/b^2 =1
    // ( cos^2/a^2 + sin^2/b^2) x^2 +
    // ( sin^2/a^2 + cos^2/b^2) y^2 +
    //  2 sin cos (1/b^2 - 1/a^2) x y +
    //  ( ( 2 v sin cos - 2 u cos^2)/a^2 - ( 2v sin cos + 2 u sin^2)/b^2) x +
    //  ( ( 2 u sin cos - 2 v sin^2)/a^2 - ( 2u sin cos + 2 v cos^2)/b^2) y +
    //  (u cos - v sin)^2/a^2 + (u sin + v cos)^2/b^2 -1 =0
    // detect whether any ellipse radius is zero
    double cs=cos(t2),si=sin(t2);
    double ucs=x2*cs,usi=x2*si,
           vcs=y2*cs,vsi=y2*si;
    double cs2=cs*cs,si2=1-cs2;
    double tcssi=2.*cs*si;
    double ia2=1./(a2*a2),ib2=1./(b2*b2);
    std::vector<double> m(0,0.);
    m.push_back( 1./(a1*a1)); //ma000
    m.push_back( 1./(b1*b1)); //ma011
    m.push_back(cs2*ia2 + si2*ib2); //ma100
    m.push_back(cs*si*(ib2 - ia2)); //ma101
    m.push_back(si2*ia2 + cs2*ib2); //ma111
    m.push_back(( y2*tcssi - 2.*x2*cs2)*ia2 - ( y2*tcssi+2*x2*si2)*ib2); //mb10
    m.push_back( ( x2*tcssi - 2.*y2*si2)*ia2 - ( x2*tcssi+2*y2*cs2)*ib2); //mb11
    m.push_back((ucs - vsi)*(ucs-vsi)*ia2+(usi+vcs)*(usi+vcs)*ib2 -1.); //mc1
	auto vs0=RS_Math::simultaneousQuadraticSolver(m);
    shifta1 = - shifta1;
    shiftc1 = - shiftc1;
	for(RS_Vector vp: vs0){
        vp.rotate(shifta1);
        vp.move(shiftc1);
        ret.push_back(vp);
    }
    return ret;
}