/*
 * $RCSfile: KBCubicSplineSegment.java,v $
 *
 * Copyright (c) 2007 Sun Microsystems, Inc. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * - Redistribution of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 * - Redistribution in binary form must reproduce the above copyright
 *   notice, this list of conditions and the following disclaimer in
 *   the documentation and/or other materials provided with the
 *   distribution.
 *
 * Neither the name of Sun Microsystems, Inc. or the names of
 * contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * This software is provided "AS IS," without a warranty of any
 * kind. ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND
 * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE OR NON-INFRINGEMENT, ARE HEREBY
 * EXCLUDED. SUN MICROSYSTEMS, INC. ("SUN") AND ITS LICENSORS SHALL
 * NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF
 * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS
 * DERIVATIVES. IN NO EVENT WILL SUN OR ITS LICENSORS BE LIABLE FOR
 * ANY LOST REVENUE, PROFIT OR DATA, OR FOR DIRECT, INDIRECT, SPECIAL,
 * CONSEQUENTIAL, INCIDENTAL OR PUNITIVE DAMAGES, HOWEVER CAUSED AND
 * REGARDLESS OF THE THEORY OF LIABILITY, ARISING OUT OF THE USE OF OR
 * INABILITY TO USE THIS SOFTWARE, EVEN IF SUN HAS BEEN ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGES.
 *
 * You acknowledge that this software is not designed, licensed or
 * intended for use in the design, construction, operation or
 * maintenance of any nuclear facility.
 *
 * $Revision: 1.4 $
 * $Date: 2007/02/09 17:20:11 $
 * $State: Exp $
 */

package com.sun.j3d.utils.behaviors.interpolators;

import javax.media.j3d.*;
import java.util.*;
import javax.vecmath.*;


/**
 * The KBCubicSplineSegment class creates the representation of a 
 * Kochanek-Bartel's (also known as the TCB or Tension-Continuity-Bias
 * Spline.  This class takes 4 key frames as its input (using KBKeyFrame). 
 * If interpolating between the i<sup>th</sup> and (i+1)<sup>th</sup> key 
 * frame then the four key frames that need to be specified are the 
 * (i-1)<sup>th</sup>, i<sup>th</sup>, (i+1)<sup>th</sup> 
 * and (i+2)<sup>th</sup>  keyframes in order. The KBCubicSegmentClass
 * then pre-computes the hermite interpolation basis coefficients if the 
 * (i+1)<sup>th</sup> frame has the linear flag set to zero. These are used to 
 * calculate the interpolated position, scale and quaternions when they 
 * requested by the user using the getInterpolated* methods. If the the 
 * (i+1)<sup>th</sup> frame's linear flag is set to 1 then the class uses 
 * linear interpolation to calculate the interpolated position, scale, heading 
 * pitch and bank it returns through the getInterpolated* methods. 
 *
 * @since Java3D 1.2
 */
public class KBCubicSplineSegment {

    // Legendre polynomial information for Gaussian quadrature of speed
    // for the domain [0,u], 0 <= u <= 1.

    // Legendre roots mapped to (root+1)/2
    static final double modRoot[] =
      {
       0.046910077,
       0.230765345,
       0.5,
       0.769234655,
       0.953089922
      };

    // original coefficients divided by 2
    static final double modCoeff[] =
      {
       0.118463442,
       0.239314335,
       0.284444444,
       0.239314335,
       0.118463442
      };

    // Key Frames
    KBKeyFrame[] keyFrame = new KBKeyFrame[4];
    
    // H.C 
    Point3f  c0, c1, c2, c3; // coefficients for position
    Point3f  e0, e1, e2, e3; // coefficients for scale 
    float    h0, h1, h2, h3; // coefficients for heading  
    float    p0, p1, p2, p3; // coefficients for pitch 
    float    b0, b1, b2, b3; // coefficients for bank 

