| /* |
| * Licensed to the Apache Software Foundation (ASF) under one or more |
| * contributor license agreements. See the NOTICE file distributed with |
| * this work for additional information regarding copyright ownership. |
| * The ASF licenses this file to You 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 |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * 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. |
| */ |
| package org.apache.commons.numbers.quaternion; |
| |
| import java.util.function.DoubleFunction; |
| |
| /** |
| * Perform spherical linear interpolation (<a href="https://en.wikipedia.org/wiki/Slerp">Slerp</a>). |
| * |
| * The <em>Slerp</em> algorithm is designed to interpolate smoothly between |
| * two rotations/orientations, producing a constant-speed motion along an arc. |
| * The original purpose of this algorithm was to animate 3D rotations. All output |
| * quaternions are in positive polar form, meaning a unit quaternion with a positive |
| * scalar component. |
| */ |
| public class Slerp implements DoubleFunction<Quaternion> { |
| /** |
| * Threshold max value for the dot product. |
| * If the quaternion dot product is greater than this value (i.e. the |
| * quaternions are very close to each other), then the quaternions are |
| * linearly interpolated instead of spherically interpolated. |
| */ |
| private static final double MAX_DOT_THRESHOLD = 0.9995; |
| /** Start of the interpolation. */ |
| private final Quaternion start; |
| /** End of the interpolation. */ |
| private final Quaternion end; |
| /** Linear or spherical interpolation algorithm. */ |
| private final DoubleFunction<Quaternion> algo; |
| |
| /** |
| * @param start Start of the interpolation. |
| * @param end End of the interpolation. |
| */ |
| public Slerp(Quaternion start, |
| Quaternion end) { |
| this.start = start.positivePolarForm(); |
| |
| final Quaternion e = end.positivePolarForm(); |
| double dot = this.start.dot(e); |
| |
| // If the dot product is negative, then the interpolation won't follow the shortest |
| // angular path between the two quaterions. In this case, invert the end quaternion |
| // to produce an equivalent rotation that will give us the path we want. |
| if (dot < 0) { |
| dot = -dot; |
| this.end = e.negate(); |
| } else { |
| this.end = e; |
| } |
| |
| algo = dot > MAX_DOT_THRESHOLD ? |
| new Linear() : |
| new Spherical(dot); |
| } |
| |
| /** |
| * Performs the interpolation. |
| * The rotation returned by this method is controlled by the interpolation parameter, {@code t}. |
| * All other values are interpolated (or extrapolated if {@code t} is outside of the |
| * {@code [0, 1]} range). The returned quaternion is in positive polar form, meaning that it |
| * is a unit quaternion with a positive scalar component. |
| * |
| * @param t Interpolation control parameter. |
| * When {@code t = 0}, a rotation equal to the start instance is returned. |
| * When {@code t = 1}, a rotation equal to the end instance is returned. |
| * @return an interpolated quaternion in positive polar form. |
| */ |
| @Override |
| public Quaternion apply(double t) { |
| // Handle no-op cases. |
| if (t == 0) { |
| return start; |
| } else if (t == 1) { |
| // Call to "positivePolarForm()" is required because "end" might |
| // not be in positive polar form. |
| return end.positivePolarForm(); |
| } |
| |
| return algo.apply(t); |
| } |
| |
| /** |
| * Linear interpolation, used when the quaternions are too closely aligned. |
| */ |
| private class Linear implements DoubleFunction<Quaternion> { |
| /** {@inheritDoc} */ |
| @Override |
| public Quaternion apply(double t) { |
| final double f = 1 - t; |
| return Quaternion.of(f * start.getW() + t * end.getW(), |
| f * start.getX() + t * end.getX(), |
| f * start.getY() + t * end.getY(), |
| f * start.getZ() + t * end.getZ()).positivePolarForm(); |
| } |
| } |
| |
| /** |
| * Spherical interpolation, used when the quaternions are too closely aligned. |
| * When we may end up dividing by zero (cf. 1/sin(theta) term below). |
| * {@link Linear} interpolation must be used. |
| */ |
| private class Spherical implements DoubleFunction<Quaternion> { |
| /** Angle of rotation. */ |
| private final double theta; |
| /** Sine of {@link #theta}. */ |
| private final double sinTheta; |
| |
| /** |
| * @param dot Dot product of the start and end quaternions. |
| */ |
| Spherical(double dot) { |
| theta = Math.acos(dot); |
| sinTheta = Math.sin(theta); |
| } |
| |
| /** {@inheritDoc} */ |
| @Override |
| public Quaternion apply(double t) { |
| final double f1 = Math.sin((1 - t) * theta) / sinTheta; |
| final double f2 = Math.sin(t * theta) / sinTheta; |
| |
| return Quaternion.of(f1 * start.getW() + f2 * end.getW(), |
| f1 * start.getX() + f2 * end.getX(), |
| f1 * start.getY() + f2 * end.getY(), |
| f1 * start.getZ() + f2 * end.getZ()).positivePolarForm(); |
| } |
| } |
| } |