peasy.InterpolationUtil.java Source code

Introduction

Here is the source code for peasy.InterpolationUtil.java

Source

/*
   The PeasyCam Processing library, which provides an easy-peasy
   camera for 3D sketching.
      
   Copyright 2008 Jonathan Feinberg
    
   Licensed 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 peasy;

import peasy.org.apache.commons.math.geometry.Rotation;
import peasy.org.apache.commons.math.geometry.Vector3D;

public class InterpolationUtil {

    // Thanks to Michael Kaufmann <mail@michael-kaufmann.ch> for improvements to this function.
    static public Rotation slerp(final Rotation a, final Rotation b, final double t) {
        final double a0 = a.getQ0(), a1 = a.getQ1(), a2 = a.getQ2(), a3 = a.getQ3();
        double b0 = b.getQ0(), b1 = b.getQ1(), b2 = b.getQ2(), b3 = b.getQ3();

        double cosTheta = a0 * b0 + a1 * b1 + a2 * b2 + a3 * b3;
        if (cosTheta < 0) {
            b0 = -b0;
            b1 = -b1;
            b2 = -b2;
            b3 = -b3;
            cosTheta = -cosTheta;
        }

        final double theta = Math.acos(cosTheta);
        final double sinTheta = Math.sqrt(1.0 - cosTheta * cosTheta);

        double w1, w2;
        if (sinTheta > 0.001) {
            w1 = Math.sin((1.0 - t) * theta) / sinTheta;
            w2 = Math.sin(t * theta) / sinTheta;
        } else {
            w1 = 1.0 - t;
            w2 = t;
        }
        return new Rotation(w1 * a0 + w2 * b0, w1 * a1 + w2 * b1, w1 * a2 + w2 * b2, w1 * a3 + w2 * b3, true);
    }

    static public double smooth(final double a, final double b, final double t) {
        final double smooth = (t * t * (3 - 2 * t));
        return (b * smooth) + (a * (1 - smooth));

    }

    static public Vector3D smooth(final Vector3D a, final Vector3D b, final double t) {
        return new Vector3D(smooth(a.getX(), b.getX(), t), smooth(a.getY(), b.getY(), t),
                smooth(a.getZ(), b.getZ(), t));
    }

    static public double linear(final double a, final double b, final double t) {
        return a + (b - a) * t;
    }

    static public Vector3D linear(final Vector3D a, final Vector3D b, final double t) {
        return new Vector3D(linear(a.getX(), b.getX(), t), linear(a.getY(), b.getY(), t),
                linear(a.getZ(), b.getZ(), t));
    }
}