public void TestVelocityOnPlane()
{
Frisbee.SimulationState st = new Frisbee.SimulationState
{
VX = 1,
VY = 1,
Theta = Math.PI / 4
};
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 }
});
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);
}
public void CanScaleAVectorWhenSettingPreviousNonzeroElementsToZero()
{
var vector = new SparseVector(20);
vector[10] = 1.0f;
vector[11] = 2.0f;
vector[11] = 0.0f;
var scaled = new SparseVector(20);
vector.Multiply(3.0f, scaled);
Assert.AreEqual(3.0f, scaled[10]);
Assert.AreEqual(0.0f, scaled[11]);
}
/// <summary>
/// Generate a <see cref="Sample"/> containing
/// a <see cref="SparseVector"/> of random numbers.
/// </summary>
/// <returns>A <see cref="SparseVector"/> of random numbers.</returns>
public Sample NextSample()
{
// generate and consume the necessary samples
// TODO: SparseVector.Random(generator)
var r = new double[Count];
_generator.Next(r);
var samples = new SparseVector(r, false /* no copy, consume */);
if (_sqrtCovariance != null)
{
samples = _sqrtCovariance * samples; // general case
}
else
{
samples.Multiply(_sqrtVariance); // degenerate case
}
return(new Sample(samples));
}
/// <summary>
/// Generate next paths.
/// </summary>
/// <returns>
/// An array containing <see cref="PathGenerator.Count"/> <see cref="Path"/>s,
/// wrapped in a (possibly weighted) <see cref="Sample"/>.
/// </returns>
public override Sample Next()
{
Matrix diffusion = new Matrix(Count, TimeSteps);
double weight = 1.0;
for (int i = 0; i < TimeSteps; i++)
{
Sample sample = ArrayGenerator.NextSample(); // get a weighted SparseVector
weight *= sample.Weight;
SparseVector v = (SparseVector)sample.Value;
v.Multiply(Math.Sqrt(TimeDelays.Data[i]));
v.Copy(diffusion.Column(i));
}
Path[] paths = new Path[Count];
for (int j = 0; j < Count; j++)
{
paths[j] = new Path(Times, Drift[j], diffusion.Row(j));
}
return(new Sample(paths, weight));
}
public void CanScaleAVectorWhenSettingPreviousNonzeroElementsToZero()
{
var vector = new SparseVector(20);
vector[10] = 1.0;
vector[11] = 2.0;
vector[11] = 0.0;
var scaled = new SparseVector(20);
vector.Multiply(3.0, scaled);
Assert.AreEqual(3.0, scaled[10].Real);
Assert.AreEqual(0.0, scaled[11].Real);
}
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);
}
