MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
hermite.cc
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann
3 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
4 * All rights reserved.
5 *
6 * This software may be modified and distributed under the terms
7 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
8 */
9
10#include <algorithm>
11#include <cmath>
12#include <sstream>
13#include <stdexcept>
14
15#include "fssr/hermite.h"
16
17#define EPSILON 1e-8
18
20
21double
22find_root_linear (double a0, double a1)
23{
24 return -a0 / a1;
25}
26
27double
28find_root_square (double a0, double a1, double a2)
29{
30 if (a2 > -EPSILON && a2 < EPSILON)
31 return find_root_linear(a0, a1);
32
33 /* Normalize polynomial. */
34 a0 = a0 / a2;
35 a1 = a1 / a2;
36
37 /* Solve using PQ formula. */
38 double const pq1 = -a1 / 2.0;
39 double const pq2 = std::sqrt(std::max(0.0, a1 * a1 / 4.0 - a0));
40 double const sol1 = pq1 + pq2;
41 double const sol2 = pq1 - pq2;
42
43 /* Select root which is closer to 0.5. */
44 if (std::abs(sol1 - 0.5) < std::abs(sol2 - 0.5))
45 return sol1;
46 else
47 return sol2;
48}
49
50/*
51 * The idea is inspired from:
52 * http://tavianator.com/solving-cubic-polynomials/
53 */
54double
55find_root_cubic (double a0, double a1, double a2, double a3)
56{
57 if (a3 > -EPSILON && a3 < EPSILON)
58 return find_root_square(a0, a1, a2);
59
60 /* Normalize polynomial. */
61 a0 = a0 / a3;
62 a1 = a1 / a3;
63 a2 = a2 / a3;
64
65 /* Reduce to depressed cubic t^3 + pt + q using x = t - a2 / (3 a3). */
66 double const p = a1 - a2 * a2 / 3.0;
67 double const q = a0 - a1 * a2 / 3.0 + a2 * a2 * a2 / 13.5;
68 double const discr = 4.0 * p * p * p + 27.0 * q * q;
69
70 if (discr < 0.0)
71 {
72 /* Three real roots. */
73 double const sqrtp3 = std::sqrt(-p / 3.0);
74 double const theta = std::acos(3.0 * q / (-2.0 * p * sqrtp3)) / 3.0;
75 double root[3];
76 root[2] = -2.0 * sqrtp3 * std::cos(theta) - a2 / 3.0;
77 root[0] = 2.0 * sqrtp3 * std::cos(4.0 * std::atan(1.0) / 3.0 - theta) - a2 / 3.0;
78 root[1] = -(root[0] + root[2] + a2);
79
80 /* Check how many roots are in [0, 1]. */
81 double the_root = 0.0;
82 int num_roots = 0;
83 for (int i = 0; i < 3; ++i)
84 if (root[i] >= 0.0 && root[i] <= 1.0)
85 {
86 the_root = root[i];
87 num_roots += 1;
88 }
89
90 if (num_roots == 1)
91 {
92 //std::cout << "One of three roots in range" << std::endl;
93 return the_root;
94 }
95 else
96 {
97#if 1
98 /* Return middle root. */
99 return root[1];
100#else
101 /* Find linear interpolant. */
102 double const v0 = a0 * a3;
103 double const v1 = (a0 + a1 + a2 + 1.0) * a3;
104 return find_root_linear(v0, v1 - v0);
105#endif
106 }
107 }
108 else if (discr > 0.0)
109 {
110 /* One real root. Return it. */
111 //double const c = std::cbrt(std::sqrt(disc / 108.0) + std::abs(q) / 2.0);
112 double const c = std::pow(std::sqrt(discr / 108.0) + std::abs(q) / 2.0, 1.0 / 3.0);
113 double const t = c - p / (3.0 * c);
114
115 if (q >= 0.0)
116 {
117 //std::cout << "One real root 1" << std::endl;
118 return -t - a2 / 3.0;
119 }
120 else
121 {
122 //std::cout << "One real root 2" << std::endl;
123 return t - a2 / 3.0;
124 }
125 }
126 else
127 {
128 /* One duplicate and one single root. Extract single root only. */
129 if (p > -EPSILON && p < EPSILON)
130 {
131 //std::cout << "Mixed case 1" << std::endl;
132 return a2 / 3.0;
133 }
134 else
135 {
136 //std::cout << "Mixed case 1" << std::endl;
137 return 3.0 * q / p;
138 }
139 }
140
141 throw std::runtime_error("Unreachable code");
142}
143
144double
145interpolate_root (double v0, double v1, double d0, double d1,
147{
148 double root = 0.0;
149 double a0 = 0.0, a1 = 0.0, a2 = 0.0, a3 = 0.0;
150 switch (type)
151 {
153 {
154 a0 = v0;
155 a1 = v1 - v0;
156 root = find_root_linear(a0, a1);
157 break;
158 }
160 {
161 double scale = 2.0 * (v1 - v0) / (d0 + d1);
162 a0 = v0;
163 a1 = d0 * scale;
164 a2 = 3.0 * (v1 - v0) - (2.0 * d0 + d1) * scale;
165 root = find_root_square(a0, a1, a2);
166 break;
167 }
169 {
170 a0 = v0;
171 a1 = (d0 - d1) / 2.0 + v1 - v0;
172 a2 = (d1 - d0) / 2.0;
173 root = find_root_square(a0, a1, a2);
174 break;
175 }
177 {
178 a0 = v0;
179 a1 = d0;
180 a2 = 3.0 * v1 - 3.0 * v0 - 2.0 * d0 - 1.0 * d1;
181 a3 = 2.0 * v0 - 2.0 * v1 + d0 + d1;
182 root = find_root_cubic(a0, a1, a2, a3);
183 break;
184 }
185 default:
186 throw std::runtime_error("Invalid interpolation type");
187 }
188
189 return std::min(1.0, std::max(0.0, root));
190}
191
#define FSSR_NAMESPACE_END
Definition defines.h:14
#define FSSR_NAMESPACE_BEGIN
Definition defines.h:13
#define EPSILON
Definition hermite.cc:17
InterpolationType
Definition hermite.h:18
@ INTERPOLATION_CUBIC
Definition hermite.h:22
@ INTERPOLATION_LSDERIV
Definition hermite.h:21
@ INTERPOLATION_SCALING
Definition hermite.h:20
@ INTERPOLATION_LINEAR
Definition hermite.h:19
double find_root_square(double a0, double a1, double a2)
Finds the root of a quadratic function f(x) = a0 + a1 * x + a2 * x^2.
Definition hermite.cc:28
double interpolate_root(double v0, double v1, double d0, double d1, InterpolationType type)
Interpolates the root of an unknown function f(x) given value and derivative constraints: f(0) = v0,...
Definition hermite.cc:145
double find_root_linear(double a0, double a1)
Finds the root of a linear function f(x) = a0 + a1(x).
Definition hermite.cc:22
double find_root_cubic(double a0, double a1, double a2, double a3)
Finds the root of a cubic function f(x) = a0 + a1 * x + a2 * x^2 + a3 * x^3.
Definition hermite.cc:55