コード例 #1
0
        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);
        }
コード例 #2
0
        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]);
        }
コード例 #3
0
        /// <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));
        }
コード例 #4
0
        /// <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));
        }
コード例 #5
0
        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);
        }
コード例 #6
0
        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);
        }