Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Replaced own solvers with Eigen polynomial solvers #59

Merged
merged 1 commit into from
May 25, 2016
Merged

Replaced own solvers with Eigen polynomial solvers #59

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions lckernel/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,9 @@ pkg_check_modules(LOG4CXX REQUIRED liblog4cxx)
include_directories(${LOG4CXX_INCLUDE_DIRS})
link_directories(${LOG4CXX_LIBRARY_DIRS})

pkg_check_modules(EIGEN REQUIRED eigen3)
include_directories(${EIGEN_INCLUDE_DIRS})

# BUILDING CONFIG
# SEPERATE BUILDING FLAG
set(SEPARATE_BUILD OFF)
Expand Down
268 changes: 43 additions & 225 deletions lckernel/cad/math/lcmath.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
#include "lcmath.h"
#include "cad/const.h"
#include <eigen3/Eigen/Dense>
#include <unsupported/Eigen/Polynomials>

using namespace lc;
using namespace geo;

Expand Down Expand Up @@ -63,111 +65,53 @@ double Math::getAngleDifference(double start, double end, bool CCW) {
}


std::vector<double> Math::quadraticSolver(const std::vector<double>& ce)
//quadratic solver for
// x^2 + ce[0] x + ce[1] =0
{
/** quadratic solver
* x^2 + ce[0] x + ce[1] = 0
@ce, a vector of size 2 contains the coefficient in order
@return, a vector contains real roots
**/
std::vector<double> Math::quadraticSolver(const std::vector<double>& ce) {
std::vector<double> ans(0, 0.);

if (ce.size() != 2) {
return ans;
}

double b = 0.25 * ce[0] * ce[0];
double discriminant = b - ce[1];
Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
Eigen::VectorXd coeff(3);

if (discriminant >= - LCTOLERANCE * std::max(std::abs(b), std::abs(ce[1]))) {
b = sqrt(std::abs(discriminant));
double a = -0.5 * ce[0];
coeff[0] = ce[1];
coeff[1] = ce[0];
coeff[2] = 1;

if (b >= LCTOLERANCE * std::abs(a)) {
ans.push_back(a + b);
ans.push_back(a - b);
} else {
ans.push_back(a);
}
}
solver.compute(coeff);

solver.realRoots(ans);
return ans;
}

std::vector<double> Math::cubicSolver(const std::vector<double>& ce)
//cubic equation solver
// x^3 + ce[0] x^2 + ce[1] x + ce[2] = 0
{
// std::cout<<"x^3 + ("<<ce[0]<<")*x^2+("<<ce[1]<<")*x+("<<ce[2]<<")==0"<<std::endl;
/** cubic solver
* x^3 + ce[0] x^2 + ce[1] x + ce[2] = 0
@ce, a vector of size 3 contains the coefficient in order
@return, a vector contains real roots
**/
std::vector<double> Math::cubicSolver(const std::vector<double>& ce) {
std::vector<double> ans(0, 0.);

if (ce.size() != 3) {
return ans;
}

// depressed cubic, Tschirnhaus transformation, x= t - b/(3a)
// t^3 + p t +q =0
double shift = (1. / 3) * ce[0];
double p = ce[1] - shift * ce[0];
double q = ce[0] * ((2. / 27) * ce[0] * ce[0] - (1. / 3) * ce[1]) + ce[2];
//Cardano's method,
// t=u+v
// u^3 + v^3 + ( 3 uv + p ) (u+v) + q =0
// select 3uv + p =0, then,
// u^3 + v^3 = -q
// u^3 v^3 = - p^3/27
// so, u^3 and v^3 are roots of equation,
// z^2 + q z - p^3/27 = 0
// and u^3,v^3 are,
// -q/2 \pm sqrt(q^2/4 + p^3/27)
// discriminant= q^2/4 + p^3/27
//std::cout<<"p="<<p<<"\tq="<<q<<std::endl;
double discriminant = (1. / 27) * p * p * p + (1. / 4) * q * q;

if (std::abs(p) < 1.0e-75) {
ans.push_back((q > 0) ? -pow(q, (1. / 3)) : pow(-q, (1. / 3)));
ans[0] -= shift;
// DEBUG_HEADER();
// std::cout<<"cubic: one root: "<<ans[0]<<std::endl;
return ans;
}

//std::cout<<"discriminant="<<discriminant<<std::endl;
if (discriminant > 0) {
std::vector<double> ce2(2, 0.);
ce2[0] = q;
ce2[1] = -1. / 27 * p * p * p;
auto&& r = quadraticSolver(ce2);
Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
Eigen::VectorXd coeff(4);

if (r.size() == 0) { //should not happen
std::cerr << __FILE__ << " : " << __FUNCTION__ << " : line " << __LINE__ << " :cubicSolver()::Error cubicSolver(" << ce[0] << ' ' << ce[1] << ' ' << ce[2] << ")\n";
}

double u, v;
u = (q <= 0) ? pow(r[0], 1. / 3) : -pow(-r[1], 1. / 3);
//u=(q<=0)?pow(-0.5*q+sqrt(discriminant),1./3):-pow(0.5*q+sqrt(discriminant),1./3);
v = (-1. / 3) * p / u;
//std::cout<<"u="<<u<<"\tv="<<v<<std::endl;
//std::cout<<"u^3="<<u*u*u<<"\tv^3="<<v*v*v<<std::endl;
ans.push_back(u + v - shift);

// DEBUG_HEADER();
// std::cout<<"cubic: one root: "<<ans[0]<<std::endl;
return ans;
}
coeff[0] = ce[2];
coeff[1] = ce[1];
coeff[2] = ce[0];
coeff[3] = 1;

std::complex<double> u(q, 0), rt[3];
u = std::pow(-0.5 * u - sqrt(0.25 * u * u + p * p * p / 27), 1. / 3);
rt[0] = u - p / (3.*u) - shift;
std::complex<double> w(-0.5, sqrt(3.) / 2);
rt[1] = u * w - p / (3.*u * w) - shift;
rt[2] = u / w - p * w / (3.*u) - shift;
// DEBUG_HEADER();
// std::cout<<"Roots:\n";
// std::cout<<rt[0]<<std::endl;
// std::cout<<rt[1]<<std::endl;
// std::cout<<rt[2]<<std::endl;
ans.push_back(rt[0].real());
ans.push_back(rt[1].real());
ans.push_back(rt[2].real());
solver.compute(coeff);

solver.realRoots(ans);
return ans;
}

Expand All @@ -177,151 +121,27 @@ std::vector<double> Math::cubicSolver(const std::vector<double>& ce)
@return, a vector contains real roots
**/
std::vector<double> Math::quarticSolver(const std::vector<double>& ce) {
// std::cout<<"x^4+("<<ce[0]<<")*x^3+("<<ce[1]<<")*x^2+("<<ce[2]<<")*x+("<<ce[3]<<")==0"<<std::endl;

std::vector<double> ans(0, 0.);
Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
Eigen::VectorXd coeff(5);

// if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
// DEBUG_HEADER();
// std::cout<<"expected array size=4, got "<<ce.size()<<std::endl;
// }
if (ce.size() != 4) {
return ans;
}

// if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
// std::cout<<"x^4+("<<ce[0]<<")*x^3+("<<ce[1]<<")*x^2+("<<ce[2]<<")*x+("<<ce[3]<<")==0"<<std::endl;
// }

// x^4 + a x^3 + b x^2 +c x + d = 0
// depressed quartic, x= t - a/4
// t^4 + ( b - 3/8 a^2 ) t^2 + (c - a b/2 + a^3/8) t + d - a c /4 + a^2 b/16 - 3 a^4/256 =0
// t^4 + p t^2 + q t + r =0
// p= b - (3./8)*a*a;
// q= c - 0.5*a*b+(1./8)*a*a*a;
// r= d - 0.25*a*c+(1./16)*a*a*b-(3./256)*a^4
double shift = 0.25 * ce[0];
double shift2 = shift * shift;
double a2 = ce[0] * ce[0];
double p = ce[1] - (3. / 8) * a2;
double q = ce[2] + ce[0] * ((1. / 8) * a2 - 0.5 * ce[1]);
double r = ce[3] - shift * ce[2] + (ce[1] - 3.*shift2) * shift2;

// if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
// DEBUG_HEADER();
// std::cout<<"x^4+("<<p<<")*x^2+("<<q<<")*x+("<<r<<")==0"<<std::endl;
// }
if (q * q <= 1.e-4 * LCTOLERANCE * std::abs(p * r)) { // Biquadratic equations
double discriminant = 0.25 * p * p - r;

if (discriminant < -1.e3 * LCTOLERANCE) {

// DEBUG_HEADER();
// std::cout<<"discriminant="<<discriminant<<"\tno root"<<std::endl;
return ans;
}

double t2[2];
t2[0] = -0.5 * p - sqrt(std::abs(discriminant));
t2[1] = -p - t2[0];

// std::cout<<"t2[0]="<<t2[0]<<std::endl;
// std::cout<<"t2[1]="<<t2[1]<<std::endl;
if (t2[1] >= 0.) { // two real roots
ans.push_back(sqrt(t2[1]) - shift);
ans.push_back(-sqrt(t2[1]) - shift);
}

if (t2[0] >= 0.) { // four real roots
ans.push_back(sqrt(t2[0]) - shift);
ans.push_back(-sqrt(t2[0]) - shift);
}

// DEBUG_HEADER();
// for(int i=0;i<ans.size();i++){
// std::cout<<"root x: "<<ans[i]<<std::endl;
// }
return ans;
}

if (std::abs(r) < 1.0e-75) {
std::vector<double> cubic(3, 0.);
cubic[1] = p;
cubic[2] = q;
ans.push_back(0.);
auto&& r = cubicSolver(cubic);
std::copy(r.begin(), r.end(), back_inserter(ans));

for (size_t i = 0; i < ans.size(); i++) {
ans[i] -= shift;
}

return ans;
}

// depressed quartic to two quadratic equations
// t^4 + p t^2 + q t + r = ( t^2 + u t + v) ( t^2 - u t + w)
// so,
// p + u^2= w+v
// q/u= w-v
// r= wv
// so,
// (p+u^2)^2 - (q/u)^2 = 4 r
// y=u^2,
// y^3 + 2 p y^2 + ( p^2 - 4 r) y - q^2 =0
//
std::vector<double> cubic(3, 0.);
cubic[0] = 2.*p;
cubic[1] = p * p - 4.*r;
cubic[2] = -q * q;
auto&& r3 = cubicSolver(cubic);

//std::cout<<"quartic_solver:: real roots from cubic: "<<ret<<std::endl;
//for(unsigned int i=0; i<ret; i++)
// std::cout<<"cubic["<<i<<"]="<<cubic[i]<<" x= "<<croots[i]<<std::endl;
if (r3.size() == 1) { //one real root from cubic
if (r3[0] < 0.) { //this should not happen
std::cerr << __FILE__ << " : " << __FUNCTION__ << " : line " << __LINE__ << std::endl;
std::cerr << "Quartic Error:: Found one real root for cubic, but negative\n";
return ans;
}

double sqrtz0 = sqrt(r3[0]);
std::vector<double> ce2(2, 0.);
ce2[0] = -sqrtz0;
ce2[1] = 0.5 * (p + r3[0]) + 0.5 * q / sqrtz0;
auto r1 = quadraticSolver(ce2);

if (r1.size() == 0) {
ce2[0] = sqrtz0;
ce2[1] = 0.5 * (p + r3[0]) - 0.5 * q / sqrtz0;
r1 = quadraticSolver(ce2);
}

for (size_t i = 0; i < r1.size(); i++) {
r1[i] -= shift;
}

return r1;
}

if (r3[0] > 0. && r3[1] > 0.) {
double sqrtz0 = sqrt(r3[0]);
std::vector<double> ce2(2, 0.);
ce2[0] = -sqrtz0;
ce2[1] = 0.5 * (p + r3[0]) + 0.5 * q / sqrtz0;
ans = quadraticSolver(ce2);
ce2[0] = sqrtz0;
ce2[1] = 0.5 * (p + r3[0]) - 0.5 * q / sqrtz0;
auto&& r1 = quadraticSolver(ce2);
std::copy(r1.begin(), r1.end(), back_inserter(ans));

for (size_t i = 0; i < ans.size(); i++) {
ans[i] -= shift;
}
coeff[0] = ce[3];
coeff[1] = ce[2];
coeff[2] = ce[1];
coeff[3] = ce[0];
coeff[4] = 1;

return ans;
}
solver.compute(coeff);

solver.realRoots(ans);
return ans;

}

/** quartic solver
Expand All @@ -331,10 +151,7 @@ std::vector<double> Math::quarticSolver(const std::vector<double>& ce) {
*ToDo, need a robust algorithm to locate zero terms, better handling of tolerances
**/
std::vector<double> Math::quarticSolverFull(const std::vector<double>& ce) {
// if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
// DEBUG_HEADER();
// std::cout<<ce[4]<<"*y^4+("<<ce[3]<<")*y^3+("<<ce[2]<<"*y^2+("<<ce[1]<<")*y+("<<ce[0]<<")==0"<<std::endl;
// }
// std::cout<<ce[4]<<"*y^4+("<<ce[3]<<")*y^3+("<<ce[2]<<"*y^2+("<<ce[1]<<")*y+("<<ce[0]<<")==0"<<std::endl;

std::vector<double> roots(0, 0.);

Expand Down Expand Up @@ -520,6 +337,7 @@ std::vector<lc::geo::Coordinate> Math::simultaneousQuadraticSolverFull(const std
// }
//quarticSolver
auto&& roots = quarticSolverFull(qy);

// if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
// std::cout<<"roots.size()= "<<roots.size()<<std::endl;
// }
Expand Down
3 changes: 3 additions & 0 deletions unittest/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,9 @@ pkg_check_modules(LOG4CXX REQUIRED liblog4cxx)
include_directories(${LOG4CXX_INCLUDE_DIRS})
link_directories(${LOG4CXX_LIBRARY_DIRS})

pkg_check_modules(EIGEN REQUIRED eigen3)
include_directories(${EIGEN_INCLUDE_DIRS})

FIND_PACKAGE ( Threads REQUIRED )

set(src
Expand Down
2 changes: 2 additions & 0 deletions unittest/code.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@
#include "cad/math/intersectionhandler.h"
#include "code.h"
#include <cad/math/lcmath.h>
#include <unsupported/Eigen/Polynomials>

using namespace lc;
using namespace entity;

Expand Down
32 changes: 0 additions & 32 deletions unittest/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,38 +18,6 @@ int main(int argc, char **argv) {
return RUN_ALL_TESTS();
}

TEST(test, quad) {
test c;


EXPECT_DOUBLE_EQ(-2, c.quad(5, 6)[0]);
EXPECT_DOUBLE_EQ(-3, c.quad(5, 6)[1]);

EXPECT_TRUE(fabs(c.quad(100, 50)[0]) - 0.50252531694167146 < 0.000000001);
EXPECT_DOUBLE_EQ(-99.497474683058329, c.quad(100, 50)[1]);

// EXPECT_DOUBLE_EQ(-2, c.quad(5, 6)[0]);
// EXPECT_DOUBLE_EQ(-3, c.quad(5, 6)[1]);

// EXPECT_DOUBLE_EQ(-2, c.quad(5, 6)[0]);
// EXPECT_DOUBLE_EQ(-3, c.quad(5, 6)[1]);
}

TEST(test, cubic) {
test c;

EXPECT_TRUE(fabs(c.cubic(5, 6, 1)[0]) - 0.19806226419516171 < .00000001);
EXPECT_DOUBLE_EQ(-1.5549581320873713, c.cubic(5, 6, 1)[1]);
EXPECT_DOUBLE_EQ(-3.2469796037174667, c.cubic(5, 6, 1)[2]);
}

TEST(test, quartic) {
test c;

EXPECT_DOUBLE_EQ(-2.0, c.quartic(5, 6, 1, 2)[0]);
EXPECT_DOUBLE_EQ(-3.1038034027355366, c.quartic(5, 6, 1, 2)[1]);
}

TEST(test, testin) {
test a;
const double rootY = 200. / sqrt(100. * 100. + 1.);
Expand Down
Loading