    // variables for destination derivative
    float    one_minus_t_in; 
    float    one_minus_c_in;
    float    one_minus_b_in;
    float    one_plus_c_in;
    float    one_plus_b_in;
    float    ddb;
    float    dda;

    // variables for source derivative
    float    one_minus_t_out;
    float    one_minus_c_out;
    float    one_minus_b_out;
    float    one_plus_c_out; 
    float    one_plus_b_out;
    float    dsb;
    float    dsa;

    // Length of the spline segment
    float    length;

    // interpolation type
    int      linear;
    
    // Default constructor 
    KBCubicSplineSegment () {

        length = 0;

    }

    /**
     * Creates a cubic spline segment between two key frames using the 
     * key frames provided. If creating a spline between the ith frame and 
     * the (i+1)<sup>th</sup> frame then send down the (i - 1)<sup>th</sup>, 
     * i<sup>th</sup> , (i+1)<sup>th</sup> and the (i+2)<sup>th</sup> key 
     * frames.  
     * 
     * @param kf0 (i - 1)<sup>th</sup> Key Frame
     * @param kf1 i<sup>th</sup> Key Frame 
     * @param kf2 (i + 1)<sup>th</sup> Key Frame
     * @param kf3 (i + 2)<sup>th</sup> Key Frame 
     */

    KBCubicSplineSegment (KBKeyFrame kf0,  KBKeyFrame kf1, KBKeyFrame kf2,
                                                           KBKeyFrame kf3) {

        // Copy KeyFrame information
        keyFrame[0] = new KBKeyFrame(kf0); 
        keyFrame[1] = new KBKeyFrame(kf1); 
        keyFrame[2] = new KBKeyFrame(kf2); 
        keyFrame[3] = new KBKeyFrame(kf3); 

        // if linear interpolation is requested then just set linear flag
        // if spline interpolation is needed then compute spline coefficients
        if (kf2.linear == 1) {
            this.linear = 1;
        } else {
            this.linear = 0;
            computeCommonCoefficients (kf0, kf1, kf2, kf3);
            computeHermiteCoefficients (kf0, kf1, kf2, kf3);
        }

        length = computeLength (1.0f);
        // System.out.println ("Segment length = " + length);

    }

    // compute the common coefficients
    private void computeCommonCoefficients (KBKeyFrame kf0, 
                                            KBKeyFrame kf1,
                                            KBKeyFrame kf2, 
                                            KBKeyFrame kf3) {

        // variables for destination derivative
        float  one_minus_t_in = 1.0f - kf1.tension;
        float  one_minus_c_in = 1.0f - kf1.continuity; 
        float  one_minus_b_in = 1.0f - kf1.bias;
        float  one_plus_c_in  = 1.0f + kf1.continuity;
        float  one_plus_b_in  = 1.0f + kf1.bias;

        // coefficients for the incoming Tangent
        ddb = one_minus_t_in * one_minus_c_in * one_minus_b_in; 
        dda = one_minus_t_in * one_plus_c_in * one_plus_b_in; 

        // variables for source derivative
        float  one_minus_t_out = 1.0f - kf2.tension; 
        float  one_minus_c_out = 1.0f - kf2.continuity; 
        float  one_minus_b_out = 1.0f - kf2.bias;
        float  one_plus_c_out  = 1.0f + kf2.continuity;
        float  one_plus_b_out  = 1.0f + kf2.bias;
                  
        // coefficients for the outgoing Tangent
        dsb = one_minus_t_in * one_plus_c_in * one_minus_b_in; 
        dsa = one_minus_t_in * one_minus_c_in * one_plus_b_in; 
    }


