830 lines
24 KiB
C#
830 lines
24 KiB
C#
using UnityEngine;
|
|
using System;
|
|
|
|
using QuaternionUtilities;
|
|
using System.Collections.Generic;
|
|
|
|
public partial class Spline : MonoBehaviour
|
|
{
|
|
SplineInterpolator splineInterpolator = new HermiteInterpolator( ); ///< The SplineInterpolator that will be used for spline interpolation.
|
|
}
|
|
|
|
/// <summary>
|
|
/// The SplineInterpolator class provides functions that are necessary to interpolate positions, rotations, values, etc. of and on splines.
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// It can be used to manually interpolate an array of vectors/quaternions/values without having to instanciate a whole new spline.
|
|
/// Depending on its settings it provides different interpolation modes.
|
|
///
|
|
/// Please note that this class doesn't provide constant-velocity interpolation, when used on its own. If you need constant-velocity interpolation
|
|
/// use the regular Spline class!
|
|
/// </remarks>
|
|
[Serializable]
|
|
public class SplineInterpolator
|
|
{
|
|
protected double[] coefficientMatrix;
|
|
protected int[] nodeIndices;
|
|
|
|
protected SplineInterpolator( )
|
|
{
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="SplineInterpolator"/> class.
|
|
/// </summary>
|
|
/// <param name='coefficientMatrix'>
|
|
/// The coefficient 4x4 matrix that will be used for the spline interpolation.
|
|
/// The array must contain exactly 16 elements!
|
|
/// </param>
|
|
/// <param name='nodeIndices'>
|
|
/// The relative indices of the nodes that are needed for the interpolation.
|
|
/// The array must contain exactly 4 elements!
|
|
/// </param>
|
|
public SplineInterpolator( double[] coefficientMatrix, int[] nodeIndices )
|
|
{
|
|
CheckMatrix( coefficientMatrix );
|
|
CheckIndices( nodeIndices );
|
|
|
|
this.coefficientMatrix = coefficientMatrix;
|
|
this.nodeIndices = nodeIndices;
|
|
}
|
|
|
|
public double[] CoefficientMatrix{
|
|
get{ return coefficientMatrix; }
|
|
set{ CheckMatrix( value ); coefficientMatrix = value; }
|
|
} ///< Returns or sets the coefficients matrix for custom interpolation
|
|
|
|
public int[] NodeIndices{
|
|
get{ return nodeIndices; }
|
|
set{ CheckIndices( value ); nodeIndices = value; }
|
|
} ///< Returns or sets the coefficients matrix for custom interpolation
|
|
|
|
public Matrix4x4 CoefficientMatrix4x4{
|
|
get{
|
|
Matrix4x4 matrix = new Matrix4x4( );
|
|
|
|
matrix[ 0] = (float)coefficientMatrix[ 0]; matrix[ 1] = (float)coefficientMatrix[ 1]; matrix[ 2] = (float)coefficientMatrix[ 2]; matrix[ 3] = (float)coefficientMatrix[ 3];
|
|
matrix[ 4] = (float)coefficientMatrix[ 4]; matrix[ 5] = (float)coefficientMatrix[ 5]; matrix[ 6] = (float)coefficientMatrix[ 6]; matrix[ 7] = (float)coefficientMatrix[ 7];
|
|
matrix[ 8] = (float)coefficientMatrix[ 8]; matrix[ 9] = (float)coefficientMatrix[ 9]; matrix[10] = (float)coefficientMatrix[10]; matrix[11] = (float)coefficientMatrix[11];
|
|
matrix[12] = (float)coefficientMatrix[12]; matrix[13] = (float)coefficientMatrix[13]; matrix[14] = (float)coefficientMatrix[14]; matrix[15] = (float)coefficientMatrix[15];
|
|
|
|
return matrix;
|
|
}
|
|
set{
|
|
coefficientMatrix[ 0] = value[ 0]; coefficientMatrix[ 1] = value[ 1]; coefficientMatrix[ 2] = value[ 2]; coefficientMatrix[ 3] = value[ 3];
|
|
coefficientMatrix[ 4] = value[ 4]; coefficientMatrix[ 5] = value[ 5]; coefficientMatrix[ 6] = value[ 6]; coefficientMatrix[ 7] = value[ 7];
|
|
coefficientMatrix[ 8] = value[ 8]; coefficientMatrix[ 9] = value[ 9]; coefficientMatrix[10] = value[10]; coefficientMatrix[11] = value[11];
|
|
coefficientMatrix[12] = value[12]; coefficientMatrix[13] = value[13]; coefficientMatrix[14] = value[14]; coefficientMatrix[15] = value[15];
|
|
}
|
|
} ///< Returns or sets the coefficients matrix for custom interpolation
|
|
|
|
/// <summary>
|
|
/// Interpolates an array of vectors.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// The interpolated vector.
|
|
/// </returns>
|
|
/// <param name='t'>
|
|
/// An interpolation parameter from 0 to 1.
|
|
/// </param>
|
|
/// <param name='index'>
|
|
/// The index of the current spline segment.
|
|
/// </param>
|
|
/// <param name='autoClose'>
|
|
/// If enabled the vector array will be treated as closed spline.
|
|
/// </param>
|
|
/// <param name='nodes'>
|
|
/// An array of vectors.
|
|
/// </param>
|
|
/// <param name='derivationOrder'>
|
|
/// The derivation order.
|
|
/// </param>
|
|
public virtual Vector3 InterpolateVector( double t, int index, bool autoClose, IList<Vector3> nodes, int derivationOrder )
|
|
{
|
|
Vector3 v0; Vector3 v1;
|
|
Vector3 v2; Vector3 v3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out v0, out v1, out v2, out v3 );
|
|
|
|
return InterpolateVector( t, v0, v1, v2, v3, derivationOrder );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates the positions of an array of SplineNodes.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// The interpolated position.
|
|
/// </returns>
|
|
/// <param name='t'>
|
|
/// An interpolation parameter from 0 to 1.
|
|
/// </param>
|
|
/// <param name='index'>
|
|
/// The index of the current spline segment.
|
|
/// </param>
|
|
/// <param name='autoClose'>
|
|
/// If enabled the node array will be treated as closed spline.
|
|
/// </param>
|
|
/// <param name='nodes'>
|
|
/// An array of SplineNodes.
|
|
/// </param>
|
|
/// <param name='derivationOrder'>
|
|
/// The derivation order.
|
|
/// </param>
|
|
public virtual Vector3 InterpolateVector( Spline spline, double t, int index, bool autoClose, IList<SplineNode> nodes, int derivationOrder )
|
|
{
|
|
SplineNode n0; SplineNode n1;
|
|
SplineNode n2; SplineNode n3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out n0, out n1, out n2, out n3 );
|
|
|
|
return InterpolateVector( t, n0.Position, n1.Position, n2.Position, n3.Position, derivationOrder );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates an array of values.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// The interpolated value
|
|
/// </returns>
|
|
/// <param name='t'>
|
|
/// An interpolation parameter from 0 to 1.
|
|
/// </param>
|
|
/// <param name='index'>
|
|
/// The index of the current spline segment.
|
|
/// </param>
|
|
/// <param name='autoClose'>
|
|
/// If enabled the value array will be treated as closed spline.
|
|
/// </param>
|
|
/// <param name='nodes'>
|
|
/// An array of float values
|
|
/// </param>
|
|
/// <param name='derivationOrder'>
|
|
/// Derivation order.
|
|
/// </param>
|
|
public virtual float InterpolateValue( double t, int index, bool autoClose, IList<float> nodes, int derivationOrder )
|
|
{
|
|
float v0; float v1;
|
|
float v2; float v3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out v0, out v1, out v2, out v3 );
|
|
|
|
return InterpolateValue( t, v0, v1, v2, v3, derivationOrder );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates the custom values of an array of SplineNodes.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// The interpolated value.
|
|
/// </returns>
|
|
/// <param name='t'>
|
|
/// An interpolation parameter from 0 to 1.
|
|
/// </param>
|
|
/// <param name='index'>
|
|
/// The index of the current spline segment.
|
|
/// </param>
|
|
/// <param name='autoClose'>
|
|
/// If enabled the node array will be treated as closed spline.
|
|
/// </param>
|
|
/// <param name='nodes'>
|
|
/// An array of SplineNodes
|
|
/// </param>
|
|
/// <param name='derivationOrder'>
|
|
/// Derivation order.
|
|
/// </param>
|
|
public virtual float InterpolateValue( Spline spline, double t, int index, bool autoClose, IList<SplineNode> nodes, int derivationOrder )
|
|
{
|
|
SplineNode n0; SplineNode n1;
|
|
SplineNode n2; SplineNode n3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out n0, out n1, out n2, out n3 );
|
|
|
|
return InterpolateValue( t, n0.CustomValue, n1.CustomValue, n2.CustomValue, n3.CustomValue, derivationOrder );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates an array of rotations.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// The interpolated rotation.
|
|
/// </returns>
|
|
/// <param name='t'>
|
|
/// An interpolation parameter from 0 to 1.
|
|
/// </param>
|
|
/// <param name='index'>
|
|
/// The index of the current spline segment.
|
|
/// </param>
|
|
/// <param name='autoClose'>
|
|
/// If enabled the quaternion array will be treated as closed spline.
|
|
/// </param>
|
|
/// <param name='nodes'>
|
|
/// An array of rotations.
|
|
/// </param>
|
|
/// <param name='derivationOrder'>
|
|
/// Derivation order.
|
|
/// </param>
|
|
public virtual Quaternion InterpolateRotation( double t, int index, bool autoClose, IList<Quaternion> nodes, int derivationOrder )
|
|
{
|
|
Quaternion q0; Quaternion q1;
|
|
Quaternion q2; Quaternion q3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out q0, out q1, out q2, out q3 );
|
|
|
|
return InterpolateRotation( t, q0, q1, q2, q3, derivationOrder );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates the rotations of an array of SplineNodes.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// The interpolated rotation.
|
|
/// </returns>
|
|
/// <param name='t'>
|
|
/// An interpolation parameter from 0 to 1.
|
|
/// </param>
|
|
/// <param name='index'>
|
|
/// The index of the current spline segment.
|
|
/// </param>
|
|
/// <param name='autoClose'>
|
|
/// If enabled the node array will be treated as closed spline.
|
|
/// </param>
|
|
/// <param name='nodes'>
|
|
/// An array of SplineNodes.
|
|
/// </param>
|
|
/// <param name='derivationOrder'>
|
|
/// Derivation
|
|
/// </param>
|
|
public virtual Quaternion InterpolateRotation( Spline spline, double t, int index, bool autoClose, IList<SplineNode> nodes, int derivationOrder )
|
|
{
|
|
SplineNode n0; SplineNode n1;
|
|
SplineNode n2; SplineNode n3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out n0, out n1, out n2, out n3 );
|
|
|
|
return InterpolateRotation( t, n0.Rotation, n1.Rotation, n2.Rotation, n3.Rotation, derivationOrder );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates 4 vectors.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// The interpolated vector.
|
|
/// </returns>
|
|
/// <param name='t'>
|
|
/// An interpolation parameter from 0 to 1.
|
|
/// </param>
|
|
/// <param name='v0'>
|
|
/// 1st Vector
|
|
/// </param>
|
|
/// <param name='v1'>
|
|
/// 2nd Vector
|
|
/// </param>
|
|
/// <param name='v2'>
|
|
/// 3rd Vector
|
|
/// </param>
|
|
/// <param name='v3'>
|
|
/// 4th Vector
|
|
/// </param>
|
|
/// <param name='derivationOrder'>
|
|
/// Derivation order.
|
|
/// </param>
|
|
public Vector3 InterpolateVector( double t, Vector3 v0, Vector3 v1, Vector3 v2, Vector3 v3, int derivationOrder )
|
|
{
|
|
float b0; float b1;
|
|
float b2; float b3;
|
|
|
|
GetCoefficients( t, out b0, out b1, out b2, out b3, derivationOrder );
|
|
|
|
return b0 * v0 + b1 * v1 + b2 * v2 + b3 * v3;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates 4 float values.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// The interpolated float value.
|
|
/// </returns>
|
|
/// <param name='t'>
|
|
/// An interpolation parameter from 0 to 1.
|
|
/// </param>
|
|
/// <param name='v0'>
|
|
/// 1st float value
|
|
/// </param>
|
|
/// <param name='v1'>
|
|
/// 2nd float value
|
|
/// </param>
|
|
/// <param name='v2'>
|
|
/// 3rd float value
|
|
/// </param>
|
|
/// <param name='v3'>
|
|
/// 4th float value
|
|
/// </param>
|
|
/// <param name='derivationOrder'>
|
|
/// Derivation order.
|
|
/// </param>
|
|
public float InterpolateValue( double t, float v0, float v1, float v2, float v3, int derivationOrder )
|
|
{
|
|
float b0; float b1;
|
|
float b2; float b3;
|
|
|
|
GetCoefficients( t, out b0, out b1, out b2, out b3, derivationOrder );
|
|
|
|
return b0 * v0 + b1 * v1 + b2 * v2 + b3 * v3;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates 4 quaternions.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// The interpolated quaternion.
|
|
/// </returns>
|
|
/// <param name='t'>
|
|
/// An interpolation parameter from 0 to 1.
|
|
/// </param>
|
|
/// <param name='q0'>
|
|
/// 1st float quaternion
|
|
/// </param>
|
|
/// <param name='q1'>
|
|
/// 2nd float quaternion
|
|
/// </param>
|
|
/// <param name='q2'>
|
|
/// 3rd float quaternion
|
|
/// </param>
|
|
/// <param name='q3'>
|
|
/// 4th float quaternion
|
|
/// </param>
|
|
/// <param name='derivationOrder'>
|
|
/// Derivation order.
|
|
/// </param>
|
|
public Quaternion InterpolateRotation( double t, Quaternion q0, Quaternion q1, Quaternion q2, Quaternion q3, int derivationOrder )
|
|
{
|
|
float b0; float b1;
|
|
float b2; float b3;
|
|
|
|
if( Quaternion.Dot( q0, q1 ) < 0 )
|
|
q1 = q1.Negative( );
|
|
|
|
if( Quaternion.Dot( q1, q2 ) < 0 )
|
|
q2 = q2.Negative( );
|
|
|
|
if( Quaternion.Dot( q2, q3 ) < 0 )
|
|
q3 = q3.Negative( );
|
|
|
|
GetCoefficients( t, out b0, out b1, out b2, out b3, derivationOrder );
|
|
|
|
Vector3 imaginary0 = new Vector3( q0.x, q0.y, q0.z );
|
|
Vector3 imaginary1 = new Vector3( q1.x, q1.y, q1.z );
|
|
Vector3 imaginary2 = new Vector3( q2.x, q2.y, q2.z );
|
|
Vector3 imaginary3 = new Vector3( q3.x, q3.y, q3.z );
|
|
|
|
Vector3 interpolatedImaginary = b0 * imaginary0 + b1 * imaginary1 + b2 * imaginary2 + b3 * imaginary3;
|
|
float interpolatedReal = b0 * q0.w + b1 * q1.w + b2 * q2.w + b3 * q3.w;
|
|
|
|
Quaternion result = new Quaternion( interpolatedImaginary.x, interpolatedImaginary.y, interpolatedImaginary.z, interpolatedReal );
|
|
|
|
return result.Normalized( );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Select 4 elements of an array according to the indices of nodes that are used for interpolation (nodeIndices).
|
|
/// </summary>
|
|
/// <param name='array'>
|
|
/// A generic array.
|
|
/// </param>
|
|
/// <param name='index'>
|
|
/// The index of the currently processed spline node
|
|
/// </param>
|
|
/// <param name='autoClose'>
|
|
/// If set to true, the array will be treated as "looping array" / closed spline.
|
|
/// </param>
|
|
/// <param name='d0'>
|
|
/// 1st element.
|
|
/// </param>
|
|
/// <param name='d1'>
|
|
/// 2nd element.
|
|
/// </param>
|
|
/// <param name='d2'>
|
|
/// 3rd element.
|
|
/// </param>
|
|
/// <param name='d3'>
|
|
/// 4th element
|
|
/// </param>
|
|
/// <typeparam name='T'>
|
|
/// The type of the array's elements.
|
|
/// </typeparam>
|
|
public void GetNodeData<T>( IList<T> array, int index, bool autoClose, out T d0, out T d1, out T d2, out T d3 )
|
|
{
|
|
int arrayLength = array.Count;
|
|
|
|
d0 = array[GetNodeIndex( autoClose, arrayLength, index, nodeIndices[0] )];
|
|
d1 = array[GetNodeIndex( autoClose, arrayLength, index, nodeIndices[1] )];
|
|
d2 = array[GetNodeIndex( autoClose, arrayLength, index, nodeIndices[2] )];
|
|
d3 = array[GetNodeIndex( autoClose, arrayLength, index, nodeIndices[3] )];
|
|
}
|
|
|
|
private int GetNodeIndex( bool autoClose, int arrayLength, int index, int offset )
|
|
{
|
|
int idxNode = index + offset;
|
|
|
|
if( autoClose )
|
|
return (idxNode % arrayLength + arrayLength) % arrayLength;
|
|
else
|
|
return Mathf.Clamp( idxNode, 0, arrayLength-1 );
|
|
}
|
|
|
|
private void GetCoefficients( double t, out float b0, out float b1, out float b2, out float b3, int derivationOrder )
|
|
{
|
|
switch( derivationOrder )
|
|
{
|
|
case 0: GetCoefficients( t, out b0, out b1, out b2, out b3 ); return;
|
|
case 1: GetCoefficientsFirstDerivative( t, out b0, out b1, out b2, out b3 ); return;
|
|
case 2: GetCoefficientsSecondDerivative( t, out b0, out b1, out b2, out b3 ); return;
|
|
}
|
|
|
|
b0 = 0; b1 = 0; b2 = 0; b3 = 0;
|
|
}
|
|
|
|
private void GetCoefficients( double t, out float b0, out float b1, out float b2, out float b3 )
|
|
{
|
|
double t2 = t * t;
|
|
double t3 = t2 * t;
|
|
|
|
b0 = (float) (coefficientMatrix[ 0] * t3 + coefficientMatrix[ 1] * t2 + coefficientMatrix[ 2] * t + coefficientMatrix[ 3]);
|
|
b1 = (float) (coefficientMatrix[ 4] * t3 + coefficientMatrix[ 5] * t2 + coefficientMatrix[ 6] * t + coefficientMatrix[ 7]);
|
|
b2 = (float) (coefficientMatrix[ 8] * t3 + coefficientMatrix[ 9] * t2 + coefficientMatrix[10] * t + coefficientMatrix[11]);
|
|
b3 = (float) (coefficientMatrix[12] * t3 + coefficientMatrix[13] * t2 + coefficientMatrix[14] * t + coefficientMatrix[15]);
|
|
} ///< Returns the interpolation coefficients for the curve
|
|
|
|
private void GetCoefficientsFirstDerivative( double t, out float b0, out float b1, out float b2, out float b3 )
|
|
{
|
|
double t2 = t * t;
|
|
|
|
t = t * 2.0;
|
|
t2 = t2 * 3.0;
|
|
|
|
b0 = (float) (coefficientMatrix[ 0] * t2 + coefficientMatrix[ 1] * t + coefficientMatrix[ 2]);
|
|
b1 = (float) (coefficientMatrix[ 4] * t2 + coefficientMatrix[ 5] * t + coefficientMatrix[ 6]);
|
|
b2 = (float) (coefficientMatrix[ 8] * t2 + coefficientMatrix[ 9] * t + coefficientMatrix[10]);
|
|
b3 = (float) (coefficientMatrix[12] * t2 + coefficientMatrix[13] * t + coefficientMatrix[14]);
|
|
} ///< Returns the interpolation coefficients for the curve's first derivative
|
|
|
|
private void GetCoefficientsSecondDerivative( double t, out float b0, out float b1, out float b2, out float b3 )
|
|
{
|
|
t *= 6.0;
|
|
|
|
b0 = (float) (coefficientMatrix[ 0] * t + coefficientMatrix[ 1] * 2f);
|
|
b1 = (float) (coefficientMatrix[ 4] * t + coefficientMatrix[ 5] * 2f);
|
|
b2 = (float) (coefficientMatrix[ 8] * t + coefficientMatrix[ 9] * 2f);
|
|
b3 = (float) (coefficientMatrix[12] * t + coefficientMatrix[13] * 2f);
|
|
} ///< Returns the interpolation coefficients for the curve's second derivative
|
|
|
|
private void CheckMatrix( double[] coefficientMatrix )
|
|
{
|
|
if( coefficientMatrix.Length != 16 )
|
|
throw new ArgumentException( "The coefficientMatrix-array must contain exactly 16 doubles!" );
|
|
}
|
|
|
|
private void CheckIndices( int[] nodeIndices )
|
|
{
|
|
if( nodeIndices.Length != 4 )
|
|
throw new ArgumentException( "nodeIndices-array must contain exactly 4 ints!" );
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// A Hermite Interpolator.
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// In contrast to the other default SplineInterpolators this Interpolator provides special functions that are necessary for hermite spline interpolation
|
|
/// </remarks>
|
|
public class HermiteInterpolator : SplineInterpolator
|
|
{
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="HermiteInterpolator"/> class.
|
|
/// </summary>
|
|
public HermiteInterpolator( )
|
|
{
|
|
CoefficientMatrix = new double[] {
|
|
2.0, -3.0, 0.0, 1.0,
|
|
-2.0, 3.0, 0.0, 0.0,
|
|
1.0, -2.0, 1.0, 0.0,
|
|
1.0, -1.0, 0.0, 0.0
|
|
};
|
|
|
|
NodeIndices = new int[] { 0, 1, -1, 2 };
|
|
}
|
|
|
|
public override Vector3 InterpolateVector( Spline spline, double t, int index, bool autoClose, IList<SplineNode> nodes, int derivationOrder )
|
|
{
|
|
SplineNode n0; SplineNode n1;
|
|
SplineNode n2; SplineNode n3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out n0, out n1, out n2, out n3 );
|
|
|
|
Vector3 v2 = n2.Position;
|
|
Vector3 v3 = n3.Position;
|
|
|
|
RecalcVectors( spline, n0, n1, ref v2, ref v3 );
|
|
|
|
return InterpolateVector( t, n0.Position, n1.Position, v2, v3, derivationOrder );
|
|
}
|
|
|
|
public override Vector3 InterpolateVector( double t, int index, bool autoClose, IList<Vector3> nodes, int derivationOrder )
|
|
{
|
|
Vector3 v0; Vector3 v1;
|
|
Vector3 v2; Vector3 v3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out v0, out v1, out v2, out v3 );
|
|
|
|
RecalcVectors( v0, v1, ref v2, ref v3 );
|
|
|
|
return InterpolateVector( t, v0, v1, v2, v3, derivationOrder );
|
|
}
|
|
|
|
public override float InterpolateValue( Spline spline, double t, int index, bool autoClose, IList<SplineNode> nodes, int derivationOrder )
|
|
{
|
|
SplineNode n0; SplineNode n1;
|
|
SplineNode n2; SplineNode n3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out n0, out n1, out n2, out n3 );
|
|
|
|
float v2 = n2.CustomValue;
|
|
float v3 = n3.CustomValue;
|
|
|
|
RecalcScalars( spline, n0, n1, ref v2, ref v3 );
|
|
|
|
return InterpolateValue( t, n0.CustomValue, n1.CustomValue, v2, v3, derivationOrder );
|
|
}
|
|
|
|
public override float InterpolateValue( double t, int index, bool autoClose, IList<float> nodes, int derivationOrder )
|
|
{
|
|
float v0; float v1;
|
|
float v2; float v3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out v0, out v1, out v2, out v3 );
|
|
|
|
RecalcScalars( v0, v1, ref v2, ref v3 );
|
|
|
|
return InterpolateValue( t, v0, v1, v2, v3, derivationOrder );
|
|
}
|
|
|
|
public override Quaternion InterpolateRotation( Spline spline, double t, int index, bool autoClose, IList<SplineNode> nodes, int derivationOrder )
|
|
{
|
|
SplineNode n0; SplineNode n1;
|
|
SplineNode n2; SplineNode n3;
|
|
|
|
GetNodeData( nodes, index, autoClose, out n0, out n1, out n2, out n3 );
|
|
|
|
Quaternion q2 = n2.Rotation;
|
|
Quaternion q3 = n3.Rotation;
|
|
|
|
RecalcRotations( n0.Rotation, n1.Rotation, ref q2, ref q3 );
|
|
|
|
return InterpolateRotation( t, n0.Rotation, n1.Rotation, q2, q3, derivationOrder );
|
|
}
|
|
|
|
/// <summary>
|
|
/// A function that returns the tangents for two spline nodes
|
|
/// </summary>
|
|
/// <param name='spline'>
|
|
/// The used spline
|
|
/// </param>
|
|
/// <param name='node0'>
|
|
/// 1st node.
|
|
/// </param>
|
|
/// <param name='node1'>
|
|
/// 2nd node.
|
|
/// </param>
|
|
/// <param name='P2'>
|
|
/// In: Position of 3rd node.
|
|
/// Out: Tangent of node0.
|
|
/// </param>
|
|
/// <param name='P3'>
|
|
/// In: Position of 4th node.
|
|
/// Out: Tangent of node1.
|
|
/// </param>
|
|
public void RecalcVectors( Spline spline, SplineNode node0, SplineNode node1, ref Vector3 P2, ref Vector3 P3 )
|
|
{
|
|
float tension0;
|
|
float tension1;
|
|
|
|
if( spline.perNodeTension )
|
|
{
|
|
tension0 = node0.tension;
|
|
tension1 = node1.tension;
|
|
}
|
|
else
|
|
{
|
|
tension0 = spline.tension;
|
|
tension1 = spline.tension;
|
|
}
|
|
|
|
if( spline.tangentMode == Spline.TangentMode.UseNodeForwardVector )
|
|
{
|
|
P2 = node0.transform.forward * tension0;
|
|
P3 = node1.transform.forward * tension1;
|
|
}
|
|
else
|
|
{
|
|
P2 = node1.Position - P2;
|
|
P3 = P3 - node0.Position;
|
|
|
|
if( spline.tangentMode != Spline.TangentMode.UseTangents )
|
|
{
|
|
P2.Normalize( );
|
|
P3.Normalize( );
|
|
}
|
|
|
|
P2 = P2 * tension0;
|
|
P3 = P3 * tension1;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// A function that returns the tangents for two spline nodes.
|
|
/// </summary>
|
|
/// <param name='P0'>
|
|
/// Position of 1st spline node.
|
|
/// </param>
|
|
/// <param name='P1'>
|
|
/// Position of 2nd spline node.
|
|
/// </param>
|
|
/// <param name='P2'>
|
|
/// In: Position of 3rd node.
|
|
/// Out: Tangent of node0.
|
|
/// </param>
|
|
/// <param name='P3'>
|
|
/// In: Position of 4th node.
|
|
/// Out: Tangent of node1.
|
|
/// </param>
|
|
public void RecalcVectors( Vector3 P0, Vector3 P1, ref Vector3 P2, ref Vector3 P3 )
|
|
{
|
|
float tension = 0.5f;
|
|
|
|
P2 = P1 - P2;
|
|
P3 = P3 - P0;
|
|
|
|
P2 = P2 * tension;
|
|
P3 = P3 * tension;
|
|
}
|
|
|
|
/// <summary>
|
|
/// A function that returns the tangents for two spline nodes
|
|
/// </summary>
|
|
/// <param name='spline'>
|
|
/// The used spline
|
|
/// </param>
|
|
/// <param name='node0'>
|
|
/// 1st node.
|
|
/// </param>
|
|
/// <param name='node1'>
|
|
/// 2nd node.
|
|
/// </param>
|
|
/// <param name='P2'>
|
|
/// In: Value of 3rd node.
|
|
/// Out: Tangent of node0.
|
|
/// </param>
|
|
/// <param name='P3'>
|
|
/// In: Value of 4th node.
|
|
/// Out: Tangent of node1.
|
|
/// </param>
|
|
public void RecalcScalars( Spline spline, SplineNode node0, SplineNode node1, ref float P2, ref float P3 )
|
|
{
|
|
float tension0;
|
|
float tension1;
|
|
|
|
if( spline.perNodeTension )
|
|
{
|
|
tension0 = node0.tension;
|
|
tension1 = node1.tension;
|
|
}
|
|
else
|
|
{
|
|
tension0 = spline.tension;
|
|
tension1 = spline.tension;
|
|
}
|
|
|
|
P2 = node1.customValue - P2;
|
|
P3 = P3 - node0.customValue;
|
|
|
|
P2 = P2 * tension0;
|
|
P3 = P3 * tension1;
|
|
}
|
|
|
|
/// <summary>
|
|
/// A function that returns the tangents for two spline nodes.
|
|
/// </summary>
|
|
/// <param name='P0'>
|
|
/// Value of 1st spline node.
|
|
/// </param>
|
|
/// <param name='P1'>
|
|
/// Value of 2nd spline node.
|
|
/// </param>
|
|
/// <param name='P2'>
|
|
/// In: Value of 3rd node.
|
|
/// Out: Tangent of node0.
|
|
/// </param>
|
|
/// <param name='P3'>
|
|
/// In: Value of 4th node.
|
|
/// Out: Tangent of node1.
|
|
/// </param>
|
|
public void RecalcScalars( float P0, float P1, ref float P2, ref float P3 )
|
|
{
|
|
float tension = 0.5f;
|
|
|
|
P2 = P1 - P2;
|
|
P3 = P3 - P0;
|
|
|
|
P2 = P2 * tension;
|
|
P3 = P3 * tension;
|
|
}
|
|
|
|
/// <summary>
|
|
/// A function that returns the intermediate rotations for two spline nodes
|
|
/// </summary>
|
|
/// <param name='Q0'>
|
|
/// The rotation of the 1st spline node.
|
|
/// </param>
|
|
/// <param name='Q1'>
|
|
/// The rotation of the 2nd spline node.
|
|
/// </param>
|
|
/// <param name='Q2'>
|
|
/// In: Rotation of the 3rd spline node.
|
|
/// Out: intermediate rotation of node0.
|
|
/// </param>
|
|
/// <param name='Q3'>
|
|
/// In: Rotation of the 4th spline node.
|
|
/// Out: intermediate rotation of node1.
|
|
/// </param>
|
|
public void RecalcRotations( Quaternion Q0, Quaternion Q1, ref Quaternion Q2, ref Quaternion Q3 )
|
|
{
|
|
//Recalc rotations
|
|
Q2 = QuaternionUtils.GetSquadIntermediate( Q0, Q1, Q2 );
|
|
Q3 = QuaternionUtils.GetSquadIntermediate( Q1, Q2, Q3 );
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// A Bezier Interpolator.
|
|
/// </summary>
|
|
public class BezierInterpolator : SplineInterpolator
|
|
{
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="BezierInterpolator"/> class.
|
|
/// </summary>
|
|
public BezierInterpolator( )
|
|
{
|
|
CoefficientMatrix = new double[] {
|
|
-1.0, 3.0, -3.0, 1.0,
|
|
3.0, -6.0, 3.0, 0.0,
|
|
-3.0, 3.0, 0.0, 0.0,
|
|
1.0, 0.0, 0.0, 0.0
|
|
};
|
|
|
|
NodeIndices = new int[] { 0, 1, 2, 3 };
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// A B-spline Interpolator.
|
|
/// </summary>
|
|
public class BSplineInterpolator : SplineInterpolator
|
|
{
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="BSplineInterpolator"/> class.
|
|
/// </summary>
|
|
public BSplineInterpolator( )
|
|
{
|
|
CoefficientMatrix = new double[] {
|
|
-1.0/6.0, 3.0/6.0, - 3.0/6.0, 1.0/6.0,
|
|
3.0/6.0, - 6.0/6.0, 0.0/6.0, 4.0/6.0,
|
|
-3.0/6.0, 3.0/6.0, 3.0/6.0, 1.0/6.0,
|
|
1.0/6.0, 0.0/6.0, 0.0/6.0, 0.0/6.0
|
|
};
|
|
|
|
NodeIndices = new int[] { -1, 0, 1, 2 };
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// A Linear Interpolator.
|
|
/// </summary>
|
|
public class LinearInterpolator : SplineInterpolator
|
|
{
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="LinearInterpolator"/> class.
|
|
/// </summary>
|
|
public LinearInterpolator( )
|
|
{
|
|
CoefficientMatrix = new double[] {
|
|
0.0, 0.0, - 1.0, 1.0,
|
|
0.0, 0.0, 1.0, 0.0,
|
|
0.0, 0.0, 0.0, 0.0,
|
|
0.0, 0.0, 0.0, 0.0
|
|
};
|
|
|
|
NodeIndices = new int[] { 0, 1, 2, 3 };
|
|
}
|
|
}
|