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);
}