    // compute the hermite interpolation basis coefficients
    private void computeHermiteCoefficients (KBKeyFrame kf0,
                                             KBKeyFrame kf1,
                                             KBKeyFrame kf2,
                                             KBKeyFrame kf3) {


        Point3f deltaP = new Point3f();
        Point3f deltaS = new Point3f();
        float   deltaH;
        float   deltaT;
        float   deltaB;

        // Find the difference in position and scale 
        deltaP.x = kf2.position.x - kf1.position.x;
        deltaP.y = kf2.position.y - kf1.position.y;
        deltaP.z = kf2.position.z - kf1.position.z;

        deltaS.x = kf2.scale.x - kf1.scale.x;
        deltaS.y = kf2.scale.y - kf1.scale.y;
        deltaS.z = kf2.scale.z - kf1.scale.z;

        // Find the difference in heading, pitch, and bank
        deltaH = kf2.heading - kf1.heading;
        deltaT = kf2.pitch - kf1.pitch;
        deltaB = kf2.bank - kf1.bank;
         
        // Incoming Tangent
        Point3f dd_pos    = new Point3f();
        Point3f dd_scale  = new Point3f();
        float   dd_heading, dd_pitch, dd_bank;

        // If this is the first keyframe of the animation 
        if (kf0.knot == kf1.knot) {

           float ddab = 0.5f * (dda + ddb);

           // Position
           dd_pos.x = ddab * deltaP.x;
           dd_pos.y = ddab * deltaP.y;
           dd_pos.z = ddab * deltaP.z;

           // Scale 
           dd_scale.x = ddab * deltaS.x;
           dd_scale.y = ddab * deltaS.y;
           dd_scale.z = ddab * deltaS.z;

           // Heading, Pitch and Bank
           dd_heading = ddab * deltaH;
           dd_pitch = ddab * deltaT;
           dd_bank = ddab * deltaB;

        } else {

           float adj0 = (kf1.knot - kf0.knot)/(kf2.knot - kf0.knot);

           // Position
           dd_pos.x = adj0 * 
              ((ddb * deltaP.x) + (dda * (kf1.position.x - kf0.position.x)));
           dd_pos.y = adj0 *
              ((ddb * deltaP.y) + (dda * (kf1.position.y - kf0.position.y)));
           dd_pos.z = adj0 * 
              ((ddb * deltaP.z) + (dda * (kf1.position.z - kf0.position.z)));

           // Scale 
           dd_scale.x = adj0 * 
              ((ddb * deltaS.x) + (dda * (kf1.scale.x - kf0.scale.x)));
           dd_scale.y = adj0 * 
              ((ddb * deltaS.y) + (dda * (kf1.scale.y - kf0.scale.y)));
           dd_scale.z = adj0 * 
              ((ddb * deltaS.z) + (dda * (kf1.scale.z - kf0.scale.z)));

           // Heading, Pitch and Bank
           dd_heading = adj0 * 
              ((ddb * deltaH) + (dda * (kf1.heading - kf0.heading)));
           dd_pitch = adj0 * 
              ((ddb * deltaT) + (dda * (kf1.pitch - kf0.pitch)));
           dd_bank = adj0 * 
              ((ddb * deltaB) + (dda * (kf1.bank - kf0.bank)));
        }
       
        // Outgoing Tangent
        Point3f ds_pos   = new Point3f();
        Point3f ds_scale = new Point3f();
        float   ds_heading, ds_pitch, ds_bank;

        // If this is the last keyframe of the animation 
        if (kf2.knot == kf3.knot) {

           float dsab = 0.5f * (dsa + dsb);

           // Position
           ds_pos.x = dsab * deltaP.x;
           ds_pos.y = dsab * deltaP.y;
           ds_pos.z = dsab * deltaP.z;
           
           // Scale
           ds_scale.x = dsab * deltaS.x;
           ds_scale.y = dsab * deltaS.y;
           ds_scale.z = dsab * deltaS.z;

           // Heading, Pitch and Bank
           ds_heading = dsab * deltaH;
           ds_pitch = dsab * deltaT;
           ds_bank = dsab * deltaB;

        } else {

           float adj1 = (kf2.knot - kf1.knot)/(kf3.knot - kf1.knot);

           // Position
           ds_pos.x = adj1 * 
             ((dsb * (kf3.position.x - kf2.position.x)) + (dsa * deltaP.x));
           ds_pos.y = adj1 * 
             ((dsb * (kf3.position.y - kf2.position.y)) + (dsa * deltaP.y));
           ds_pos.z = adj1 * 
             ((dsb * (kf3.position.z - kf2.position.z)) + (dsa * deltaP.z));

           // Scale
           ds_scale.x = adj1 * 
             ((dsb * (kf3.scale.x - kf2.scale.x)) + (dsa * deltaS.x));
           ds_scale.y = adj1 * 
             ((dsb * (kf3.scale.y - kf2.scale.y)) + (dsa * deltaS.y));
           ds_scale.z = adj1 * 
             ((dsb * (kf3.scale.z - kf2.scale.z)) + (dsa * deltaS.z));

           // Heading, Pitch and Bank 
           ds_heading = adj1 * 
             ((dsb * (kf3.heading - kf2.heading)) + (dsa * deltaH));
           ds_pitch = adj1 * 
             ((dsb * (kf3.pitch - kf2.pitch)) + (dsa * deltaT));
           ds_bank = adj1 * 
             ((dsb * (kf3.bank - kf2.bank)) + (dsa * deltaB));
        }

        // Calculate the coefficients of the polynomial for position
        c0 = new Point3f();
        c0.x = kf1.position.x;
        c0.y = kf1.position.y;
        c0.z = kf1.position.z;

        c1 = new Point3f();
        c1.x = dd_pos.x;
        c1.y = dd_pos.y;
        c1.z = dd_pos.z;

        c2 = new Point3f();
        c2.x = 3*deltaP.x - 2*dd_pos.x - ds_pos.x;
        c2.y = 3*deltaP.y - 2*dd_pos.y - ds_pos.y;
        c2.z = 3*deltaP.z - 2*dd_pos.z - ds_pos.z;

        c3 = new Point3f();
        c3.x = -2*deltaP.x + dd_pos.x + ds_pos.x;
        c3.y = -2*deltaP.y + dd_pos.y + ds_pos.y;
        c3.z = -2*deltaP.z + dd_pos.z + ds_pos.z;

        // Calculate the coefficients of the polynomial for scale 
        e0 = new Point3f();
        e0.x = kf1.scale.x;
        e0.y = kf1.scale.y;
        e0.z = kf1.scale.z;

        e1 = new Point3f();
        e1.x = dd_scale.x;
        e1.y = dd_scale.y;
        e1.z = dd_scale.z;

        e2 = new Point3f();
        e2.x = 3*deltaS.x - 2*dd_scale.x - ds_scale.x;
        e2.y = 3*deltaS.y - 2*dd_scale.y - ds_scale.y;
        e2.z = 3*deltaS.z - 2*dd_scale.z - ds_scale.z;

        e3 = new Point3f();
        e3.x = -2*deltaS.x + dd_scale.x + ds_scale.x;
        e3.y = -2*deltaS.y + dd_scale.y + ds_scale.y;
        e3.z = -2*deltaS.z + dd_scale.z + ds_scale.z;

        // Calculate the coefficients of the polynomial for heading, pitch 
        // and bank
        h0 = kf1.heading;
        p0 = kf1.pitch;
        b0 = kf1.bank;

        h1 = dd_heading;
        p1 = dd_pitch;
        b1 = dd_bank;

        h2 = 3*deltaH - 2*dd_heading - ds_heading;
        p2 = 3*deltaT - 2*dd_pitch - ds_pitch;
        b2 = 3*deltaB - 2*dd_bank - ds_bank;

        h3 = -2*deltaH + dd_heading + ds_heading;
        p3 = -2*deltaT + dd_pitch + ds_pitch;
        b3 = -2*deltaB + dd_bank + ds_bank;
    }


