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     * Create an instance.
047     *
048     * @param start Start of the interpolation.
049     * @param end End of the interpolation.
050     */
051    public Slerp(Quaternion start,
052                 Quaternion end) {
053        this.start = start.positivePolarForm();
054
055        final Quaternion e = end.positivePolarForm();
056        double dot = this.start.dot(e);
057
058        // If the dot product is negative, then the interpolation won't follow the shortest
059        // angular path between the two quaterions. In this case, invert the end quaternion
060        // to produce an equivalent rotation that will give us the path we want.
061        if (dot < 0) {
062            dot = -dot;
063            this.end = e.negate();
064        } else {
065            this.end = e;
066        }
067
068        algo = dot > MAX_DOT_THRESHOLD ?
069            new Linear() :
070            new Spherical(dot);
071    }
072
073    /**
074     * Performs the interpolation.
075     * The rotation returned by this method is controlled by the interpolation parameter, {@code t}.
076     * All other values are interpolated (or extrapolated if {@code t} is outside of the
077     * {@code [0, 1]} range). The returned quaternion is in positive polar form, meaning that it
078     * is a unit quaternion with a positive scalar component.
079     *
080     * @param t Interpolation control parameter.
081     * When {@code t = 0}, a rotation equal to the start instance is returned.
082     * When {@code t = 1}, a rotation equal to the end instance is returned.
083     * @return an interpolated quaternion in positive polar form.
084     */
085    @Override
086    public Quaternion apply(double t) {
087        // Handle no-op cases.
088        if (t == 0) {
089            return start;
090        } else if (t == 1) {
091            // Call to "positivePolarForm()" is required because "end" might
092            // not be in positive polar form.
093            return end.positivePolarForm();
094        }
095
096        return algo.apply(t);
097    }
098
099    /**
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}