001/*
002 * Licensed to the Apache Software Foundation (ASF) under one or more
003 * contributor license agreements.  See the NOTICE file distributed with
004 * this work for additional information regarding copyright ownership.
005 * The ASF licenses this file to You under the Apache License, Version 2.0
006 * (the "License"); you may not use this file except in compliance with
007 * the License.  You may obtain a copy of the License at
008 *
009 *      http://www.apache.org/licenses/LICENSE-2.0
010 *
011 * Unless required by applicable law or agreed to in writing, software
012 * distributed under the License is distributed on an "AS IS" BASIS,
013 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
014 * See the License for the specific language governing permissions and
015 * limitations under the License.
016 */
017package org.apache.commons.numbers.quaternion;
018
019import java.util.function.DoubleFunction;
020
021/**
022 * Perform spherical linear interpolation (<a href="https://en.wikipedia.org/wiki/Slerp">Slerp</a>).
023 *
024 * The <em>Slerp</em> algorithm is designed to interpolate smoothly between
025 * two rotations/orientations, producing a constant-speed motion along an arc.
026 * The original purpose of this algorithm was to animate 3D rotations. All output
027 * quaternions are in positive polar form, meaning a unit quaternion with a positive
028 * scalar component.
029 */
030public class Slerp implements DoubleFunction<Quaternion> {
031    /**
032     * Threshold max value for the dot product.
033     * If the quaternion dot product is greater than this value (i.e. the
034     * quaternions are very close to each other), then the quaternions are
035     * linearly interpolated instead of spherically interpolated.
036     */
037    private static final double MAX_DOT_THRESHOLD = 0.9995;
038    /** Start of the interpolation. */
039    private final Quaternion start;
040    /** End of the interpolation. */
041    private final Quaternion end;
042    /** Linear or spherical interpolation algorithm. */
043    private final DoubleFunction<Quaternion> algo;
044
045    /**
046     * @param start Start of the interpolation.
047     * @param end End of the interpolation.
048     */
049    public Slerp(Quaternion start,
050                 Quaternion end) {
051        this.start = start.positivePolarForm();
052
053        final Quaternion e = end.positivePolarForm();
054        double dot = this.start.dot(e);
055
056        // If the dot product is negative, then the interpolation won't follow the shortest
057        // angular path between the two quaterions. In this case, invert the end quaternion
058        // to produce an equivalent rotation that will give us the path we want.
059        if (dot < 0) {
060            dot = -dot;
061            this.end = e.negate();
062        } else {
063            this.end = e;
064        }
065
066        algo = dot > MAX_DOT_THRESHOLD ?
067            new Linear() :
068            new Spherical(dot);
069    }
070
071    /**
072     * Performs the interpolation.
073     * The rotation returned by this method is controlled by the interpolation parameter, {@code t}.
074     * All other values are interpolated (or extrapolated if {@code t} is outside of the
075     * {@code [0, 1]} range). The returned quaternion is in positive polar form, meaning that it
076     * is a unit quaternion with a positive scalar component.
077     *
078     * @param t Interpolation control parameter.
079     * When {@code t = 0}, a rotation equal to the start instance is returned.
080     * When {@code t = 1}, a rotation equal to the end instance is returned.
081     * @return an interpolated quaternion in positive polar form.
082     */
083    @Override
084    public Quaternion apply(double t) {
085        // Handle no-op cases.
086        if (t == 0) {
087            return start;
088        } else if (t == 1) {
089            // Call to "positivePolarForm()" is required because "end" might
090            // not be in positive polar form.
091            return end.positivePolarForm();
092        }
093
094        return algo.apply(t);
095    }
096
097    /**
098     * Linear interpolation, used when the quaternions are too closely aligned.
099     */
100    private class Linear implements DoubleFunction<Quaternion> {
101        /** {@inheritDoc} */
102        @Override
103        public Quaternion apply(double t) {
104            final double f = 1 - t;
105            return Quaternion.of(f * start.getW() + t * end.getW(),
106                                 f * start.getX() + t * end.getX(),
107                                 f * start.getY() + t * end.getY(),
108                                 f * start.getZ() + t * end.getZ()).positivePolarForm();
109        }
110    }
111
112    /**
113     * Spherical interpolation, used when the quaternions are too closely aligned.
114     * When we may end up dividing by zero (cf. 1/sin(theta) term below).
115     * {@link Linear} interpolation must be used.
116     */
117    private class Spherical implements DoubleFunction<Quaternion> {
118        /** Angle of rotation. */
119        private final double theta;
120        /** Sine of {@link #theta}. */
121        private final double sinTheta;
122
123        /**
124         * @param dot Dot product of the start and end quaternions.
125         */
126        Spherical(double dot) {
127            theta = Math.acos(dot);
128            sinTheta = Math.sin(theta);
129        }
130
131        /** {@inheritDoc} */
132        @Override
133        public Quaternion apply(double t) {
134            final double f1 = Math.sin((1 - t) * theta) / sinTheta;
135            final double f2 = Math.sin(t * theta) / sinTheta;
136
137            return Quaternion.of(f1 * start.getW() + f2 * end.getW(),
138                                 f1 * start.getX() + f2 * end.getX(),
139                                 f1 * start.getY() + f2 * end.getY(),
140                                 f1 * start.getZ() + f2 * end.getZ()).positivePolarForm();
141        }
142    }
143}