LC_Quadratic::LC_Quadratic(const LC_Quadratic& lc0): m_bIsQuadratic(lc0.isQuadratic()) ,m_bValid(lc0.isValid()) { if(m_bValid==false) return; if(m_bIsQuadratic) m_mQuad=lc0.getQuad(); m_vLinear=lc0.getLinear(); m_dConst=lc0.m_dConst; }
RS_VectorSolutions LC_Quadratic::getIntersection(const LC_Quadratic& l1, const LC_Quadratic& l2) { RS_VectorSolutions ret; if( l1.isValid() && l2.isValid() == false ) return ret; auto p1=&l1; auto p2=&l2; if(p1->isQuadratic()==false){ std::swap(p1,p2); } if(p1->isQuadratic()==false){ //two lines QVector<QVector<double> > ce(2,QVector<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; QVector<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))<fabs(p2->m_vLinear(1))){ return getIntersection(p1->flipXY(),p2->flipXY()).flipXY(); } } std::vector<std::vector<double> > ce(0); ce.push_back(p1->getCoefficients()); ce.push_back(p2->getCoefficients()); //DEBUG_HEADER(); //std::cout<<*p1<<std::endl; //std::cout<<*p2<<std::endl; return RS_Math::simultaneousQuadraticSolverFull(ce); }
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){ if(v.magnitude()>=RS_MAXDOUBLE){ valid=false; break; } } if(valid) return sol; ce.clear(); ce.push_back(p1->flipXY().getCoefficients()); ce.push_back(p2->flipXY().getCoefficients()); sol=RS_Math::simultaneousQuadraticSolverFull(ce); ret.clear(); for(auto const& v: sol){ if(v.magnitude()<=RS_MAXDOUBLE){ ret.push_back(v); if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){ DEBUG_HEADER std::cout<<v<<std::endl; } } } return ret; }
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); } // DEBUG_HEADER(); // std::cout<<*p1<<std::endl; // std::cout<<*p2<<std::endl; if(p1->isQuadratic()==false){ //two lines QVector<QVector<double> > ce(2,QVector<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; QVector<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))<fabs(p2->m_vLinear(1))){ return getIntersection(p1->flipXY(),p2->flipXY()).flipXY(); } } 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; } return RS_Math::simultaneousQuadraticSolverFull(ce); }