From 04bf63d0dec4cdb9f527a81d37d17ce156f2f656 Mon Sep 17 00:00:00 2001 From: Paul Romano Date: Fri, 17 Dec 2021 17:00:12 -0500 Subject: [PATCH] Run clang-format --- include/openmc/surface.h | 12 +- src/surface.cpp | 732 +++++++++++++++++++++++++-------------- 2 files changed, 486 insertions(+), 258 deletions(-) diff --git a/include/openmc/surface.h b/include/openmc/surface.h index f4d093c8bb..c1aaf2cc15 100644 --- a/include/openmc/surface.h +++ b/include/openmc/surface.h @@ -377,10 +377,10 @@ public: //! \f$(x-x_0)^2 + (\sqrt{(y-y_0)^2 + (z-z_0)^2}-A)^2/C^2 -1 \f$ //============================================================================== -class SurfaceXTorus : public CSGSurface -{ +class SurfaceXTorus : public CSGSurface { // (x-x0)^2/B^2 + (((y-y0)^2 + (z-z0)^2)^-1/2 -A))^2/C^2 -1 = 0 double x0_, y0_, z0_, A_, B_, C_; + public: explicit SurfaceXTorus(pugi::xml_node surf_node); double evaluate(Position r) const; @@ -395,10 +395,10 @@ public: //! \f$(x-x_0)^2 + (\sqrt{(y-y_0)^2 + (z-z_0)^2}-A)^2/C^2 -1 \f$ //============================================================================== -class SurfaceYTorus : public CSGSurface -{ +class SurfaceYTorus : public CSGSurface { // (y-y0)^2/B^2 + (((x-x0)^2 + (z-z0)^2)^-1/2 -A))^2/C^2 -1 = 0 double x0_, y0_, z0_, A_, B_, C_; + public: explicit SurfaceYTorus(pugi::xml_node surf_node); double evaluate(Position r) const; @@ -413,10 +413,10 @@ public: //! \f$(x-x_0)^2 + (\sqrt{(y-y_0)^2 + (z-z_0)^2}-A)^2/C^2 -1 \f$ //============================================================================== -class SurfaceZTorus : public CSGSurface -{ +class SurfaceZTorus : public CSGSurface { // (z-z0)^2/B^2 + (((x-x0)^2 + (y-y0)^2)^-1/2 -A))^2/C^2 -1 = 0 double x0_, y0_, z0_, A_, B_, C_; + public: explicit SurfaceZTorus(pugi::xml_node surf_node); double evaluate(Position r) const; diff --git a/src/surface.cpp b/src/surface.cpp index 273349ec0e..20b2e6ddb3 100644 --- a/src/surface.cpp +++ b/src/surface.cpp @@ -85,10 +85,10 @@ void read_coeffs(pugi::xml_node surf_node, int surf_id, double& c1, double& c2, } } -void read_coeffs(pugi::xml_node surf_node, int surf_id, double &c1, double &c2, - double &c3, double &c4, double &c5, double &c6) +void read_coeffs(pugi::xml_node surf_node, int surf_id, double& c1, double& c2, + double& c3, double& c4, double& c5, double& c6) { - // Check the given number of coefficients. + // Check the given number of coefficients. std::string coeffs = get_node_value(surf_node, "coeffs"); int n_words = word_count(coeffs); if (n_words != 6) { @@ -98,15 +98,14 @@ void read_coeffs(pugi::xml_node surf_node, int surf_id, double &c1, double &c2, fatal_error(err_msg); } - // Parse the coefficients. - int stat = sscanf(coeffs.c_str(), "%lf %lf %lf %lf %lf %lf", &c1, &c2, &c3, &c4, &c5, &\ -c6); + // Parse the coefficients. + int stat = sscanf( + coeffs.c_str(), "%lf %lf %lf %lf %lf %lf", &c1, &c2, &c3, &c4, &c5, &c6); if (stat != 6) { fatal_error("Something went wrong reading surface coeffs"); } } - void read_coeffs(pugi::xml_node surf_node, int surf_id, double& c1, double& c2, double& c3, double& c4, double& c5, double& c6, double& c7, double& c8, double& c9, double& c10) @@ -1010,24 +1009,25 @@ void SurfaceQuadric::to_hdf5_inner(hid_t group_id) const // Generic functions for quadratic, cubic & quartic solver //============================================================================== -int quadratic_solve(double a, double b, double c, std::array &x) { +int quadratic_solve(double a, double b, double c, std::array& x) +{ - double func = std::pow(b,2) - 4*a*c; - - if ( func < 0 ) { + double func = std::pow(b, 2) - 4 * a * c; + + if (func < 0) { // this would be imaginary } else { - x[0] = -b/(2.*a) - std::sqrt(func)/(2.*a); - x[1] = -b/(2.*a) + std::sqrt(func)/(2.*a); + x[0] = -b / (2. * a) - std::sqrt(func) / (2. * a); + x[1] = -b / (2. * a) + std::sqrt(func) / (2. * a); } return 0; } -const double M_2PI = 2*PI; -const double eps=FP_COINCIDENT; +const double M_2PI = 2 * PI; +const double eps = FP_COINCIDENT; -//typedef std::complex DComplex; +// typedef std::complex DComplex; //--------------------------------------------------------------------------- // solve cubic equation x^3 + a*x^2 + b*x + c @@ -1035,143 +1035,141 @@ const double eps=FP_COINCIDENT; // In case 3 real roots: => x[0], x[1], x[2], return 3 // 2 real roots: x[0], x[1], return 2 // 1 real root : x[0], x[1] ± i*x[2], return 1 -unsigned int solve_cubic(const double a, const double b, const double c, std::array &x) +unsigned int solve_cubic( + const double a, const double b, const double c, std::array& x) { - double a2 = a*a; - double q = (a2 - 3*b)/9; - double r = (a*(2*a2-9*b) + 27*c)/54; - double r2 = r*r; - double q3 = std::pow(q,3); - double A,B; + double a2 = a * a; + double q = (a2 - 3 * b) / 9; + double r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; + double r2 = r * r; + double q3 = std::pow(q, 3); + double A, B; double a_prime = 0.; - if( r2 < q3 ) // 3 roots + if (r2 < q3) // 3 roots { - double t=r/sqrt(q3); - if( t<-1) t=-1; - if( t> 1) t= 1; - t=std::acos(t); - a_prime = a/3.; - q=-2*std::sqrt(q); - x[0]=q*std::cos(t/3)-a_prime; - x[1]=q*std::cos((t+M_2PI)/3)-a_prime; - x[2]=q*std::cos((t-M_2PI)/3)-a_prime; - return 3; - } - else - { - A =-std::pow(std::fabs(r)+std::sqrt(r2-q3),1./3); - if( r<0 ) A=-A; - B = (0 == A ? 0 : q/A); - - a_prime = a/3; - x[0] =(A+B)-a_prime; - x[1] =-0.5*(A+B)-a_prime; - x[2] = 0.5*std::sqrt(3.)*(A-B); + double t = r / sqrt(q3); + if (t < -1) + t = -1; + if (t > 1) + t = 1; + t = std::acos(t); + a_prime = a / 3.; + q = -2 * std::sqrt(q); + x[0] = q * std::cos(t / 3) - a_prime; + x[1] = q * std::cos((t + M_2PI) / 3) - a_prime; + x[2] = q * std::cos((t - M_2PI) / 3) - a_prime; + return 3; + } else { + A = -std::pow(std::fabs(r) + std::sqrt(r2 - q3), 1. / 3); + if (r < 0) + A = -A; + B = (0 == A ? 0 : q / A); + + a_prime = a / 3; + x[0] = (A + B) - a_prime; + x[1] = -0.5 * (A + B) - a_prime; + x[2] = 0.5 * std::sqrt(3.) * (A - B); // 2 real roots - if(std::fabs(x[2]) &real_roots) { - double a3 = -b; - double b3 = a*c -4.*d; - double c3 = -a*a*d - c*c + 4.*b*d; +// Attention - this function returns dynamically allocated array. It has to be +// released afterwards. +void quartic_solve( + double a, double b, double c, double d, std::array& real_roots) +{ + double a3 = -b; + double b3 = a * c - 4. * d; + double c3 = -a * a * d - c * c + 4. * b * d; // cubic resolvent // y^3 − b*y^2 + (ac−4d)*y − a^2*d−c^2+4*b*d = 0 - std::array cube_roots; + std::array cube_roots; unsigned int num_roots = solve_cubic(a3, b3, c3, cube_roots); - + double q1, q2, p1, p2, D, sqD, y; - + y = cube_roots[0]; // The essence - choosing Y with maximal absolute value. - if(num_roots != 1) - { - if(std::fabs(cube_roots[1]) > std::fabs(y)) y = cube_roots[1]; - if(std::fabs(cube_roots[2]) > std::fabs(y)) y = cube_roots[2]; + if (num_roots != 1) { + if (std::fabs(cube_roots[1]) > std::fabs(y)) + y = cube_roots[1]; + if (std::fabs(cube_roots[2]) > std::fabs(y)) + y = cube_roots[2]; } - + // h1+h2 = y && h1*h2 = d <=> h^2 -y*h + d = 0 (h === q) - - D = y*y - 4*d; - if(std::fabs(D) < eps) //in other words - D==0 + + D = y * y - 4 * d; + if (std::fabs(D) < eps) // in other words - D==0 { - q1 = q2 = y * 0.5; - // g1+g2 = a && g1+g2 = b-y <=> g^2 - a*g + b-y = 0 (p === g) - D = a*a - 4*(b-y); - if(std::fabs(D) < eps) { - p1 = p2 = a * 0.5; - } - else - { - sqD = std::sqrt(D); - p1 = (a + sqD) * 0.5; - p2 = (a - sqD) * 0.5; - } + q1 = q2 = y * 0.5; + // g1+g2 = a && g1+g2 = b-y <=> g^2 - a*g + b-y = 0 (p === g) + D = a * a - 4 * (b - y); + if (std::fabs(D) < eps) { + p1 = p2 = a * 0.5; + } else { + sqD = std::sqrt(D); + p1 = (a + sqD) * 0.5; + p2 = (a - sqD) * 0.5; + } + } else { + sqD = std::sqrt(D); + q1 = (y + sqD) * 0.5; + q2 = (y - sqD) * 0.5; + p1 = (a * q1 - c) / (q1 - q2); + p2 = (c - a * q2) / (q1 - q2); } - else - { - sqD = std::sqrt(D); - q1 = (y + sqD) * 0.5; - q2 = (y - sqD) * 0.5; - p1 = (a*q1-c)/(q1-q2); - p2 = (c-a*q2)/(q1-q2); - } - - std::array,4> roots; //the roots to return - + + std::array, 4> roots; // the roots to return + // solving quadratic eq. - x^2 + p1*x + q1 = 0 - D = p1*p1 - 4*q1; - if(D < 0.0) - { - roots[0].real( -p1 * 0.5 ); - roots[0].imag( std::sqrt(-D) * 0.5 ); + D = p1 * p1 - 4 * q1; + if (D < 0.0) { + roots[0].real(-p1 * 0.5); + roots[0].imag(std::sqrt(-D) * 0.5); roots[1] = std::conj(roots[0]); - } - else - { + } else { sqD = std::sqrt(D); - roots[0].real( (-p1 + sqD) * 0.5 ); - roots[1].real( (-p1 - sqD) * 0.5 ); + roots[0].real((-p1 + sqD) * 0.5); + roots[1].real((-p1 - sqD) * 0.5); } - + // solving quadratic eq. - x^2 + p2*x + q2 = 0 - D = p2*p2 - 4*q2; - if(D < 0.0) - { - roots[2].real( -p2 * 0.5 ); - roots[2].imag( std::sqrt(-D) * 0.5 ); + D = p2 * p2 - 4 * q2; + if (D < 0.0) { + roots[2].real(-p2 * 0.5); + roots[2].imag(std::sqrt(-D) * 0.5); roots[3] = std::conj(roots[2]); - } - else - { + } else { sqD = std::sqrt(D); - roots[2].real( (-p2 + sqD) * 0.5 ); - roots[3].real( (-p2 - sqD) * 0.5 ); + roots[2].real((-p2 + sqD) * 0.5); + roots[3].real((-p2 - sqD) * 0.5); } - - for ( int i = 0 ; i < 4 ; i++ ) { - if (roots[i].imag() == 0. ) + + for (int i = 0; i < 4; i++) { + if (roots[i].imag() == 0.) real_roots[i] = roots[i].real(); else real_roots[i] = 0.; } - std::sort(real_roots.begin(),real_roots.end()); + std::sort(real_roots.begin(), real_roots.end()); return; } //============================================================================== -SurfaceXTorus::SurfaceXTorus(pugi::xml_node surf_node) - : CSGSurface(surf_node) +SurfaceXTorus::SurfaceXTorus(pugi::xml_node surf_node) : CSGSurface(surf_node) { read_coeffs(surf_node, id_, x0_, y0_, z0_, A_, B_, C_); } @@ -1179,31 +1177,30 @@ SurfaceXTorus::SurfaceXTorus(pugi::xml_node surf_node) void SurfaceXTorus::to_hdf5_inner(hid_t group_id) const { write_string(group_id, "type", "x-torus", false); - std::array coeffs {{x0_, y0_, z0_, A_, B_, C_ }}; + std::array coeffs {{x0_, y0_, z0_, A_, B_, C_}}; write_dataset(group_id, "coefficients", coeffs); } // todo should do some alebgra first to get rid of the sqrt -double -SurfaceXTorus::evaluate(Position r) const +double SurfaceXTorus::evaluate(Position r) const { const double x = r.x; const double y = r.y; const double z = r.z; - const double x_coeff2 = std::pow(x-x0_,2); - const double y_coeff2 = std::pow(y-y0_,2); - const double z_coeff2 = std::pow(z-z0_,2); + const double x_coeff2 = std::pow(x - x0_, 2); + const double y_coeff2 = std::pow(y - y0_, 2); + const double z_coeff2 = std::pow(z - z0_, 2); - const double B2 = B_*B_; - const double C2 = C_*C_; + const double B2 = B_ * B_; + const double C2 = C_ * C_; - return x_coeff2/B2 + std::pow(std::sqrt(y_coeff2 + z_coeff2)-A_,2)/C2 - 1.; + return x_coeff2 / B2 + std::pow(std::sqrt(y_coeff2 + z_coeff2) - A_, 2) / C2 - + 1.; } -double -SurfaceXTorus::distance(Position r, Direction ang, bool coincident) const +double SurfaceXTorus::distance(Position r, Direction ang, bool coincident) const { double u = ang.x; double v = ang.y; @@ -1220,74 +1217,119 @@ SurfaceXTorus::distance(Position r, Direction ang, bool coincident) const double x0 = x0_ + xr; double y0 = y0_ + yr; double z0 = z0_ + zr; - - double a,b,c,d,e; // coefficients of quartic + + double a, b, c, d, e; // coefficients of quartic // a is always 1 maybe save the maths? - a = std::pow(v,4) + 2*std::pow(v,2)*std::pow(w,2) + std::pow(w,4) + 2*std::pow(C,2)*std::pow(u,2)*std::pow(v,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(u,2)*std::pow(w,2)/std::pow(B,2) + std::pow(C,4)*std::pow(u,4)/std::pow(B,4); - b = 4*std::pow(v,3)*y0 + 4*std::pow(v,2)*w*z0 + 4*v*std::pow(w,2)*y0 + 4*std::pow(w,3)*z0 + 4*std::pow(C,2)*std::pow(u,2)*v*y0/std::pow(B,2) + 4*std::pow(C,2)*std::pow(u,2)*w*z0/std::pow(B,2) + 4*std::pow(C,2)*u*std::pow(v,2)*x0/std::pow(B,2) + 4*std::pow(C,2)*u*std::pow(w,2)*x0/std::pow(B,2) + 4*std::pow(C,4)*std::pow(u,3)*x0/std::pow(B,4); - c = -2*std::pow(A,2)*std::pow(v,2) - 2*std::pow(A,2)*std::pow(w,2) + 2*std::pow(A,2)*std::pow(C,2)*std::pow(u,2)/std::pow(B,2) - 2*std::pow(C,2)*std::pow(v,2) - 2*std::pow(C,2)*std::pow(w,2) + 6*std::pow(v,2)*std::pow(y0,2) + 2*std::pow(v,2)*std::pow(z0,2) + 8*v*w*y0*z0 + 2*std::pow(w,2)*std::pow(y0,2) + 6*std::pow(w,2)*std::pow(z0,2) - 2*std::pow(C,4)*std::pow(u,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(u,2)*std::pow(y0,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(u,2)*std::pow(z0,2)/std::pow(B,2) + 8*std::pow(C,2)*u*v*x0*y0/std::pow(B,2) + 8*std::pow(C,2)*u*w*x0*z0/std::pow(B,2) + 2*std::pow(C,2)*std::pow(v,2)*std::pow(x0,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(w,2)*std::pow(x0,2)/std::pow(B,2) + 6*std::pow(C,4)*std::pow(u,2)*std::pow(x0,2)/std::pow(B,4); - d = -4*std::pow(A,2)*v*y0 - 4*std::pow(A,2)*w*z0 + 4*std::pow(A,2)*std::pow(C,2)*u*x0/std::pow(B,2) - 4*std::pow(C,2)*v*y0 - 4*std::pow(C,2)*w*z0 + 4*v*std::pow(y0,3) + 4*v*y0*std::pow(z0,2) + 4*w*std::pow(y0,2)*z0 + 4*w*std::pow(z0,3) - 4*std::pow(C,4)*u*x0/std::pow(B,2) + 4*std::pow(C,2)*u*x0*std::pow(y0,2)/std::pow(B,2) + 4*std::pow(C,2)*u*x0*std::pow(z0,2)/std::pow(B,2) + 4*std::pow(C,2)*v*std::pow(x0,2)*y0/std::pow(B,2) + 4*std::pow(C,2)*w*std::pow(x0,2)*z0/std::pow(B,2) + 4*std::pow(C,4)*u*std::pow(x0,3)/std::pow(B,4); - e = std::pow(A,4) - 2*std::pow(A,2)*std::pow(C,2) - 2*std::pow(A,2)*std::pow(y0,2) - 2*std::pow(A,2)*std::pow(z0,2) + 2*std::pow(A,2)*std::pow(C,2)*std::pow(x0,2)/std::pow(B,2) + std::pow(C,4) - 2*std::pow(C,2)*std::pow(y0,2) - 2*std::pow(C,2)*std::pow(z0,2) + std::pow(y0,4) + 2*std::pow(y0,2)*std::pow(z0,2) + std::pow(z0,4) - 2*std::pow(C,4)*std::pow(x0,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(x0,2)*std::pow(y0,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(x0,2)*std::pow(z0,2)/std::pow(B,2) + std::pow(C,4)*std::pow(x0,4)/std::pow(B,4); + a = std::pow(v, 4) + 2 * std::pow(v, 2) * std::pow(w, 2) + std::pow(w, 4) + + 2 * std::pow(C, 2) * std::pow(u, 2) * std::pow(v, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(u, 2) * std::pow(w, 2) / std::pow(B, 2) + + std::pow(C, 4) * std::pow(u, 4) / std::pow(B, 4); + b = 4 * std::pow(v, 3) * y0 + 4 * std::pow(v, 2) * w * z0 + + 4 * v * std::pow(w, 2) * y0 + 4 * std::pow(w, 3) * z0 + + 4 * std::pow(C, 2) * std::pow(u, 2) * v * y0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * std::pow(u, 2) * w * z0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * u * std::pow(v, 2) * x0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * u * std::pow(w, 2) * x0 / std::pow(B, 2) + + 4 * std::pow(C, 4) * std::pow(u, 3) * x0 / std::pow(B, 4); + c = + -2 * std::pow(A, 2) * std::pow(v, 2) - 2 * std::pow(A, 2) * std::pow(w, 2) + + 2 * std::pow(A, 2) * std::pow(C, 2) * std::pow(u, 2) / std::pow(B, 2) - + 2 * std::pow(C, 2) * std::pow(v, 2) - 2 * std::pow(C, 2) * std::pow(w, 2) + + 6 * std::pow(v, 2) * std::pow(y0, 2) + + 2 * std::pow(v, 2) * std::pow(z0, 2) + 8 * v * w * y0 * z0 + + 2 * std::pow(w, 2) * std::pow(y0, 2) + + 6 * std::pow(w, 2) * std::pow(z0, 2) - + 2 * std::pow(C, 4) * std::pow(u, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(u, 2) * std::pow(y0, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(u, 2) * std::pow(z0, 2) / std::pow(B, 2) + + 8 * std::pow(C, 2) * u * v * x0 * y0 / std::pow(B, 2) + + 8 * std::pow(C, 2) * u * w * x0 * z0 / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(v, 2) * std::pow(x0, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(w, 2) * std::pow(x0, 2) / std::pow(B, 2) + + 6 * std::pow(C, 4) * std::pow(u, 2) * std::pow(x0, 2) / std::pow(B, 4); + d = -4 * std::pow(A, 2) * v * y0 - 4 * std::pow(A, 2) * w * z0 + + 4 * std::pow(A, 2) * std::pow(C, 2) * u * x0 / std::pow(B, 2) - + 4 * std::pow(C, 2) * v * y0 - 4 * std::pow(C, 2) * w * z0 + + 4 * v * std::pow(y0, 3) + 4 * v * y0 * std::pow(z0, 2) + + 4 * w * std::pow(y0, 2) * z0 + 4 * w * std::pow(z0, 3) - + 4 * std::pow(C, 4) * u * x0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * u * x0 * std::pow(y0, 2) / std::pow(B, 2) + + 4 * std::pow(C, 2) * u * x0 * std::pow(z0, 2) / std::pow(B, 2) + + 4 * std::pow(C, 2) * v * std::pow(x0, 2) * y0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * w * std::pow(x0, 2) * z0 / std::pow(B, 2) + + 4 * std::pow(C, 4) * u * std::pow(x0, 3) / std::pow(B, 4); + e = std::pow(A, 4) - 2 * std::pow(A, 2) * std::pow(C, 2) - + 2 * std::pow(A, 2) * std::pow(y0, 2) - + 2 * std::pow(A, 2) * std::pow(z0, 2) + + 2 * std::pow(A, 2) * std::pow(C, 2) * std::pow(x0, 2) / std::pow(B, 2) + + std::pow(C, 4) - 2 * std::pow(C, 2) * std::pow(y0, 2) - + 2 * std::pow(C, 2) * std::pow(z0, 2) + std::pow(y0, 4) + + 2 * std::pow(y0, 2) * std::pow(z0, 2) + std::pow(z0, 4) - + 2 * std::pow(C, 4) * std::pow(x0, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(x0, 2) * std::pow(y0, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(x0, 2) * std::pow(z0, 2) / std::pow(B, 2) + + std::pow(C, 4) * std::pow(x0, 4) / std::pow(B, 4); // - std::array roots; - quartic_solve(b,c,d,e,roots); - - if (coincident) r += ang*TINY_BIT; + std::array roots; + quartic_solve(b, c, d, e, roots); + + if (coincident) + r += ang * TINY_BIT; // special degerenate case two sets of repated // roots - if ( b == 0.0 && d == 0) { - if ( roots[1] - roots[0] < 1e-5 ) return INFTY; - if ( roots[3] - roots[2] < 1e-5 ) return INFTY; + if (b == 0.0 && d == 0) { + if (roots[1] - roots[0] < 1e-5) + return INFTY; + if (roots[3] - roots[2] < 1e-5) + return INFTY; } - for ( int i = 0 ; i < 4 ; i++ ) { + for (int i = 0; i < 4; i++) { // need something better than just raw tolerance // use fastQS to get back lost precision - if ( roots[i] > 1e-6) { + if (roots[i] > 1e-6) { return roots[i]; - } + } } // otherwise no hit return INFTY; } -Direction -SurfaceXTorus::normal(Position r) const +Direction SurfaceXTorus::normal(Position r) const { // reduce the expansion of the full form for torus double x = r.x - x0_; double y = r.y - y0_; double z = r.z - z0_; - double A2 = A_*A_; - double B2 = B_*B_; - double C2 = C_*C_; + double A2 = A_ * A_; + double B2 = B_ * B_; + double C2 = C_ * C_; // coefficients for the x, x^2, x^3 and x^4 term - // but since we differeniate - we get x4 -> 4x3 - double dx_4 = 4*x*x*x*C2*C2/(B2*B2); - double dx_2 = 4*x*C2/B2*(z*z + 2*y*y - 2*C2 + 2*A2); + // but since we differeniate - we get x4 -> 4x3 + double dx_4 = 4 * x * x * x * C2 * C2 / (B2 * B2); + double dx_2 = 4 * x * C2 / B2 * (z * z + 2 * y * y - 2 * C2 + 2 * A2); double dx = dx_4 + dx_2; // similarly to y - double dy_4 = 4*y*y*y; - double dy_2 = 4*y*(z*z + C2*x*x/B2 - A2 - C2); + double dy_4 = 4 * y * y * y; + double dy_2 = 4 * y * (z * z + C2 * x * x / B2 - A2 - C2); double dy = dy_2 + dy_4; // and z - double dz_4 = 4*z*z*z; - double dz_2 = 4*z*(C2*x*x/B2 + y*y - C2 - A2); + double dz_4 = 4 * z * z * z; + double dz_2 = 4 * z * (C2 * x * x / B2 + y * y - C2 - A2); double dz = dz_2 + dz_4; - double length = std::sqrt(dx*dx + dy*dy + dz*dz); + double length = std::sqrt(dx * dx + dy * dy + dz * dz); - return {dx/length,dy/length,dz/length}; + return {dx / length, dy / length, dz / length}; } -SurfaceYTorus::SurfaceYTorus(pugi::xml_node surf_node) - : CSGSurface(surf_node) +SurfaceYTorus::SurfaceYTorus(pugi::xml_node surf_node) : CSGSurface(surf_node) { read_coeffs(surf_node, id_, x0_, y0_, z0_, A_, B_, C_); } @@ -1295,29 +1337,28 @@ SurfaceYTorus::SurfaceYTorus(pugi::xml_node surf_node) void SurfaceYTorus::to_hdf5_inner(hid_t group_id) const { write_string(group_id, "type", "y-torus", false); - std::array coeffs {{x0_, y0_, z0_, A_, B_, C_ }}; + std::array coeffs {{x0_, y0_, z0_, A_, B_, C_}}; write_dataset(group_id, "coefficients", coeffs); } -double -SurfaceYTorus::evaluate(Position r) const +double SurfaceYTorus::evaluate(Position r) const { const double x = r.x; const double y = r.y; const double z = r.z; - const double x_coeff2 = std::pow(x-x0_,2); - const double y_coeff2 = std::pow(y-y0_,2); - const double z_coeff2 = std::pow(z-z0_,2); + const double x_coeff2 = std::pow(x - x0_, 2); + const double y_coeff2 = std::pow(y - y0_, 2); + const double z_coeff2 = std::pow(z - z0_, 2); - const double B2 = B_*B_; - const double C2 = C_*C_; + const double B2 = B_ * B_; + const double C2 = C_ * C_; - return y_coeff2/B2 + std::pow(std::sqrt(x_coeff2 + z_coeff2)-A_,2)/C2 - 1.; + return y_coeff2 / B2 + std::pow(std::sqrt(x_coeff2 + z_coeff2) - A_, 2) / C2 - + 1.; } -double -SurfaceYTorus::distance(Position r, Direction ang, bool coincident) const +double SurfaceYTorus::distance(Position r, Direction ang, bool coincident) const { double u = ang.x; double v = ang.y; @@ -1334,33 +1375,80 @@ SurfaceYTorus::distance(Position r, Direction ang, bool coincident) const double x0 = x0_ + xr; double y0 = y0_ + yr; double z0 = z0_ + zr; - - double a,b,c,d,e; // coefficients of quartic - a = std::pow(u,4) + 2*std::pow(u,2)*std::pow(w,2) + std::pow(w,4) + 2*std::pow(C,2)*std::pow(u,2)*std::pow(v,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(v,2)*std::pow(w,2)/std::pow(B,2) + std::pow(C,4)*std::pow(v,4)/std::pow(B,4); - b = 4*std::pow(u,3)*x0 + 4*std::pow(u,2)*w*z0 + 4*u*std::pow(w,2)*x0 + 4*std::pow(w,3)*z0 + 4*std::pow(C,2)*std::pow(u,2)*v*y0/std::pow(B,2) + 4*std::pow(C,2)*u*std::pow(v,2)*x0/std::pow(B,2) + 4*std::pow(C,2)*std::pow(v,2)*w*z0/std::pow(B,2) + 4*std::pow(C,2)*v*std::pow(w,2)*y0/std::pow(B,2) + 4*std::pow(C,4)*std::pow(v,3)*y0/std::pow(B,4); - c = -2*std::pow(A,2)*std::pow(u,2) - 2*std::pow(A,2)*std::pow(w,2) + 2*std::pow(A,2)*std::pow(C,2)*std::pow(v,2)/std::pow(B,2) - 2*std::pow(C,2)*std::pow(u,2) - 2*std::pow(C,2)*std::pow(w,2) + 6*std::pow(u,2)*std::pow(x0,2) + 2*std::pow(u,2)*std::pow(z0,2) + 8*u*w*x0*z0 + 2*std::pow(w,2)*std::pow(x0,2) + 6*std::pow(w,2)*std::pow(z0,2) - 2*std::pow(C,4)*std::pow(v,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(u,2)*std::pow(y0,2)/std::pow(B,2) + 8*std::pow(C,2)*u*v*x0*y0/std::pow(B,2) + 2*std::pow(C,2)*std::pow(v,2)*std::pow(x0,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(v,2)*std::pow(z0,2)/std::pow(B,2) + 8*std::pow(C,2)*v*w*y0*z0/std::pow(B,2) + 2*std::pow(C,2)*std::pow(w,2)*std::pow(y0,2)/std::pow(B,2) + 6*std::pow(C,4)*std::pow(v,2)*std::pow(y0,2)/std::pow(B,4); - d = -4*std::pow(A,2)*u*x0 - 4*std::pow(A,2)*w*z0 + 4*std::pow(A,2)*std::pow(C,2)*v*y0/std::pow(B,2) - 4*std::pow(C,2)*u*x0 - 4*std::pow(C,2)*w*z0 + 4*u*std::pow(x0,3) + 4*u*x0*std::pow(z0,2) + 4*w*std::pow(x0,2)*z0 + 4*w*std::pow(z0,3) - 4*std::pow(C,4)*v*y0/std::pow(B,2) + 4*std::pow(C,2)*u*x0*std::pow(y0,2)/std::pow(B,2) + 4*std::pow(C,2)*v*std::pow(x0,2)*y0/std::pow(B,2) + 4*std::pow(C,2)*v*y0*std::pow(z0,2)/std::pow(B,2) + 4*std::pow(C,2)*w*std::pow(y0,2)*z0/std::pow(B,2) + 4*std::pow(C,4)*v*std::pow(y0,3)/std::pow(B,4); - e = std::pow(A,4) - 2*std::pow(A,2)*std::pow(C,2) - 2*std::pow(A,2)*std::pow(x0,2) - 2*std::pow(A,2)*std::pow(z0,2) + 2*std::pow(A,2)*std::pow(C,2)*std::pow(y0,2)/std::pow(B,2) + std::pow(C,4) - 2*std::pow(C,2)*std::pow(x0,2) - 2*std::pow(C,2)*std::pow(z0,2) + std::pow(x0,4) + 2*std::pow(x0,2)*std::pow(z0,2) + std::pow(z0,4) - 2*std::pow(C,4)*std::pow(y0,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(x0,2)*std::pow(y0,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(y0,2)*std::pow(z0,2)/std::pow(B,2) + std::pow(C,4)*std::pow(y0,4)/std::pow(B,4); + double a, b, c, d, e; // coefficients of quartic + + a = std::pow(u, 4) + 2 * std::pow(u, 2) * std::pow(w, 2) + std::pow(w, 4) + + 2 * std::pow(C, 2) * std::pow(u, 2) * std::pow(v, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(v, 2) * std::pow(w, 2) / std::pow(B, 2) + + std::pow(C, 4) * std::pow(v, 4) / std::pow(B, 4); + b = 4 * std::pow(u, 3) * x0 + 4 * std::pow(u, 2) * w * z0 + + 4 * u * std::pow(w, 2) * x0 + 4 * std::pow(w, 3) * z0 + + 4 * std::pow(C, 2) * std::pow(u, 2) * v * y0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * u * std::pow(v, 2) * x0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * std::pow(v, 2) * w * z0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * v * std::pow(w, 2) * y0 / std::pow(B, 2) + + 4 * std::pow(C, 4) * std::pow(v, 3) * y0 / std::pow(B, 4); + c = + -2 * std::pow(A, 2) * std::pow(u, 2) - 2 * std::pow(A, 2) * std::pow(w, 2) + + 2 * std::pow(A, 2) * std::pow(C, 2) * std::pow(v, 2) / std::pow(B, 2) - + 2 * std::pow(C, 2) * std::pow(u, 2) - 2 * std::pow(C, 2) * std::pow(w, 2) + + 6 * std::pow(u, 2) * std::pow(x0, 2) + + 2 * std::pow(u, 2) * std::pow(z0, 2) + 8 * u * w * x0 * z0 + + 2 * std::pow(w, 2) * std::pow(x0, 2) + + 6 * std::pow(w, 2) * std::pow(z0, 2) - + 2 * std::pow(C, 4) * std::pow(v, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(u, 2) * std::pow(y0, 2) / std::pow(B, 2) + + 8 * std::pow(C, 2) * u * v * x0 * y0 / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(v, 2) * std::pow(x0, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(v, 2) * std::pow(z0, 2) / std::pow(B, 2) + + 8 * std::pow(C, 2) * v * w * y0 * z0 / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(w, 2) * std::pow(y0, 2) / std::pow(B, 2) + + 6 * std::pow(C, 4) * std::pow(v, 2) * std::pow(y0, 2) / std::pow(B, 4); + d = -4 * std::pow(A, 2) * u * x0 - 4 * std::pow(A, 2) * w * z0 + + 4 * std::pow(A, 2) * std::pow(C, 2) * v * y0 / std::pow(B, 2) - + 4 * std::pow(C, 2) * u * x0 - 4 * std::pow(C, 2) * w * z0 + + 4 * u * std::pow(x0, 3) + 4 * u * x0 * std::pow(z0, 2) + + 4 * w * std::pow(x0, 2) * z0 + 4 * w * std::pow(z0, 3) - + 4 * std::pow(C, 4) * v * y0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * u * x0 * std::pow(y0, 2) / std::pow(B, 2) + + 4 * std::pow(C, 2) * v * std::pow(x0, 2) * y0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * v * y0 * std::pow(z0, 2) / std::pow(B, 2) + + 4 * std::pow(C, 2) * w * std::pow(y0, 2) * z0 / std::pow(B, 2) + + 4 * std::pow(C, 4) * v * std::pow(y0, 3) / std::pow(B, 4); + e = std::pow(A, 4) - 2 * std::pow(A, 2) * std::pow(C, 2) - + 2 * std::pow(A, 2) * std::pow(x0, 2) - + 2 * std::pow(A, 2) * std::pow(z0, 2) + + 2 * std::pow(A, 2) * std::pow(C, 2) * std::pow(y0, 2) / std::pow(B, 2) + + std::pow(C, 4) - 2 * std::pow(C, 2) * std::pow(x0, 2) - + 2 * std::pow(C, 2) * std::pow(z0, 2) + std::pow(x0, 4) + + 2 * std::pow(x0, 2) * std::pow(z0, 2) + std::pow(z0, 4) - + 2 * std::pow(C, 4) * std::pow(y0, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(x0, 2) * std::pow(y0, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(y0, 2) * std::pow(z0, 2) / std::pow(B, 2) + + std::pow(C, 4) * std::pow(y0, 4) / std::pow(B, 4); // - std::array roots; - quartic_solve(b,c,d,e,roots); - - if (coincident) r += ang*TINY_BIT; + std::array roots; + quartic_solve(b, c, d, e, roots); + + if (coincident) + r += ang * TINY_BIT; // special degerenate case two sets of repated - if ( b == 0.0 && d == 0) { - if ( roots[1] - roots[0] < 1e-5 ) return INFTY; - if ( roots[3] - roots[2] < 1e-5 ) return INFTY; + if (b == 0.0 && d == 0) { + if (roots[1] - roots[0] < 1e-5) + return INFTY; + if (roots[3] - roots[2] < 1e-5) + return INFTY; } - for ( int i = 0 ; i < 4 ; i++ ) { + for (int i = 0; i < 4; i++) { // need something better than just raw tolerance // use fastQS to get back lost precision - if ( roots[i] > 1e-6) { + if (roots[i] > 1e-6) { return roots[i]; - } + } } // otherwise no hit @@ -1369,44 +1457,42 @@ SurfaceYTorus::distance(Position r, Direction ang, bool coincident) const return 0; } -Direction -SurfaceYTorus::normal(Position r) const +Direction SurfaceYTorus::normal(Position r) const { // reduce the expansion of the full form for torus double x = r.x - x0_; double y = r.y - y0_; double z = r.z - z0_; - double A2 = A_*A_; - double B2 = B_*B_; - double C2 = C_*C_; - - // a**4 - 2*a**2*c**2 - 2*a**2*x**2 - 2*a**2*z**2 + 2*a**2*c**2*y**2/b**2 + c**4 - // - 2*c**2*x**2 - 2*c**2*z**2 + x**4 + 2*x**2*z**2 + z**4 - 2*c**4*y**2/b**2 + - // 2*c**2*x**2*y**2/b**2 + 2*c**2*y**2*z**2/b**2 + c**4*y**4/b**4 + double A2 = A_ * A_; + double B2 = B_ * B_; + double C2 = C_ * C_; + // a**4 - 2*a**2*c**2 - 2*a**2*x**2 - 2*a**2*z**2 + 2*a**2*c**2*y**2/b**2 + + // c**4 + // - 2*c**2*x**2 - 2*c**2*z**2 + x**4 + 2*x**2*z**2 + z**4 - 2*c**4*y**2/b**2 + // + 2*c**2*x**2*y**2/b**2 + 2*c**2*y**2*z**2/b**2 + c**4*y**4/b**4 // coefficients for the x, x^2, x^3 and x^4 term - // but since we differeniate - we get x4 -> 4x3 - double dx_4 = 4*x*x*x; - double dx_2 = 4*x*(z*z + C2*y*y/B2 - A2 - C2); + // but since we differeniate - we get x4 -> 4x3 + double dx_4 = 4 * x * x * x; + double dx_2 = 4 * x * (z * z + C2 * y * y / B2 - A2 - C2); double dx = dx_4 + dx_2; // similarly to y - double dy_4 = 4*y*y*y*C2*C2/(B2*B2); - double dy_2 = 4*y*(z*z + C2*x*x/B2 - A2 - C2); + double dy_4 = 4 * y * y * y * C2 * C2 / (B2 * B2); + double dy_2 = 4 * y * (z * z + C2 * x * x / B2 - A2 - C2); double dy = dy_2 + dy_4; // and z - double dz_4 = 4*z*z*z; - double dz_2 = 4*z*(C2*y*y/B2 + x*x - C2 - A2); + double dz_4 = 4 * z * z * z; + double dz_2 = 4 * z * (C2 * y * y / B2 + x * x - C2 - A2); double dz = dz_2 + dz_4; - double length = std::sqrt(dx*dx + dy*dy + dz*dz); + double length = std::sqrt(dx * dx + dy * dy + dz * dz); - return {dx/length,dy/length,dz/length}; + return {dx / length, dy / length, dz / length}; } -SurfaceZTorus::SurfaceZTorus(pugi::xml_node surf_node) - : CSGSurface(surf_node) +SurfaceZTorus::SurfaceZTorus(pugi::xml_node surf_node) : CSGSurface(surf_node) { read_coeffs(surf_node, id_, x0_, y0_, z0_, A_, B_, C_); } @@ -1414,29 +1500,28 @@ SurfaceZTorus::SurfaceZTorus(pugi::xml_node surf_node) void SurfaceZTorus::to_hdf5_inner(hid_t group_id) const { write_string(group_id, "type", "z-torus", false); - std::array coeffs {{x0_, y0_, z0_, A_, B_, C_ }}; + std::array coeffs {{x0_, y0_, z0_, A_, B_, C_}}; write_dataset(group_id, "coefficients", coeffs); } -double -SurfaceZTorus::evaluate(Position r) const +double SurfaceZTorus::evaluate(Position r) const { const double x = r.x; const double y = r.y; const double z = r.z; - const double x_coeff2 = std::pow(x-x0_,2); - const double y_coeff2 = std::pow(y-y0_,2); - const double z_coeff2 = std::pow(z-z0_,2); + const double x_coeff2 = std::pow(x - x0_, 2); + const double y_coeff2 = std::pow(y - y0_, 2); + const double z_coeff2 = std::pow(z - z0_, 2); - const double B2 = B_*B_; - const double C2 = C_*C_; + const double B2 = B_ * B_; + const double C2 = C_ * C_; - return z_coeff2/B2 + std::pow(std::sqrt(x_coeff2 + y_coeff2)-A_,2)/C2 - 1.; + return z_coeff2 / B2 + std::pow(std::sqrt(x_coeff2 + y_coeff2) - A_, 2) / C2 - + 1.; } -double -SurfaceZTorus::distance(Position r, Direction ang, bool coincident) const +double SurfaceZTorus::distance(Position r, Direction ang, bool coincident) const { double u = ang.x; double v = ang.y; @@ -1453,79 +1538,222 @@ SurfaceZTorus::distance(Position r, Direction ang, bool coincident) const double x0 = x0_; double y0 = y0_; double z0 = z0_; - - double a,b,c,d,e; // coefficients of quartic + + double a, b, c, d, e; // coefficients of quartic // 4th order coefficient - a = std::pow(u,4) + 2*std::pow(u,2)*std::pow(v,2) + std::pow(v,4) + 2*std::pow(C,2)*std::pow(u,2)*std::pow(w,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(v,2)*std::pow(w,2)/std::pow(B,2) + std::pow(C,4)*std::pow(w,4)/std::pow(B,4); + a = std::pow(u, 4) + 2 * std::pow(u, 2) * std::pow(v, 2) + std::pow(v, 4) + + 2 * std::pow(C, 2) * std::pow(u, 2) * std::pow(w, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(v, 2) * std::pow(w, 2) / std::pow(B, 2) + + std::pow(C, 4) * std::pow(w, 4) / std::pow(B, 4); // 3rd order coefficient - b = -4*std::pow(u,3)*x0 + 4*std::pow(u,3)*xr - 4*std::pow(u,2)*v*y0 + 4*std::pow(u,2)*v*yr - 4*u*std::pow(v,2)*x0 + 4*u*std::pow(v,2)*xr - 4*std::pow(v,3)*y0 + 4*std::pow(v,3)*yr - 4*std::pow(C,2)*std::pow(u,2)*w*z0/std::pow(B,2) + 4*std::pow(C,2)*std::pow(u,2)*w*zr/std::pow(B,2) - 4*std::pow(C,2)*u*std::pow(w,2)*x0/std::pow(B,2) + 4*std::pow(C,2)*u*std::pow(w,2)*xr/std::pow(B,2) - 4*std::pow(C,2)*std::pow(v,2)*w*z0/std::pow(B,2) + 4*std::pow(C,2)*std::pow(v,2)*w*zr/std::pow(B,2) - 4*std::pow(C,2)*v*std::pow(w,2)*y0/std::pow(B,2) + 4*std::pow(C,2)*v*std::pow(w,2)*yr/std::pow(B,2) - 4*std::pow(C,4)*std::pow(w,3)*z0/std::pow(B,4) + 4*std::pow(C,4)*std::pow(w,3)*zr/std::pow(B,4); + b = -4 * std::pow(u, 3) * x0 + 4 * std::pow(u, 3) * xr - + 4 * std::pow(u, 2) * v * y0 + 4 * std::pow(u, 2) * v * yr - + 4 * u * std::pow(v, 2) * x0 + 4 * u * std::pow(v, 2) * xr - + 4 * std::pow(v, 3) * y0 + 4 * std::pow(v, 3) * yr - + 4 * std::pow(C, 2) * std::pow(u, 2) * w * z0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * std::pow(u, 2) * w * zr / std::pow(B, 2) - + 4 * std::pow(C, 2) * u * std::pow(w, 2) * x0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * u * std::pow(w, 2) * xr / std::pow(B, 2) - + 4 * std::pow(C, 2) * std::pow(v, 2) * w * z0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * std::pow(v, 2) * w * zr / std::pow(B, 2) - + 4 * std::pow(C, 2) * v * std::pow(w, 2) * y0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * v * std::pow(w, 2) * yr / std::pow(B, 2) - + 4 * std::pow(C, 4) * std::pow(w, 3) * z0 / std::pow(B, 4) + + 4 * std::pow(C, 4) * std::pow(w, 3) * zr / std::pow(B, 4); // 2nd order coefficient - c= -2*std::pow(A,2)*std::pow(u,2) - 2*std::pow(A,2)*std::pow(v,2) + 2*std::pow(A,2)*std::pow(C,2)*std::pow(w,2)/std::pow(B,2) - 2*std::pow(C,2)*std::pow(u,2) - 2*std::pow(C,2)*std::pow(v,2) + 6*std::pow(u,2)*std::pow(x0,2) - 12*std::pow(u,2)*x0*xr + 6*std::pow(u,2)*std::pow(xr,2) + 2*std::pow(u,2)*std::pow(y0,2) - 4*std::pow(u,2)*y0*yr + 2*std::pow(u,2)*std::pow(yr,2) + 8*u*v*x0*y0 - 8*u*v*x0*yr - 8*u*v*xr*y0 + 8*u*v*xr*yr + 2*std::pow(v,2)*std::pow(x0,2) - 4*std::pow(v,2)*x0*xr + 2*std::pow(v,2)*std::pow(xr,2) + 6*std::pow(v,2)*std::pow(y0,2) - 12*std::pow(v,2)*y0*yr + 6*std::pow(v,2)*std::pow(yr,2) - 2*std::pow(C,4)*std::pow(w,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(u,2)*std::pow(z0,2)/std::pow(B,2) - 4*std::pow(C,2)*std::pow(u,2)*z0*zr/std::pow(B,2) + 2*std::pow(C,2)*std::pow(u,2)*std::pow(zr,2)/std::pow(B,2) + 8*std::pow(C,2)*u*w*x0*z0/std::pow(B,2) - 8*std::pow(C,2)*u*w*x0*zr/std::pow(B,2) - 8*std::pow(C,2)*u*w*xr*z0/std::pow(B,2) + 8*std::pow(C,2)*u*w*xr*zr/std::pow(B,2) + 2*std::pow(C,2)*std::pow(v,2)*std::pow(z0,2)/std::pow(B,2) - 4*std::pow(C,2)*std::pow(v,2)*z0*zr/std::pow(B,2) + 2*std::pow(C,2)*std::pow(v,2)*std::pow(zr,2)/std::pow(B,2) + 8*std::pow(C,2)*v*w*y0*z0/std::pow(B,2) - 8*std::pow(C,2)*v*w*y0*zr/std::pow(B,2) - 8*std::pow(C,2)*v*w*yr*z0/std::pow(B,2) + 8*std::pow(C,2)*v*w*yr*zr/std::pow(B,2) + 2*std::pow(C,2)*std::pow(w,2)*std::pow(x0,2)/std::pow(B,2) - 4*std::pow(C,2)*std::pow(w,2)*x0*xr/std::pow(B,2) + 2*std::pow(C,2)*std::pow(w,2)*std::pow(xr,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(w,2)*std::pow(y0,2)/std::pow(B,2) - 4*std::pow(C,2)*std::pow(w,2)*y0*yr/std::pow(B,2) + 2*std::pow(C,2)*std::pow(w,2)*std::pow(yr,2)/std::pow(B,2) + 6*std::pow(C,4)*std::pow(w,2)*std::pow(z0,2)/std::pow(B,4) - 12*std::pow(C,4)*std::pow(w,2)*z0*zr/std::pow(B,4) + 6*std::pow(C,4)*std::pow(w,2)*std::pow(zr,2)/std::pow(B,4); + c = + -2 * std::pow(A, 2) * std::pow(u, 2) - 2 * std::pow(A, 2) * std::pow(v, 2) + + 2 * std::pow(A, 2) * std::pow(C, 2) * std::pow(w, 2) / std::pow(B, 2) - + 2 * std::pow(C, 2) * std::pow(u, 2) - 2 * std::pow(C, 2) * std::pow(v, 2) + + 6 * std::pow(u, 2) * std::pow(x0, 2) - 12 * std::pow(u, 2) * x0 * xr + + 6 * std::pow(u, 2) * std::pow(xr, 2) + + 2 * std::pow(u, 2) * std::pow(y0, 2) - 4 * std::pow(u, 2) * y0 * yr + + 2 * std::pow(u, 2) * std::pow(yr, 2) + 8 * u * v * x0 * y0 - + 8 * u * v * x0 * yr - 8 * u * v * xr * y0 + 8 * u * v * xr * yr + + 2 * std::pow(v, 2) * std::pow(x0, 2) - 4 * std::pow(v, 2) * x0 * xr + + 2 * std::pow(v, 2) * std::pow(xr, 2) + + 6 * std::pow(v, 2) * std::pow(y0, 2) - 12 * std::pow(v, 2) * y0 * yr + + 6 * std::pow(v, 2) * std::pow(yr, 2) - + 2 * std::pow(C, 4) * std::pow(w, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(u, 2) * std::pow(z0, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * std::pow(u, 2) * z0 * zr / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(u, 2) * std::pow(zr, 2) / std::pow(B, 2) + + 8 * std::pow(C, 2) * u * w * x0 * z0 / std::pow(B, 2) - + 8 * std::pow(C, 2) * u * w * x0 * zr / std::pow(B, 2) - + 8 * std::pow(C, 2) * u * w * xr * z0 / std::pow(B, 2) + + 8 * std::pow(C, 2) * u * w * xr * zr / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(v, 2) * std::pow(z0, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * std::pow(v, 2) * z0 * zr / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(v, 2) * std::pow(zr, 2) / std::pow(B, 2) + + 8 * std::pow(C, 2) * v * w * y0 * z0 / std::pow(B, 2) - + 8 * std::pow(C, 2) * v * w * y0 * zr / std::pow(B, 2) - + 8 * std::pow(C, 2) * v * w * yr * z0 / std::pow(B, 2) + + 8 * std::pow(C, 2) * v * w * yr * zr / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(w, 2) * std::pow(x0, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * std::pow(w, 2) * x0 * xr / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(w, 2) * std::pow(xr, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(w, 2) * std::pow(y0, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * std::pow(w, 2) * y0 * yr / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(w, 2) * std::pow(yr, 2) / std::pow(B, 2) + + 6 * std::pow(C, 4) * std::pow(w, 2) * std::pow(z0, 2) / std::pow(B, 4) - + 12 * std::pow(C, 4) * std::pow(w, 2) * z0 * zr / std::pow(B, 4) + + 6 * std::pow(C, 4) * std::pow(w, 2) * std::pow(zr, 2) / std::pow(B, 4); // the 1st order terms - d = 4*std::pow(A,2)*u*x0 - 4*std::pow(A,2)*u*xr + 4*std::pow(A,2)*v*y0 - 4*std::pow(A,2)*v*yr - 4*std::pow(A,2)*std::pow(C,2)*w*z0/std::pow(B,2) + 4*std::pow(A,2)*std::pow(C,2)*w*zr/std::pow(B,2) + 4*std::pow(C,2)*u*x0 - 4*std::pow(C,2)*u*xr + 4*std::pow(C,2)*v*y0 - 4*std::pow(C,2)*v*yr - 4*u*std::pow(x0,3) + 12*u*std::pow(x0,2)*xr - 12*u*x0*std::pow(xr,2) - 4*u*x0*std::pow(y0,2) + 8*u*x0*y0*yr - 4*u*x0*std::pow(yr,2) + 4*u*std::pow(xr,3) + 4*u*xr*std::pow(y0,2) - 8*u*xr*y0*yr + 4*u*xr*std::pow(yr,2) - 4*v*std::pow(x0,2)*y0 + 4*v*std::pow(x0,2)*yr + 8*v*x0*xr*y0 - 8*v*x0*xr*yr - 4*v*std::pow(xr,2)*y0 + 4*v*std::pow(xr,2)*yr - 4*v*std::pow(y0,3) + 12*v*std::pow(y0,2)*yr - 12*v*y0*std::pow(yr,2) + 4*v*std::pow(yr,3) + 4*std::pow(C,4)*w*z0/std::pow(B,2) - 4*std::pow(C,4)*w*zr/std::pow(B,2) - 4*std::pow(C,2)*u*x0*std::pow(z0,2)/std::pow(B,2) + 8*std::pow(C,2)*u*x0*z0*zr/std::pow(B,2) - 4*std::pow(C,2)*u*x0*std::pow(zr,2)/std::pow(B,2) + 4*std::pow(C,2)*u*xr*std::pow(z0,2)/std::pow(B,2) - 8*std::pow(C,2)*u*xr*z0*zr/std::pow(B,2) + 4*std::pow(C,2)*u*xr*std::pow(zr,2)/std::pow(B,2) - 4*std::pow(C,2)*v*y0*std::pow(z0,2)/std::pow(B,2) + 8*std::pow(C,2)*v*y0*z0*zr/std::pow(B,2) - 4*std::pow(C,2)*v*y0*std::pow(zr,2)/std::pow(B,2) + 4*std::pow(C,2)*v*yr*std::pow(z0,2)/std::pow(B,2) - 8*std::pow(C,2)*v*yr*z0*zr/std::pow(B,2) + 4*std::pow(C,2)*v*yr*std::pow(zr,2)/std::pow(B,2) - 4*std::pow(C,2)*w*std::pow(x0,2)*z0/std::pow(B,2) + 4*std::pow(C,2)*w*std::pow(x0,2)*zr/std::pow(B,2) + 8*std::pow(C,2)*w*x0*xr*z0/std::pow(B,2) - 8*std::pow(C,2)*w*x0*xr*zr/std::pow(B,2) - 4*std::pow(C,2)*w*std::pow(xr,2)*z0/std::pow(B,2) + 4*std::pow(C,2)*w*std::pow(xr,2)*zr/std::pow(B,2) - 4*std::pow(C,2)*w*std::pow(y0,2)*z0/std::pow(B,2) + 4*std::pow(C,2)*w*std::pow(y0,2)*zr/std::pow(B,2) + 8*std::pow(C,2)*w*y0*yr*z0/std::pow(B,2) - 8*std::pow(C,2)*w*y0*yr*zr/std::pow(B,2) - 4*std::pow(C,2)*w*std::pow(yr,2)*z0/std::pow(B,2) + 4*std::pow(C,2)*w*std::pow(yr,2)*zr/std::pow(B,2) - 4*std::pow(C,4)*w*std::pow(z0,3)/std::pow(B,4) + 12*std::pow(C,4)*w*std::pow(z0,2)*zr/std::pow(B,4) - 12*std::pow(C,4)*w*z0*std::pow(zr,2)/std::pow(B,4) + 4*std::pow(C,4)*w*std::pow(zr,3)/std::pow(B,4); + d = 4 * std::pow(A, 2) * u * x0 - 4 * std::pow(A, 2) * u * xr + + 4 * std::pow(A, 2) * v * y0 - 4 * std::pow(A, 2) * v * yr - + 4 * std::pow(A, 2) * std::pow(C, 2) * w * z0 / std::pow(B, 2) + + 4 * std::pow(A, 2) * std::pow(C, 2) * w * zr / std::pow(B, 2) + + 4 * std::pow(C, 2) * u * x0 - 4 * std::pow(C, 2) * u * xr + + 4 * std::pow(C, 2) * v * y0 - 4 * std::pow(C, 2) * v * yr - + 4 * u * std::pow(x0, 3) + 12 * u * std::pow(x0, 2) * xr - + 12 * u * x0 * std::pow(xr, 2) - 4 * u * x0 * std::pow(y0, 2) + + 8 * u * x0 * y0 * yr - 4 * u * x0 * std::pow(yr, 2) + + 4 * u * std::pow(xr, 3) + 4 * u * xr * std::pow(y0, 2) - + 8 * u * xr * y0 * yr + 4 * u * xr * std::pow(yr, 2) - + 4 * v * std::pow(x0, 2) * y0 + 4 * v * std::pow(x0, 2) * yr + + 8 * v * x0 * xr * y0 - 8 * v * x0 * xr * yr - + 4 * v * std::pow(xr, 2) * y0 + 4 * v * std::pow(xr, 2) * yr - + 4 * v * std::pow(y0, 3) + 12 * v * std::pow(y0, 2) * yr - + 12 * v * y0 * std::pow(yr, 2) + 4 * v * std::pow(yr, 3) + + 4 * std::pow(C, 4) * w * z0 / std::pow(B, 2) - + 4 * std::pow(C, 4) * w * zr / std::pow(B, 2) - + 4 * std::pow(C, 2) * u * x0 * std::pow(z0, 2) / std::pow(B, 2) + + 8 * std::pow(C, 2) * u * x0 * z0 * zr / std::pow(B, 2) - + 4 * std::pow(C, 2) * u * x0 * std::pow(zr, 2) / std::pow(B, 2) + + 4 * std::pow(C, 2) * u * xr * std::pow(z0, 2) / std::pow(B, 2) - + 8 * std::pow(C, 2) * u * xr * z0 * zr / std::pow(B, 2) + + 4 * std::pow(C, 2) * u * xr * std::pow(zr, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * v * y0 * std::pow(z0, 2) / std::pow(B, 2) + + 8 * std::pow(C, 2) * v * y0 * z0 * zr / std::pow(B, 2) - + 4 * std::pow(C, 2) * v * y0 * std::pow(zr, 2) / std::pow(B, 2) + + 4 * std::pow(C, 2) * v * yr * std::pow(z0, 2) / std::pow(B, 2) - + 8 * std::pow(C, 2) * v * yr * z0 * zr / std::pow(B, 2) + + 4 * std::pow(C, 2) * v * yr * std::pow(zr, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * w * std::pow(x0, 2) * z0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * w * std::pow(x0, 2) * zr / std::pow(B, 2) + + 8 * std::pow(C, 2) * w * x0 * xr * z0 / std::pow(B, 2) - + 8 * std::pow(C, 2) * w * x0 * xr * zr / std::pow(B, 2) - + 4 * std::pow(C, 2) * w * std::pow(xr, 2) * z0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * w * std::pow(xr, 2) * zr / std::pow(B, 2) - + 4 * std::pow(C, 2) * w * std::pow(y0, 2) * z0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * w * std::pow(y0, 2) * zr / std::pow(B, 2) + + 8 * std::pow(C, 2) * w * y0 * yr * z0 / std::pow(B, 2) - + 8 * std::pow(C, 2) * w * y0 * yr * zr / std::pow(B, 2) - + 4 * std::pow(C, 2) * w * std::pow(yr, 2) * z0 / std::pow(B, 2) + + 4 * std::pow(C, 2) * w * std::pow(yr, 2) * zr / std::pow(B, 2) - + 4 * std::pow(C, 4) * w * std::pow(z0, 3) / std::pow(B, 4) + + 12 * std::pow(C, 4) * w * std::pow(z0, 2) * zr / std::pow(B, 4) - + 12 * std::pow(C, 4) * w * z0 * std::pow(zr, 2) / std::pow(B, 4) + + 4 * std::pow(C, 4) * w * std::pow(zr, 3) / std::pow(B, 4); // the 0th order terms - e = std::pow(A,4) - 2*std::pow(A,2)*std::pow(C,2) - 2*std::pow(A,2)*std::pow(x0,2) + 4*std::pow(A,2)*x0*xr - 2*std::pow(A,2)*std::pow(xr,2) - 2*std::pow(A,2)*std::pow(y0,2) + 4*std::pow(A,2)*y0*yr - 2*std::pow(A,2)*std::pow(yr,2) + 2*std::pow(A,2)*std::pow(C,2)*std::pow(z0,2)/std::pow(B,2) - 4*std::pow(A,2)*std::pow(C,2)*z0*zr/std::pow(B,2) + 2*std::pow(A,2)*std::pow(C,2)*std::pow(zr,2)/std::pow(B,2) + std::pow(C,4) - 2*std::pow(C,2)*std::pow(x0,2) + 4*std::pow(C,2)*x0*xr - 2*std::pow(C,2)*std::pow(xr,2) - 2*std::pow(C,2)*std::pow(y0,2) + 4*std::pow(C,2)*y0*yr - 2*std::pow(C,2)*std::pow(yr,2) + std::pow(x0,4) - 4*std::pow(x0,3)*xr + 6*std::pow(x0,2)*std::pow(xr,2) + 2*std::pow(x0,2)*std::pow(y0,2) - 4*std::pow(x0,2)*y0*yr + 2*std::pow(x0,2)*std::pow(yr,2) - 4*x0*std::pow(xr,3) - 4*x0*xr*std::pow(y0,2) + 8*x0*xr*y0*yr - 4*x0*xr*std::pow(yr,2) + std::pow(xr,4) + 2*std::pow(xr,2)*std::pow(y0,2) - 4*std::pow(xr,2)*y0*yr + 2*std::pow(xr,2)*std::pow(yr,2) + std::pow(y0,4) - 4*std::pow(y0,3)*yr + 6*std::pow(y0,2)*std::pow(yr,2) - 4*y0*std::pow(yr,3) + std::pow(yr,4) - 2*std::pow(C,4)*std::pow(z0,2)/std::pow(B,2) + 4*std::pow(C,4)*z0*zr/std::pow(B,2) - 2*std::pow(C,4)*std::pow(zr,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(x0,2)*std::pow(z0,2)/std::pow(B,2) - 4*std::pow(C,2)*std::pow(x0,2)*z0*zr/std::pow(B,2) + 2*std::pow(C,2)*std::pow(x0,2)*std::pow(zr,2)/std::pow(B,2) - 4*std::pow(C,2)*x0*xr*std::pow(z0,2)/std::pow(B,2) + 8*std::pow(C,2)*x0*xr*z0*zr/std::pow(B,2) - 4*std::pow(C,2)*x0*xr*std::pow(zr,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(xr,2)*std::pow(z0,2)/std::pow(B,2) - 4*std::pow(C,2)*std::pow(xr,2)*z0*zr/std::pow(B,2) + 2*std::pow(C,2)*std::pow(xr,2)*std::pow(zr,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(y0,2)*std::pow(z0,2)/std::pow(B,2) - 4*std::pow(C,2)*std::pow(y0,2)*z0*zr/std::pow(B,2) + 2*std::pow(C,2)*std::pow(y0,2)*std::pow(zr,2)/std::pow(B,2) - 4*std::pow(C,2)*y0*yr*std::pow(z0,2)/std::pow(B,2) + 8*std::pow(C,2)*y0*yr*z0*zr/std::pow(B,2) - 4*std::pow(C,2)*y0*yr*std::pow(zr,2)/std::pow(B,2) + 2*std::pow(C,2)*std::pow(yr,2)*std::pow(z0,2)/std::pow(B,2) - 4*std::pow(C,2)*std::pow(yr,2)*z0*zr/std::pow(B,2) + 2*std::pow(C,2)*std::pow(yr,2)*std::pow(zr,2)/std::pow(B,2) + std::pow(C,4)*std::pow(z0,4)/std::pow(B,4) - 4*std::pow(C,4)*std::pow(z0,3)*zr/std::pow(B,4) + 6*std::pow(C,4)*std::pow(z0,2)*std::pow(zr,2)/std::pow(B,4) - 4*std::pow(C,4)*z0*std::pow(zr,3)/std::pow(B,4) + std::pow(C,4)*std::pow(zr,4)/std::pow(B,4); + e = std::pow(A, 4) - 2 * std::pow(A, 2) * std::pow(C, 2) - + 2 * std::pow(A, 2) * std::pow(x0, 2) + 4 * std::pow(A, 2) * x0 * xr - + 2 * std::pow(A, 2) * std::pow(xr, 2) - + 2 * std::pow(A, 2) * std::pow(y0, 2) + 4 * std::pow(A, 2) * y0 * yr - + 2 * std::pow(A, 2) * std::pow(yr, 2) + + 2 * std::pow(A, 2) * std::pow(C, 2) * std::pow(z0, 2) / std::pow(B, 2) - + 4 * std::pow(A, 2) * std::pow(C, 2) * z0 * zr / std::pow(B, 2) + + 2 * std::pow(A, 2) * std::pow(C, 2) * std::pow(zr, 2) / std::pow(B, 2) + + std::pow(C, 4) - 2 * std::pow(C, 2) * std::pow(x0, 2) + + 4 * std::pow(C, 2) * x0 * xr - 2 * std::pow(C, 2) * std::pow(xr, 2) - + 2 * std::pow(C, 2) * std::pow(y0, 2) + 4 * std::pow(C, 2) * y0 * yr - + 2 * std::pow(C, 2) * std::pow(yr, 2) + std::pow(x0, 4) - + 4 * std::pow(x0, 3) * xr + 6 * std::pow(x0, 2) * std::pow(xr, 2) + + 2 * std::pow(x0, 2) * std::pow(y0, 2) - 4 * std::pow(x0, 2) * y0 * yr + + 2 * std::pow(x0, 2) * std::pow(yr, 2) - 4 * x0 * std::pow(xr, 3) - + 4 * x0 * xr * std::pow(y0, 2) + 8 * x0 * xr * y0 * yr - + 4 * x0 * xr * std::pow(yr, 2) + std::pow(xr, 4) + + 2 * std::pow(xr, 2) * std::pow(y0, 2) - 4 * std::pow(xr, 2) * y0 * yr + + 2 * std::pow(xr, 2) * std::pow(yr, 2) + std::pow(y0, 4) - + 4 * std::pow(y0, 3) * yr + 6 * std::pow(y0, 2) * std::pow(yr, 2) - + 4 * y0 * std::pow(yr, 3) + std::pow(yr, 4) - + 2 * std::pow(C, 4) * std::pow(z0, 2) / std::pow(B, 2) + + 4 * std::pow(C, 4) * z0 * zr / std::pow(B, 2) - + 2 * std::pow(C, 4) * std::pow(zr, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(x0, 2) * std::pow(z0, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * std::pow(x0, 2) * z0 * zr / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(x0, 2) * std::pow(zr, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * x0 * xr * std::pow(z0, 2) / std::pow(B, 2) + + 8 * std::pow(C, 2) * x0 * xr * z0 * zr / std::pow(B, 2) - + 4 * std::pow(C, 2) * x0 * xr * std::pow(zr, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(xr, 2) * std::pow(z0, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * std::pow(xr, 2) * z0 * zr / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(xr, 2) * std::pow(zr, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(y0, 2) * std::pow(z0, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * std::pow(y0, 2) * z0 * zr / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(y0, 2) * std::pow(zr, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * y0 * yr * std::pow(z0, 2) / std::pow(B, 2) + + 8 * std::pow(C, 2) * y0 * yr * z0 * zr / std::pow(B, 2) - + 4 * std::pow(C, 2) * y0 * yr * std::pow(zr, 2) / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(yr, 2) * std::pow(z0, 2) / std::pow(B, 2) - + 4 * std::pow(C, 2) * std::pow(yr, 2) * z0 * zr / std::pow(B, 2) + + 2 * std::pow(C, 2) * std::pow(yr, 2) * std::pow(zr, 2) / std::pow(B, 2) + + std::pow(C, 4) * std::pow(z0, 4) / std::pow(B, 4) - + 4 * std::pow(C, 4) * std::pow(z0, 3) * zr / std::pow(B, 4) + + 6 * std::pow(C, 4) * std::pow(z0, 2) * std::pow(zr, 2) / std::pow(B, 4) - + 4 * std::pow(C, 4) * z0 * std::pow(zr, 3) / std::pow(B, 4) + + std::pow(C, 4) * std::pow(zr, 4) / std::pow(B, 4); // - std::array roots; - quartic_solve(b,c,d,e,roots); - - if (coincident) r += ang*TINY_BIT; + std::array roots; + quartic_solve(b, c, d, e, roots); + + if (coincident) + r += ang * TINY_BIT; // special degerenate case two sets of repated // roots - if ( b == 0.0 && d == 0) { - if ( roots[1] - roots[0] < 1e-5 ) return INFTY; - if ( roots[3] - roots[2] < 1e-5 ) return INFTY; + if (b == 0.0 && d == 0) { + if (roots[1] - roots[0] < 1e-5) + return INFTY; + if (roots[3] - roots[2] < 1e-5) + return INFTY; } - for ( int i = 0 ; i < 4 ; i++ ) { + for (int i = 0; i < 4; i++) { // need something better than just raw tolerance // use fastQS to get back lost precision - if ( roots[i] > 1e-6) { + if (roots[i] > 1e-6) { return roots[i]; - } + } } // otherwise no hit return INFTY; } -Direction -SurfaceZTorus::normal(Position r) const +Direction SurfaceZTorus::normal(Position r) const { // reduce the expansion of the full form for torus double x = r.x - x0_; double y = r.y - y0_; double z = r.z - z0_; - double A2 = A_*A_; - double B2 = B_*B_; - double C2 = C_*C_; - + double A2 = A_ * A_; + double B2 = B_ * B_; + double C2 = C_ * C_; // coefficients for the x, x^2, x^3 and x^4 term - // but since we differeniate - we get x4 -> 4x3 + // but since we differeniate - we get x4 -> 4x3 double dx_4 = 1.0; - double dx_2 = -2.*(A2 - C2 + y*y + C2*z*z/B2); - double dx = 4.*dx_4*x*x*x + 2.*dx_2*x; + double dx_2 = -2. * (A2 - C2 + y * y + C2 * z * z / B2); + double dx = 4. * dx_4 * x * x * x + 2. * dx_2 * x; // similarly to y double dy_4 = 1.0; - double dy_2 = -2.*(A2 - C2 + x*x + C2*z*z/B2); - double dy = 4.*dy_4*y*y*y + 2.*dy_2*y; + double dy_2 = -2. * (A2 - C2 + x * x + C2 * z * z / B2); + double dy = 4. * dy_4 * y * y * y + 2. * dy_2 * y; // and z - double dz_4 = std::pow(C_,4)/std::pow(B_,4); - double dz_2 = 2.*(A2*C2/B2 - std::pow(C_,4)/B2 + C2*x*x/B2 + C2*y*y/B2); - double dz = 4.*dz_4*z*z*z + 2.*dz_2*z; + double dz_4 = std::pow(C_, 4) / std::pow(B_, 4); + double dz_2 = 2. * (A2 * C2 / B2 - std::pow(C_, 4) / B2 + C2 * x * x / B2 + + C2 * y * y / B2); + double dz = 4. * dz_4 * z * z * z + 2. * dz_2 * z; - double length = std::sqrt(dx*dx + dy*dy + dz*dz); + double length = std::sqrt(dx * dx + dy * dy + dz * dz); - return {dx/length,dy/length,dz/length}; + return {dx / length, dy / length, dz / length}; } - - //============================================================================== void read_surfaces(pugi::xml_node node)