    /**
     * Computes the length of the curve at a given point between
     * key frames.
     * @param u specifies the point between keyframes where 0 <= u <= 1. 
     */

    public float computeLength (float u) {

        float result = 0f;

        // if linear interpolation
        if (linear == 1) {
            result = u*keyFrame[2].position.distance(keyFrame[1].position);
        } else {
            // Need to transform domain [0,u] to [-1,1].  If 0 <= x <= u
            // and -1 <= t <= 1, then x = u*(t+1)/2.
            int degree = 5;
            for (int i = 0; i < degree; i++)
                result += (float)modCoeff[i]*computeSpeed(u*(float)modRoot[i]);
            result *= u;
        }

        return result;
    }

    // Velocity along curve
    private float computeSpeed (float u) {
        Point3f v = new Point3f();

        v.x = c1.x + u * (2 * c2.x + 3 * u * c3.x);
        v.y = c1.y + u * (2 * c2.y + 3 * u * c3.y);
        v.z = c1.z + u * (2 * c2.z + 3 * u * c3.z);

        return (float)(Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z)); 
    }


    /**
     * Computes the interpolated scale along the curve at a given point
     * between key frames and returns a Point3f with the interpolated 
     * x, y, and z scale components. This routine uses linear
     * interpolation if the (i+1)<sup>th</sup> key frame's linear 
     * value is equal to 1. 
     * 
     * @param u specifies the point between keyframes where 0 <= u <= 1. 
     * @param newScale returns the interpolated x,y,z scale value in a Point3f
     */

    public void getInterpolatedScale (float u, Point3f newScale) {
        
        // if linear interpolation
        if (this.linear == 1) {

            newScale.x = keyFrame[1].scale.x + 
                      ((keyFrame[2].scale.x - keyFrame[1].scale.x) * u);
            newScale.y = keyFrame[1].scale.y + 
                      ((keyFrame[2].scale.y - keyFrame[1].scale.y) * u);
            newScale.z = keyFrame[1].scale.z + 
                      ((keyFrame[2].scale.z - keyFrame[1].scale.z) * u);

        } else {

            newScale.x = e0.x + u * (e1.x + u * (e2.x + u * e3.x));
            newScale.y = e0.y + u * (e1.y + u * (e2.y + u * e3.y));
            newScale.z = e0.z + u * (e1.z + u * (e2.z + u * e3.z));

        }
    }


    /**
     * Computes the interpolated position along the curve at a given point
     * between key frames and returns a Point3f with the interpolated 
     * x, y, and z scale components. This routine uses linear
     * interpolation if the (i+1)<sup>th</sup> key frame's linear
     * value is equal to 1.
     *
     * @param u specifies the point between keyframes where 0 <= u <= 1.
     * @param newPos returns the interpolated x,y,z position in a Point3f
     */

    public void getInterpolatedPosition (float u, Point3f newPos) {
        
        // if linear interpolation
        if (this.linear == 1) {
            newPos.x = keyFrame[1].position.x + 
                      ((keyFrame[2].position.x - keyFrame[1].position.x) * u);
            newPos.y = keyFrame[1].position.y + 
                      ((keyFrame[2].position.y - keyFrame[1].position.y) * u);
            newPos.z = keyFrame[1].position.z + 
                      ((keyFrame[2].position.z - keyFrame[1].position.z) * u);
        } else {

            newPos.x = c0.x + u * (c1.x + u * (c2.x + u * c3.x));
            newPos.y = c0.y + u * (c1.y + u * (c2.y + u * c3.y));
            newPos.z = c0.z + u * (c1.z + u * (c2.z + u * c3.z));

        }
    }


    /**
     * Computes the interpolated position along the curve at a given point
     * between key frames and returns a Vector3f with the interpolated 
     * x, y, and z scale components. This routine uses linear
     * interpolation if the (i+1)<sup>th</sup> key frame's linear
     * value is equal to 1.
     *
     * @param u specifies the point between keyframes where 0 <= u <= 1.
     * @param newPos returns the interpolated x,y,z position in a Vector3f. 
     */

    public void getInterpolatedPositionVector (float u, Vector3f newPos) {
        // if linear interpolation
        if (this.linear == 1) {
            newPos.x = keyFrame[1].position.x + 
                      ((keyFrame[2].position.x - keyFrame[1].position.x) * u);
            newPos.y = keyFrame[1].position.y + 
                      ((keyFrame[2].position.y - keyFrame[1].position.y) * u);
            newPos.z = keyFrame[1].position.z + 
                      ((keyFrame[2].position.z - keyFrame[1].position.z) * u);
        } else {

            newPos.x = c0.x + u * (c1.x + u * (c2.x + u * c3.x));
            newPos.y = c0.y + u * (c1.y + u * (c2.y + u * c3.y));
            newPos.z = c0.z + u * (c1.z + u * (c2.z + u * c3.z));

        }
    }

    /**
     * Computes the interpolated heading along the curve at a given point
     * between key frames and returns the interpolated value as a float 
     * This routine uses linear interpolation if the (i+1)<sup>th</sup> 
     * key frame's linear value is equal to 1.
     *
     * @param u specifies the point between keyframes where 0 <= u <= 1.
     * @return returns the interpolated heading value 
     */

    public float getInterpolatedHeading (float u) {

        float newHeading;
        
        // if linear interpolation
        if (this.linear == 1) {

            newHeading = keyFrame[1].heading + 
                      ((keyFrame[2].heading - keyFrame[1].heading) * u);
        } else {

            newHeading = h0 + u * (h1 + u * (h2 + u * h3));

        }

        return newHeading;
    }


    /**
     * Computes the interpolated pitch along the curve at a given point
     * between key frames and returns the interpolated value as a float 
     * This routine uses linear interpolation if the (i+1)<sup>th</sup> 
     * key frame's linear value is equal to 1.
     *
     * @param u specifies the point between keyframes where 0 <= u <= 1.
     * @return returns the interpolated pitch value 
     */

    public float getInterpolatedPitch (float u) {

        float newPitch;
        
        // if linear interpolation
        if (this.linear == 1) {

            newPitch = keyFrame[1].pitch + 
                      ((keyFrame[2].pitch - keyFrame[1].pitch) * u);
        } else {

            newPitch = p0 + u * (p1 + u * (p2 + u * p3));

        }

        return newPitch;
    }

    /**
     * Computes the interpolated bank along the curve at a given point
     * between key frames and returns the interpolated value as a float 
     * This routine uses linear interpolation if the (i+1)<sup>th</sup> 
     * key frame's linear value is equal to 1.
     *
     * @param u specifies the point between keyframes where 0 <= u <= 1.
     * @return returns the interpolated bank value 
     */

    public float getInterpolatedBank (float u) {

        float newBank;
        
        // if linear interpolation
        if (this.linear == 1) {

            newBank = keyFrame[1].bank + 
                      ((keyFrame[2].bank - keyFrame[1].bank) * u);
        } else {

            newBank = b0 + u * (b1 + u * (b2 + u * b3));

        }

        return newBank;
    }


    /**
     * Computes the ratio of the length of the spline from the i<sup>th</sup>
     * key frame to the position specified by u to the length of the entire
     * spline segment from the i<sup>th</sup> key frame to the (i+1)
     * <sup>th</sup> key frame. When the (i+1)<sup>th</sup> key frame's linear
     * value is equal to 1, this is meaninful otherwise it should return u. 
     *
     * @param u specifies the point between keyframes where 0 <= u <= 1.
     * @return the interpolated ratio 
     */

    public float getInterpolatedValue (float u) {
        return (computeLength(u)/this.length);
    }
}
