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}