1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * The ASF licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.apache.commons.numbers.quaternion;
18
19 import java.util.function.DoubleFunction;
20
21 /**
22 * Perform spherical linear interpolation (<a href="https://en.wikipedia.org/wiki/Slerp">Slerp</a>).
23 *
24 * The <em>Slerp</em> algorithm is designed to interpolate smoothly between
25 * two rotations/orientations, producing a constant-speed motion along an arc.
26 * The original purpose of this algorithm was to animate 3D rotations. All output
27 * quaternions are in positive polar form, meaning a unit quaternion with a positive
28 * scalar component.
29 */
30 public class Slerp implements DoubleFunction<Quaternion> {
31 /**
32 * Threshold max value for the dot product.
33 * If the quaternion dot product is greater than this value (i.e. the
34 * quaternions are very close to each other), then the quaternions are
35 * linearly interpolated instead of spherically interpolated.
36 */
37 private static final double MAX_DOT_THRESHOLD = 0.9995;
38 /** Start of the interpolation. */
39 private final Quaternion start;
40 /** End of the interpolation. */
41 private final Quaternion end;
42 /** Linear or spherical interpolation algorithm. */
43 private final DoubleFunction<Quaternion> algo;
44
45 /**
46 * Create an instance.
47 *
48 * @param start Start of the interpolation.
49 * @param end End of the interpolation.
50 */
51 public Slerp(Quaternion start,
52 Quaternion end) {
53 this.start = start.positivePolarForm();
54
55 final Quaternion e = end.positivePolarForm();
56 double dot = this.start.dot(e);
57
58 // If the dot product is negative, then the interpolation won't follow the shortest
59 // angular path between the two quaterions. In this case, invert the end quaternion
60 // to produce an equivalent rotation that will give us the path we want.
61 if (dot < 0) {
62 dot = -dot;
63 this.end = e.negate();
64 } else {
65 this.end = e;
66 }
67
68 algo = dot > MAX_DOT_THRESHOLD ?
69 new Linear() :
70 new Spherical(dot);
71 }
72
73 /**
74 * Performs the interpolation.
75 * The rotation returned by this method is controlled by the interpolation parameter, {@code t}.
76 * All other values are interpolated (or extrapolated if {@code t} is outside of the
77 * {@code [0, 1]} range). The returned quaternion is in positive polar form, meaning that it
78 * is a unit quaternion with a positive scalar component.
79 *
80 * @param t Interpolation control parameter.
81 * When {@code t = 0}, a rotation equal to the start instance is returned.
82 * When {@code t = 1}, a rotation equal to the end instance is returned.
83 * @return an interpolated quaternion in positive polar form.
84 */
85 @Override
86 public Quaternion apply(double t) {
87 // Handle no-op cases.
88 if (t == 0) {
89 return start;
90 } else if (t == 1) {
91 // Call to "positivePolarForm()" is required because "end" might
92 // not be in positive polar form.
93 return end.positivePolarForm();
94 }
95
96 return algo.apply(t);
97 }
98
99 /**
100 * Linear interpolation, used when the quaternions are too closely aligned.
101 */
102 private final class Linear implements DoubleFunction<Quaternion> {
103 /** Package-private constructor. */
104 Linear() {}
105
106 /** {@inheritDoc} */
107 @Override
108 public Quaternion apply(double t) {
109 final double f = 1 - t;
110 return Quaternion.of(f * start.getW() + t * end.getW(),
111 f * start.getX() + t * end.getX(),
112 f * start.getY() + t * end.getY(),
113 f * start.getZ() + t * end.getZ()).positivePolarForm();
114 }
115 }
116
117 /**
118 * Spherical interpolation, used when the quaternions are too closely aligned.
119 * When we may end up dividing by zero (cf. 1/sin(theta) term below).
120 * {@link Linear} interpolation must be used.
121 */
122 private final class Spherical implements DoubleFunction<Quaternion> {
123 /** Angle of rotation. */
124 private final double theta;
125 /** Sine of {@link #theta}. */
126 private final double sinTheta;
127
128 /**
129 * @param dot Dot product of the start and end quaternions.
130 */
131 Spherical(double dot) {
132 theta = Math.acos(dot);
133 sinTheta = Math.sin(theta);
134 }
135
136 /** {@inheritDoc} */
137 @Override
138 public Quaternion apply(double t) {
139 final double f1 = Math.sin((1 - t) * theta) / sinTheta;
140 final double f2 = Math.sin(t * theta) / sinTheta;
141
142 return Quaternion.of(f1 * start.getW() + f2 * end.getW(),
143 f1 * start.getX() + f2 * end.getX(),
144 f1 * start.getY() + f2 * end.getY(),
145 f1 * start.getZ() + f2 * end.getZ()).positivePolarForm();
146 }
147 }
148 }