If you have gnuplot, you can use it to show how badly 1-cos(x) degrades when calculated naively, and that the latter formulation behaves well (all the way down to the smallest expressible numbers, which, on my computer, is the same in gnuplot as for doubles in a C/C++ program):
% gnuplot gnuplot> plot [-6:6] 1-cos(x), 2*sin(.5*x)**2 gnuplot> plot [-4e-8:4e-8] 1-cos(x), 2*sin(.5*x)**2 gnuplot> plot [-1e-152:1e-152] 1-cos(x), 2*sin(.5*x)**2Note also that it's not just for tiny x that 1-cos(x) catastrophically loses precision, it's near any multiple of 2*pi:
gnuplot> plot [2*pi-4e-8:2*pi+4e-8] 1-cos(x), 2*sin(.5*x)**2
gnuplot> plot [0:2] acos(1-x), 2*asin(sqrt(.5*x)) gnuplot> plot [0:1e-15] acos(1-x), 2*asin(sqrt(.5*x)) gnuplot> plot [0:1e-304] acos(1-x), 2*asin(sqrt(.5*x))
tanh(x/2) = (e^{x/2}-e^{-x/2})/(e^{x/2}+e^{-x/2}) = (e^{x}-1)/(e^{x}+1)so
e^{x}-1 = tanh(x/2) (e^{x}+1)Or, use the following C code (I found this, with analysis, in William Kahan's web page http://www.cs.berkeley.edu/~wkahan/Math128/Sumnfp.pdf ; it achieves “nearly full working relative accuracy despite cancellation no matter how tiny x may be”):
#include(XXX note, the above can be easily modified to get a nice expm1_over_x function as well; this is mentioned and analyzed in Nicholas J. Higham's book Accuracy and Stability of Numerical Algorithms, section 1.14.1, with attribution to Kahan)double expm1(double x) { double u = exp(x); if (u == 1.) return x; if (u-1. == -1.) return -1.; return (u-1.)*x/log(u); /* where log is natural logarithm */ }
ln(1+x) = 2*atanh(x/(x+2))Or, use the following, again due to Kahan (he's the undisputed master of this stuff):
#include(XXX just like the expm1 function mentioned above, this can be easily modified to get a nice log1p_over_x function)double log1p(double x) { double u = 1.+x; if (u == 1.) return x; else return log(u)*x/(u-1.); }
tanh(x) = (e^{x}-e^{-x}) / (e^{x}+e^{-x}) = (e^{2*x}-1) / (e^{2*x}+1) = expm1(2*x) / (expm1(2*x) + 2)(note that only one call of the expm1 function is required here).
tanh^{-1}(x) = log((1+x)/(1-x)) / 2 = log((1-x+2*x)/(1-x)) / 2 = log(1 + 2*x/(1-x)) / 2 = log1p(2*x/(1-x)) / 2
sinh(x) = (e^{x} - e^{-x}) / 2 = (e^{x}-1) * (e^{x}+1) / e^{x} / 2 = expm1(x) * (expm1(x)+2) / (expm1(x)+1) / 2To avoid overflow, we can re-order it as follows:
double u = expm1(x); return .5 * u / (u+1.) * (u+2.);
sinh^{-1}(x) = log(x + sqrt(x^{2}+1)) = log1p(x + sqrt(x^{2}+1) - 1) = log1p(x + x^{2}/(sqrt(x^{2}+1)+1) (from "sqrt(1+x)-1", below) = log1p(x * (1 + x/(sqrt(x^{2}+1)+1)))
sqrt(1+x)-1 = (sqrt(1+x)-1) * 1 = (sqrt(1+x)-1) * ((sqrt(1+x)+1) / (sqrt(1+x)+1)) = ((sqrt(1+x)-1) * (sqrt(1+x)+1)) / (sqrt(1+x)+1)) = (1+x - 1) / (sqrt(1+x)+1)) = x / (sqrt(1+x)+1)
1-sqrt(1-x) = -(sqrt(1-x)-1) = -(sqrt(1+y)-1) where y = -x = - y / (sqrt(1+y)+1) by the previous result = x / (sqrt(1-x)+1)
(1+x)^{2}-1 = 1 + 2*x + x^{2} - 1 = 2*x + x^{2} = x * (2 + x)
1-(1-x)^{2} = 1 - (1 - 2*x + x^{2}) = 2*x - x^{2} = x * (2 - x)
But do we want to make a special case for very small x, to avoid taking the ratio of two tiny numbers?
Assuming a well-behaved implementation of sin(x), it will actually return x for sufficiently small x, down to the smallest expressible floating-point number, so the following is probably okay, uningeneous as it may seem:
(1) if (x == 0.) return 1.; else return sin(x)/x;However, I'm not sure I trust sin(x) down that small, and furthermore the answer is indistinguishable from 1 long before we get anywhere close to the smallest representable number, so let's see how far we can go in the other direction, i.e. how large x can be before it becomes necessary to actually calculate sin(x)/x. Note that the power series expansion of sin(x)/x is:
sin(x)/x = 1 - x^{2}/3! + x^{4}/5! - x^{6}/7! + ...so if x is small enough so that 1-x^{2}/3! is indistinguishable from 1, then sin(x)/x will be indistinguishable from 1 anyway. So we can say:
(2) if (1. + x*x*(1./6.) == 1.) return 1.; else return sin(x)/x;Since there is a lot of slack between (1) and (2) (either of which should theoretically work), I suggest the following very simple test:
if (1. + x*x == 1.) return 1.; else return sin(x)/x;
(1-cos(x))/x = x/2! - x^{3}/4! + x^{5}/6! - x^{7}/8! + ...so if x is so small that x/2 - x^{3}/24 is indistinguishable from x/2, then the result will be indistinguishable from x/2. This is pretty much the same as 1/2 - x^{2}/24 being indistinguishable from 1/2, but maybe off by as much as 1 ULP (if x is just on the wrong side of a power of 2, or something) so we could be conservative and test whether 1/2 - x^{2}/12 is indistinguishable from 1/2 instead. This is exactly the same as asking whether 1 - x^{2}/6 is indistinguishable from 1 (coincidentally, the exact same condition we arrived at for sin(x)/x above). So the simpler (and slightly harder to satisfy) test should be fine:
if (1. + x*x == 1.) return .5*x; else return cosf1(x)/x; // where cosf1(x) is the robust implementation // of 1-cos(x) described earlier
The following works well:
2*atan2(|u-v|/2, |u+v|/2) = 2*atan2(|u-v|, |u+v|)however, it ends up requiring more square roots than necessary, so use this instead.
if (dot(u,v) < 0.) return M_PI - 2*asin(|-v-u|/2) else return 2*asin(|v-u|/2)
sin((1-t)*Ang) sin(t*Ang) (1) slerp(v_{0},v_{1},t) = ----------------*v_{0} + -----------*v_{1} sin(Ang) sin(Ang)where Ang is the angle between v_{0} and v_{1}.
This is very elegant, but there are at least three things to watch out for:
sin((1-t)*Ang)/((1-t)*Ang) sin(t*Ang)/(t*Ang) slerp(v_{0},v_{1},t) = ----------------------------*(1-t)*v_{0} + --------------------*t*v_{0} sin(Ang)/Ang sin(Ang)/Ang sin_over_x((1-t)*Ang) sin_over_x(t*Ang) (2) = -----------------------*(1-t)*v_{0} + -------------------*t*v_{1} sin_over_x(Ang) sin_over_x(Ang)where sin_over_x(x) is the robust implementation of sin(x)/x described earlier on this page. Note that this final expression behaves very nicely when Ang is close to 0: all the sin_over_x's approach 1, and so the whole expression approaches (1-t)*v_{0} + t*v_{1}, i.e. linear interpolation, as it should.
In this case we can pick an arbitrary vector v_{.5} perpendicular to v_{0} (and hence v_{1}), and rewrite the problem as:
(3) slerp(v_{0},v_{1},t) = slerp(v_{0},v_{.5},2*t), if t < .5 slerp(v_{.5},v_{1},2*(t-.5)) if t >= .5.Unfortunately I don't see a graceful way to ease into this case (among other problems, it's impossible to come up with a continuous vector function f(v) from the set of all unit vectors to itself such that f(v) is perpendicular to v for all v; this is the unsolvable “combing a fuzzy sphere” problem). The consequences of using (2) when v_{0} and v_{1} differ by close to 180 degrees can be unstable arithmetic, resulting in answers that are truly bad (i.e. don't represent *any* of the infinitely many legitimate spherical linear paths from v_{0} to v_{1}), so we would do well to switch to avoid it in favor of method (3) fairly early. The best I can suggest is to use a semi-arbitrary threshold test, e.g. if (1. + |v_{1}+v_{0}|^{4} == 1.) then use (3). (On my computer, using double-precision arithmetic, this is equivalent to roughly |v_{1}+v_{0}| < 1e-5 or so.) Note that splitting (3) into two cases guarantees that the beginning and end of the path will be v_{0} and v_{1} respectively (whereas just using slerp(v_{0},v_{.5},2*t) may result in missing v_{1} by more than necessary).
(XXX actually I think we can do a bit better than this by *always* using (3),
with a vector v_{.5} perpendicular to v_{0} and v_{1},
chosen using some clever application of atan2 that gracefully degrades
into semi-randomness but still always produces a vector exactly
half way between v_{0} and v_{1}.)
(XXX Or, come up with a vector 90 degrees away from v_{0}
in the direction of v_{1} (gracefully degrading into some random direction
when v_{1} = v_{0} or -v_{0}) and slerp between v_{0} and that instead of v_{0} and v_{1}.
But then the clever equations (1) and (2) become completely
irrelevant :-) Actually, I think this is the right way to do it
and this section should be rewritten...)
Fortunately for qslerp this case doesn't happen in practice: if q is a unit quaternion then -q represents the same orientation as q, so when qslerping from q_{0} to q_{1} we usually negate either q_{0} or q_{1} if their dot product is < 0 (i.e. when their angular distance on the 4D sphere is > 90 degrees, i.e. their distance as a 3D rotation is > 180 degrees around some axis), and then (2) becomes stable.
For definiteness, let's consider heading/pitch/roll Euler angles (or yaw/pitch/roll if you like). The subtlety occurs when the pitch is 90 degrees (or close to it): in this case any heading value could alternatively be expressed as roll, and vice-versa. So, if you are asked to decompose a rotation (matrix) in which the pitch is 90 degrees, you can choose the heading arbitrarily and then choose the roll in terms of it (or vice-versa).
Notice that in this case there is one degree of freedom in your answer, but not two! So if you have determined that the pitch is 90 degrees (or sufficiently close), you can't just throw your hands up in the air and return arbitrary numbers (e.g. zero) for both heading and roll; in general, you won't get a representation of the original matrix, which would be a bad bug in your implementation. You can return an arbitrary number for either heading or roll, but then that determines the other.
A good robust way do do this is to start by choosing the heading (or roll) using the atan2 function as usual. Note that if the pitch is 90 degrees or close to it, then you will be taking the atan2 of two numbers very close (or equal) to zero, which is unstable of course, but the worst atan2 can do is return a random angle, which is fine, and even appropriate, in this case. The next step is to create a rotation matrix representing the inverse of the heading (or roll) you have chosen, and multiply it by the original matrix on the appropriate side, effectively factoring out your chosen heading (or roll). Now you are left with a simpler matrix, from which you can robustly extract the remaining two euler angles, and when you compose them all together you are guaranteed to get back the original matrix.
Last Modified: Fri Jul 4 02:17:56 PDT 2003 Don Hatch hatch@plunk.org |