libcruft-util/polynomial.cpp

158 lines
4.7 KiB
C++
Raw Normal View History

2015-01-21 23:40:45 +11:00
/*
2015-04-13 18:05:28 +10:00
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
2015-01-21 23:40:45 +11:00
*
2015-04-13 18:05:28 +10:00
* http://www.apache.org/licenses/LICENSE-2.0
2015-01-21 23:40:45 +11:00
*
2015-04-13 18:05:28 +10:00
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
2015-01-21 23:40:45 +11:00
*
* Copyright 2015 Danny Robson <danny@nerdcruft.net>
*/
#include "polynomial.hpp"
#include "maths.hpp"
#include <limits>
#include <cmath>
static const size_t NEWTON_ITERATIONS = 1u;
2015-01-21 23:40:45 +11:00
//-----------------------------------------------------------------------------
2016-12-21 20:21:28 +11:00
namespace util::polynomial {
2015-01-21 23:40:45 +11:00
template <>
std::array<float,1>
2015-07-06 21:41:03 +10:00
roots (std::array<float,2> coeff)
2015-01-21 23:40:45 +11:00
{
const float a = coeff[0];
const float b = coeff[1];
if (almost_zero (a))
return { std::numeric_limits<float>::quiet_NaN () };
return { -b / a };
}
2016-12-21 20:21:28 +11:00
}
2015-01-21 23:40:45 +11:00
//-----------------------------------------------------------------------------
2016-12-21 20:21:28 +11:00
namespace util::polynomial {
2015-01-21 23:40:45 +11:00
template <>
std::array<float,2>
2015-07-06 21:41:03 +10:00
roots (std::array<float,3> coeff)
2015-01-21 23:40:45 +11:00
{
const float a = coeff[0];
const float b = coeff[1];
const float c = coeff[2];
if (almost_zero (a)) {
2015-07-06 21:41:03 +10:00
auto s = roots<1> ({b, c});
2015-01-21 23:40:45 +11:00
return { s[0], std::numeric_limits<float>::quiet_NaN () };
}
2016-12-21 20:21:28 +11:00
auto descriminator = std::sqrt (pow2 (b) - 4 * a * c);
return {
(-b - descriminator) / (2 * a),
(-b + descriminator) / (2 * a)
};
2015-01-21 23:40:45 +11:00
}
2016-12-21 20:21:28 +11:00
}
2015-01-21 23:40:45 +11:00
//-----------------------------------------------------------------------------
// From graphics gems: http://tog.acm.org/resources/GraphicsGems/gemsiv/vec_mat/ray/solver.c
2016-12-21 20:21:28 +11:00
namespace util::polynomial {
2015-01-21 23:40:45 +11:00
template <>
std::array<float,3>
2015-07-06 21:41:03 +10:00
roots (std::array<float,4> coeffs)
2015-01-21 23:40:45 +11:00
{
const float _a = coeffs[0];
const float _b = coeffs[1];
const float _c = coeffs[2];
const float _d = coeffs[3];
2015-01-21 23:40:45 +11:00
2015-01-29 15:38:53 +11:00
// Take care of degenerate quadratic cases. We can also pass off if 'd'
// is zero, but the benefit isn't clear given we have to merge results
// at the end anyway.
2015-01-21 23:40:45 +11:00
if (almost_zero (_a)) {
2015-07-06 21:41:03 +10:00
auto s = roots<2> ({_b, _c, _d});
2015-01-21 23:40:45 +11:00
return {s[0], s[1], std::numeric_limits<float>::quiet_NaN () };
}
2015-01-29 15:38:07 +11:00
std::array<float,3> s;
2015-01-21 23:40:45 +11:00
// Normalise to x^3 + ax^2 + bx + c = 0
const float a = _b / _a;
const float b = _c / _a;
const float c = _d / _a;
// Substituate x = y - a / 3 to eliminate the quadric. Now: x^3 + px + q = 0
2015-01-29 15:37:32 +11:00
const float p = (-a * a / 3.f + b) / 3.f;
const float q = (2 * a * a * a / 27.f - a * b /3.f + c) / 2.f;
2015-01-21 23:40:45 +11:00
2015-01-29 15:37:32 +11:00
// Polynomial descriminant
const float D = q * q + p * p * p;
2015-01-21 23:40:45 +11:00
2015-01-29 15:37:32 +11:00
// Solve using Cardano's method
2015-01-21 23:40:45 +11:00
if (almost_zero (D))
{
if (almost_zero (q)) {
s[0] = 0.f;
2015-01-29 15:38:07 +11:00
s[1] = std::numeric_limits<float>::quiet_NaN ();
s[2] = std::numeric_limits<float>::quiet_NaN ();
2015-01-21 23:40:45 +11:00
} else {
2015-01-29 15:37:32 +11:00
const float u = std::cbrt (-q);
2015-01-21 23:40:45 +11:00
s[0] = 2 * u;
s[1] = -u;
2015-01-29 15:38:07 +11:00
s[2] = std::numeric_limits<float>::quiet_NaN ();
2015-01-21 23:40:45 +11:00
}
} else if (D < 0) {
2015-01-29 15:37:32 +11:00
const float phi = std::acos (-q / std::sqrt (-p * p * p)) / 3.f;
const float t = 2 * std::sqrt (-p);
2015-01-21 23:40:45 +11:00
s[0] = t * std::cos (phi);
s[1] = -t * std::cos (phi + PI<float> / 3.f);
s[2] = -t * std::cos (phi - PI<float> / 3.f);
2015-01-21 23:40:45 +11:00
} else {
2015-01-29 15:38:07 +11:00
float u = std::cbrt (std::sqrt (D) + abs (q));
2015-01-21 23:40:45 +11:00
if (q > 0.f)
s[0] = -u + p / u;
else
s[0] = u - p / u;
2015-01-29 15:38:07 +11:00
s[1] = std::numeric_limits<float>::quiet_NaN ();
s[2] = std::numeric_limits<float>::quiet_NaN ();
2015-01-21 23:40:45 +11:00
}
// Resubstitute a / 3 from above
2015-01-29 15:37:32 +11:00
const float sub = a / 3.f;
2015-01-21 23:40:45 +11:00
for (auto &i: s)
i -= sub;
// Run some iterations of Newtons method to make the results slightly
// more accurate, they're a little loose straight out of the bat.
const float da = 3 * _a;
const float db = 2 * _b;
const float dc = 1 * _c;
for (auto &i: s) {
for (size_t j = 0; j < NEWTON_ITERATIONS; ++j) {
float deriv = da * i * i + db * i + dc;
if (almost_zero (deriv))
continue;
i = i - eval (coeffs, i) / deriv;
}
}
2015-01-21 23:40:45 +11:00
return s;
}
2016-12-21 20:21:28 +11:00
}