/// <summary>
/// Create a linear spline interpolation based on arbitrary points (sorted ascending).
/// </summary>
/// <param name="points">The sample points t, sorted ascending. Supports both lists and arrays.</param>
/// <param name="values">The sample point values x(t). Supports both lists and arrays.</param>
/// <returns>
/// An interpolation scheme optimized for the given sample points and values,
/// which can then be used to compute interpolations and extrapolations
/// on arbitrary points.
/// </returns>
public static IInterpolation LinearBetweenPoints(IList <double> points, IList <double> values)
{
var method = new LinearSplineInterpolation();
method.Initialize(points, values);
return(method);
}
public XYPoint GetPointAtChainage(double chainage)
{
var distinctpoints = Points.DistinctBy(pp => pp.Chainage).ToList();
LinearSplineInterpolation lspx = new LinearSplineInterpolation(distinctpoints.Select(xc => xc.Chainage).ToList(), distinctpoints.Select(xc => xc.X).ToList());
LinearSplineInterpolation lspy = new LinearSplineInterpolation(distinctpoints.Select(xc => xc.Chainage).ToList(), distinctpoints.Select(xc => xc.Y).ToList());
return(new XYPoint(lspx.Interpolate(chainage), lspy.Interpolate(chainage)));
}
public void FitsAtArbitraryPointsWithMaple(double t, double x, double maxAbsoluteError)
{
IInterpolation interpolation = new LinearSplineInterpolation(_t, _x);
Assert.AreEqual(x, interpolation.Interpolate(t), maxAbsoluteError, "Interpolation at {0}", t);
var actual = interpolation.DifferentiateAll(t);
Assert.AreEqual(x, actual.Item1, maxAbsoluteError, "Interpolation as by-product of differentiation at {0}", t);
}
CreateLinearSpline(
IList <double> points,
IList <double> values
)
{
LinearSplineInterpolation method = new LinearSplineInterpolation();
method.Init(points, values);
return(method);
}
public void FitsAtArbitraryPointsWithMaple(double t, double x, double maxAbsoluteError)
{
IInterpolation interpolation = new LinearSplineInterpolation(_t, _x);
Assert.AreEqual(x, interpolation.Interpolate(t), maxAbsoluteError, "Interpolation at {0}", t);
double interpolatedValue;
double secondDerivative;
interpolation.Differentiate(t, out interpolatedValue, out secondDerivative);
Assert.AreEqual(x, interpolatedValue, maxAbsoluteError, "Interpolation as by-product of differentiation at {0}", t);
}
public void SupportsLinearCase(int samples)
{
double[] x, y, xtest, ytest;
LinearInterpolationCase.Build(out x, out y, out xtest, out ytest, samples);
IInterpolation interpolation = new LinearSplineInterpolation(x, y);
for (int i = 0; i < xtest.Length; i++)
{
Assert.AreEqual(ytest[i], interpolation.Interpolate(xtest[i]), 1e-15, "Linear with {0} samples, sample {1}", samples, i);
}
}
public void FitsAtSamplePoints()
{
IInterpolation interpolation = new LinearSplineInterpolation(_t, _x);
for (int i = 0; i < _x.Length; i++)
{
Assert.AreEqual(_x[i], interpolation.Interpolate(_t[i]), "A Exact Point " + i);
var actual = interpolation.DifferentiateAll(_t[i]);
Assert.AreEqual(_x[i], actual.Item1, "B Exact Point " + i);
}
}
public void FitsAtArbitraryPointsWithMaple(double t, double x, double maxAbsoluteError)
{
IInterpolation interpolation = new LinearSplineInterpolation(_t, _x);
Assert.AreEqual(x, interpolation.Interpolate(t), maxAbsoluteError, "Interpolation at {0}", t);
double interpolatedValue;
double secondDerivative;
interpolation.Differentiate(t, out interpolatedValue, out secondDerivative);
Assert.AreEqual(x, interpolatedValue, maxAbsoluteError, "Interpolation as by-product of differentiation at {0}", t);
}
/// <summary>
/// Interpolation of the interface points onto the equidistant Fourier points
/// </summary>
protected void InterpolateOntoFourierPoints(MultidimensionalArray interP, double[] samplP)
{
int numP = interP.Lengths[0];
// set interpolation data
double[] independentVal = new double[numP + 2];
double[] dependentVal = new double[numP + 2];
for (int sp = 1; sp <= numP; sp++)
{
independentVal[sp] = interP[sp - 1, 0];
dependentVal[sp] = interP[sp - 1, 1];
}
// extend the interpolation data for sample points at the boundary of the domain
independentVal[0] = interP[numP - 1, 0] - DomainSize;
dependentVal[0] = interP[numP - 1, 1];
independentVal[numP + 1] = interP[0, 0] + DomainSize;
dependentVal[numP + 1] = interP[0, 1];
switch (this.InterpolationType)
{
case Interpolationtype.LinearSplineInterpolation:
LinearSplineInterpolation LinSpline = new LinearSplineInterpolation();
LinSpline.Initialize(independentVal, dependentVal);
for (int sp = 0; sp < numFp; sp++)
{
samplP[sp] = LinSpline.Interpolate(FourierP[sp]);
//invDFT_coeff[sp] = (Complex)samplP[sp];
}
break;
case Interpolationtype.CubicSplineInterpolation:
CubicSplineInterpolation CubSpline = new CubicSplineInterpolation();
CubSpline.Initialize(independentVal, dependentVal);
for (int sp = 0; sp < numFp; sp++)
{
samplP[sp] = CubSpline.Interpolate(FourierP[sp]);
//invDFT_coeff[sp] = (Complex)samplP[sp];
}
break;
default:
throw new NotImplementedException();
}
}
public void FitsAtArbitraryPointsWithMaple(
[Values(-2.4, -0.9, -0.5, -0.1, 0.1, 0.4, 1.2, 10.0, -10.0)] double t,
[Values(.6, 1.7, .5, -.7, -.9, -.6, .2, 9.0, -7.0)] double x,
[Values(1e-15, 1e-15, 1e-15, 1e-15, 1e-15, 1e-15, 1e-15, 1e-15, 1e-15)] double maxAbsoluteError)
{
IInterpolation interpolation = new LinearSplineInterpolation(_t, _x);
Assert.AreEqual(x, interpolation.Interpolate(t), maxAbsoluteError, "Interpolation at {0}", t);
double interpolatedValue;
double secondDerivative;
interpolation.Differentiate(t, out interpolatedValue, out secondDerivative);
Assert.AreEqual(x, interpolatedValue, maxAbsoluteError, "Interpolation as by-product of differentiation at {0}", t);
}
public void FitsAtSamplePoints()
{
IInterpolation interpolation = new LinearSplineInterpolation(_t, _x);
for (int i = 0; i < _x.Length; i++)
{
Assert.AreEqual(_x[i], interpolation.Interpolate(_t[i]), "A Exact Point " + i);
double interpolatedValue;
double secondDerivative;
interpolation.Differentiate(_t[i], out interpolatedValue, out secondDerivative);
Assert.AreEqual(_x[i], interpolatedValue, "B Exact Point " + i);
}
}
public void FitsAtSamplePoints()
{
IInterpolation interpolation = new LinearSplineInterpolation(_t, _x);
for (int i = 0; i < _x.Length; i++)
{
Assert.AreEqual(_x[i], interpolation.Interpolate(_t[i]), "A Exact Point " + i);
double interpolatedValue;
double secondDerivative;
interpolation.Differentiate(_t[i], out interpolatedValue, out secondDerivative);
Assert.AreEqual(_x[i], interpolatedValue, "B Exact Point " + i);
}
}
/// <summary>
/// Gets the bottomlevel of the branch. Interpolates linearly between points
/// </summary>
/// <param name="chainage"></param>
/// <returns></returns>
public double GetBottomLevelAtChainage(double chainage)
{
if (chainage <= ChainageStart)
{
return(CrossSections.First().BottomLevel);
}
if (chainage >= ChainageEnd)
{
return(CrossSections.Last().BottomLevel);
}
LinearSplineInterpolation lsp = new LinearSplineInterpolation(CrossSections.Select(xc => xc.Chainage).ToList(), CrossSections.Select(xc => xc.BottomLevel).ToList());
return(lsp.Interpolate(chainage));
}
public void FitsAtArbitraryPointsWithMaple(
[Values(-2.4, -0.9, -0.5, -0.1, 0.1, 0.4, 1.2, 10.0, -10.0)] double t,
[Values(.6, 1.7, .5, -.7, -.9, -.6, .2, 9.0, -7.0)] double x,
[Values(1e-15, 1e-15, 1e-15, 1e-15, 1e-15, 1e-15, 1e-15, 1e-15, 1e-15)] double maxAbsoluteError)
{
IInterpolation interpolation = new LinearSplineInterpolation(_t, _x);
Assert.AreEqual(x, interpolation.Interpolate(t), maxAbsoluteError, "Interpolation at {0}", t);
double interpolatedValue;
double secondDerivative;
interpolation.Differentiate(t, out interpolatedValue, out secondDerivative);
Assert.AreEqual(x, interpolatedValue, maxAbsoluteError, "Interpolation as by-product of differentiation at {0}", t);
}
public void FitsAtSamplePoints()
{
IInterpolation interpolation = new LinearSplineInterpolation(_t, _x);
for (int i = 0; i < _x.Length; i++)
{
Assert.AreEqual(_x[i], interpolation.Interpolate(_t[i]), "A Exact Point " + i);
var actual = interpolation.DifferentiateAll(_t[i]);
Assert.AreEqual(_x[i], actual.Item1, "B Exact Point " + i);
}
}
/// <summary>
/// Interpolation of no equidistant sample point onto equidistant Fourier points
/// </summary>
public static void InterpolateOntoFourierPoints(MultidimensionalArray samplP, double DomainSize, double[] samplFp)
{
int numSp = samplP.Lengths[0];
// set interpolation data (delete multiple independent values)
ArrayList independentList = new ArrayList();
ArrayList dependentList = new ArrayList();
for (int sp = 0; sp < numSp; sp++)
{
if (independentList.Contains(samplP[sp, 0]) == false)
{
independentList.Add(samplP[sp, 0]);
dependentList.Add(samplP[sp, 1]);
}
}
// extend the interpolation data for sample points at the boundary of the domain
independentList.Insert(0, samplP[numSp - 1, 0] - DomainSize);
independentList.Insert(independentList.Count, samplP[0, 0] + DomainSize);
dependentList.Insert(0, samplP[numSp - 1, 1]);
dependentList.Insert(dependentList.Count, samplP[0, 1]);
double[] independentVal = (double[])independentList.ToArray(typeof(double));
double[] dependentVal = (double[])dependentList.ToArray(typeof(double));
// set Fourier points
int numSFp = samplFp.Length;
double[] FourierP = new double[numSFp];
for (int i = 0; i < numSFp; i++)
{
FourierP[i] = (DomainSize / numSFp) * i;
}
switch (InterpolationType)
{
case Interpolationtype.LinearSplineInterpolation:
LinearSplineInterpolation LinSpline = new LinearSplineInterpolation();
LinSpline.Initialize(independentVal, dependentVal);
for (int Fp = 0; Fp < numSFp; Fp++)
{
samplFp[Fp] = LinSpline.Interpolate(FourierP[Fp]);
}
break;
case Interpolationtype.CubicSplineInterpolation:
CubicSplineInterpolation CubSpline = new CubicSplineInterpolation();
CubSpline.Initialize(independentVal, dependentVal);
for (int Fp = 0; Fp < numSFp; Fp++)
{
samplFp[Fp] = CubSpline.Interpolate(FourierP[Fp]);
}
break;
default:
throw new NotImplementedException();
}
}
public void Constructor_SamplePointsNotStrictlyAscending_Throws()
{
var x = new[] { -1.0, 0.0, 1.5, 1.5, 2.5, 4.0 };
var y = new[] { 1.0, 0.3, -0.7, -0.6, -0.1, 0.4 };
var interpolation = new LinearSplineInterpolation(x, y);
}
/// <summary>
/// Create a linear spline interpolation based on arbitrary points.
/// </summary>
/// <param name="points">The sample points t. Supports both lists and arrays.</param>
/// <param name="values">The sample point values x(t). Supports both lists and arrays.</param>
/// <returns>
/// An interpolation scheme optimized for the given sample points and values,
/// which can then be used to compute interpolations and extrapolations
/// on arbitrary points.
/// </returns>
public static IInterpolationMethod CreateLinearSpline(
IList<double> points,
IList<double> values
)
{
LinearSplineInterpolation method = new LinearSplineInterpolation();
method.Init(points, values);
return method;
}
public void FitsAtArbitraryPointsWithMaple(double t, double x, double maxAbsoluteError)
{
IInterpolation interpolation = new LinearSplineInterpolation(_t, _x);
Assert.AreEqual(x, interpolation.Interpolate(t), maxAbsoluteError, "Interpolation at {0}", t);
var actual = interpolation.DifferentiateAll(t);
Assert.AreEqual(x, actual.Item1, maxAbsoluteError, "Interpolation as by-product of differentiation at {0}", t);
}
public double[] Simulate(double t, double[] values)
{
SimulationState st = new SimulationState
{
VX = values[3],
VY = values[4],
VZ = values[5],
Phi = values[6],
Theta = values[7],
PhiDot = values[8],
ThetaDot = values[9],
GammaDot = values[10],
Gamma = values[11],
};
//double CLo, CLa, CDo, CDa, CMo, CMa;
//double CL_data, CD_data, CM_data;
double[] CRr_rad = new[] { -0.0873, -0.0698, -0.0524, -0.0349, -0.0175, 0.0000, 0.0175, 0.0349, 0.0524, 0.0698, 0.0873, 0.1047, 0.1222, 0.1396, 0.1571, 0.1745, 0.1920, 0.2094, 0.2269, 0.2443, 0.2618, 0.5236 };
double[] CRr_AdvR = new[] { 2, 1.04, 0.69, 0.35, 0.17, 0 };
double[,] CRr_data = new [, ]
{
{ -0.0172, -0.0192, -0.018, -0.0192, -0.018, -0.0172, -0.0172, -0.0168, -0.0188, -0.0164, -0.0136, -0.01, -0.0104, -0.0108, -0.0084, -0.008, -0.008, -0.006, -0.0048, -0.0064, -0.008, -0.003 },
{ -0.0112, -0.0132, -0.012, -0.0132, -0.012, -0.0112, -0.0112, -0.0108, -0.0128, -0.0104, -0.0096, -0.0068, -0.0072, -0.0076, -0.0052, -0.0048, -0.0048, -0.0028, -0.0032, -0.0048, -0.0064, -0.003 },
{ -0.0056, -0.0064, -0.0064, -0.0068, -0.0064, -0.0064, -0.0052, -0.0064, -0.0028, -0.0028, -0.004, -0.002, -0.004, -0.002, -0.0016, 0, 0, 0, 0, -0.002, -0.0048, -0.003 },
{ -0.0012, -0.0016, -0.0004, -0.0028, -0.0016, -0.0016, -0.0004, 0.0004, 0.0004, 0.0008, 0.0004, 0.0008, 0.0012, 0.0008, 0.002, 0.0028, 0.0032, 0.0024, 0.0028, 0.0004, -0.0012, -0.003 },
{ -0.0012, -0.0012, -0.0016, -0.0016, -0.0012, -0.0004, 0.0004, 0.0008, 0.0008, 0.0016, 0.0004, 0.002, 0.0004, 0.0016, 0.002, 0.002, 0.002, 0.0012, 0.0012, 0, -0.0012, -0.003 },
{ -0.0012, -0.0012, -0.0004, -0.0008, -0.0008, -0.0008, 0.0004, 0.0004, 0.0004, 0.0008, 0.0004, 0.0008, -0.0004, 0, 0, 0.0004, 0, 0, 0.0004, -0.002, -0.0012, -0.003 }
};
const double CMq = -0.005;
const double CRp = -0.0055;
const double CNr = 0.0000071;
double diameter = 2 * Math.Sqrt(Area / Math.PI);
// Rotation matrix: http://s-mat-pcs.oulu.fi/~mpa/matreng/eem1_3-7.htm
// y
// ------------------> x ^
// |\ |
// | \ |
// | \ |
// | \ theta = pitch | gamma = yaw
// | |
// v --------------------> x
// z
// z
// ^
// |
// |
// |
// | phi = roll
// |
// ------------------> y
//
// 3D homogenous transformation matrix
//
// g = gamma = yaw
// t = theta = pitch
// p = phi = roll
//
// http://en.wikipedia.org/wiki/Rotation_matrix
// http://www.gregslabaugh.name/publications/euler.pdf
// -- --
// | cos(g)*cos(t), cos(g)*sin(t)*sin(p)-sin(g)*cos(p), cos(g)*sin(t)*cos(p)+sin(g)*sin(p) |
// | |
// T = | sin(g)*cos(t), sin(g)*sin(t)*sin(p)-cos(g)*cos(p), sin(g)*sin(t)*cos(p)+cos(g)*sin(p) |
// | |
// | -sin(t) , cos(t)*sin(p) , cos(t)*cos(p) |
// -- --
//
// With g = yaw = 0 and sin(t) = -sin(t) since z is positive downward
//
// -- --
// | cos(t) , sin(t)*sin(p) , sin(t)*cos(p) |
// | |
// T = | 0 , cos(p) , sin(p) |
// | |
// | -sin(t) , cos(t)*sin(p) , cos(t)*cos(p) |
// -- --
Matrix <double> transformation = new SparseMatrix(new [, ]
{
{ st.CosTheta, st.SinTheta * st.SinPhi, -st.SinTheta * st.CosPhi },
{ 0, st.CosPhi, st.SinPhi },
{ st.SinTheta, -st.CosTheta * st.SinPhi, st.CosTheta * st.CosPhi }
});
// Eigenvector & eigenvalue
// --
// | x1
// X = | x2
// | x3
// --
//
// --
// | a11, a12, a13
// A = | a21, a22, a23
// | a31, a32, a33
// --
//
//
// Usually, the multiplication of a vector x by a square matrix A changes both the magnitude and the direction
// of the vector upon which it acts; but in the special case where it changes only the scale (magnitude) of the
// vector and leaves the direction unchanged, or switches the vector to the opposite direction, then that vector
// is called an eigenvector of that matrix (the term "eigenvector" is meaningless except in relation to some
// particular matrix). When multiplied by a matrix, each eigenvector of that matrix changes its magnitude by a
// factor, called the eigenvalue corresponding to that eigenvector.
//
//
// See local frame vs global frame:
//
Evd evd = new UserEvd(transformation);
//Matrix<double> eigenVectors = evd.EigenVectors();
Vector <Complex> temp = evd.EigenValues();
Vector <double> eigenValues = new SparseVector(3);
eigenValues[0] = temp[0].Real;
eigenValues[1] = temp[1].Real;
eigenValues[1] = temp[1].Real;
//eigenValues.Norm
//
// If you have Theta and Phi = 0 you have a transformation matrix like this:
//
// | 1, 0, 0 |
// | 0, 1, 0 |
// | 0, 0, 1 |
//
// So each row represents the rotated X, Y or Z axis expressed as N-Frame coordinates. In this case,
// there is no rotation so you have the axis (1,0,0), (0,1,0), (0,0,1).
// For example, the first row represents the X Axis after the rotation. Since the rotation is 0,
// the X axis is a vector (1,0,0) in the N-Frame.
//
//
//
//
//SparseVector c1 = new SparseVector(transformation.Row(0));
//SparseVector c2 = new SparseVector(transformation.Row(1));
SparseVector c3 = new SparseVector(transformation.Row(2));
SparseVector velocity = new SparseVector(new [] { st.VX, st.VY, st.VZ });
double velocityMagnitude = velocity.Norm(2);
double velocityC3 = velocity.DotProduct(c3);
Vector <double> vp = velocity.Subtract(c3.Multiply(velocityC3));
double vpMagnitude = vp.Norm(2);
double alpha = Math.Atan(velocityC3 / vp.Norm(2));
double adp = Area * Rho * velocityMagnitude * velocityMagnitude / 2;
Vector <double> unitVelocity = velocity.Divide(velocityMagnitude);
Vector <double> unitVp = vp.Divide(vpMagnitude);
//c3.
Vector <double> unitLat = ConvertVector(Vector3D.CrossProduct(ConvertVector(c3), ConvertVector(unitVp)));
Matrix <double> omegaD_N_inC = new SparseMatrix(new [, ] {
{ st.PhiDot *st.CosTheta, st.ThetaDot, st.PhiDot *st.SinTheta + st.GammaDot }
}); // expressed in c1,c2,c3
Vector <double> omegaD_N_inN = transformation.Transpose().Multiply(omegaD_N_inC.Transpose()).Column(0); // expressed in c1,c2,c3
double omegaVp = omegaD_N_inN.DotProduct(unitVp);
double omegaLat = omegaD_N_inN.DotProduct(unitLat);
double omegaSpin = omegaD_N_inN.DotProduct(c3);
double aDvR = diameter * omegaSpin / 2 / vpMagnitude;
LinearSplineInterpolation interpolation = new LinearSplineInterpolation(m_xCL, m_yCL);
double CL = interpolation.Interpolate(alpha);
interpolation = new LinearSplineInterpolation(m_xCD, m_yCD);
double CD = interpolation.Interpolate(alpha);
interpolation = new LinearSplineInterpolation(m_xCM, m_yCM);
double CM = interpolation.Interpolate(alpha);
alglib.spline2d.spline2dinterpolant interpolant = new alglib.spline2d.spline2dinterpolant();
alglib.spline2d.spline2dbuildbilinear(CRr_rad, CRr_AdvR, CRr_data, 6, 22, interpolant);
double CRr = alglib.spline2d.spline2dcalc(interpolant, alpha, aDvR);
Vector <double> mvp = unitVp.Multiply(adp * diameter * (CRr * diameter * omegaSpin / 2 / velocityMagnitude + CRp * omegaVp)); // Roll moment, expressed in N
double lift = CL * adp;
double drag = CD * adp;
Vector <double> unitLift = -ConvertVector(Vector3D.CrossProduct(ConvertVector(unitVelocity), ConvertVector(unitLat)));
Vector <double> unitDrag = -unitVelocity;
Vector <double> forceAerodynamic = unitLift.Multiply(lift).Add(unitDrag.Multiply(drag));
Vector <double> gravityForceN = new SparseVector(new[] { 0, 0, m * g });
Vector <double> force = forceAerodynamic.Add(gravityForceN);
Vector <double> mLat = unitLat.Multiply(adp * diameter * (CM + CMq * omegaLat));
Vector <double> mSpin = new SparseVector(new [] { 0, 0, CNr * omegaSpin }); // TODO: Check if missing element
Vector <double> mvpInC = transformation.Multiply(mvp);
Vector <double> mLatInC = transformation.Multiply(mLat);
Vector <double> moment = mvpInC.Add(mLatInC).Add(mSpin);
Vector <double> acceleration = force.Divide(m);
double[] result = new double[12];
result[0] = velocity[0];
result[1] = velocity[1];
result[2] = velocity[2];
result[3] = acceleration[0];
result[4] = acceleration[1];
result[5] = acceleration[2];
result[6] = -st.PhiDot;
result[7] = st.ThetaDot;
result[8] = (moment[0] + Id * st.ThetaDot * st.PhiDot * st.SinTheta -
Ia * st.ThetaDot * (st.PhiDot * st.SinTheta + st.GammaDot) + Id * st.ThetaDot * st.PhiDot * st.SinTheta) / Id /
st.CosTheta;
result[9] = (moment[1] + Ia * st.PhiDot * st.CosTheta * (st.PhiDot * st.SinTheta + st.GammaDot) - Id * st.PhiDot * st.PhiDot * st.CosTheta * st.SinTheta) / Id;
result[10] = (moment[2] - Ia * (result[9] * st.SinTheta + st.ThetaDot * st.PhiDot * st.CosTheta)) / Ia;
result[11] = result[10];
return(result);
}
public void SupportsLinearCase([Values(2, 4, 12)] int samples)
{
double[] x, y, xtest, ytest;
LinearInterpolationCase.Build(out x, out y, out xtest, out ytest, samples);
IInterpolation interpolation = new LinearSplineInterpolation(x, y);
for (int i = 0; i < xtest.Length; i++)
{
Assert.AreEqual(ytest[i], interpolation.Interpolate(xtest[i]), 1e-15, "Linear with {0} samples, sample {1}", samples, i);
}
}
public void Constructor_SamplePointsNotStrictlyAscending_Throws()
{
var x = new[] { -1.0, 0.0, 1.5, 1.5, 2.5, 4.0 };
var y = new[] { 1.0, 0.3, -0.7, -0.6, -0.1, 0.4 };
var interpolation = new LinearSplineInterpolation(x, y);
}
