Beispiel #1
0
        public static void sO3FromMu(Vector3d w, Matrix3x3d result)
        {
            double thetaSq = Vector3d.dot(w, w);
            double theta = Math.Sqrt(thetaSq);
            double kA, kB;

            if (thetaSq < 1.0E-08D)
            {
                kA = 1.0D - 0.16666667163372D * thetaSq;
                kB = 0.5D;
            }
            else
            {
                if (thetaSq < 1.0E-06D)
                {
                    kB = 0.5D - 0.0416666679084301D * thetaSq;
                    kA = 1.0D - thetaSq * 0.16666667163372D * (1.0D - 0.16666667163372D * thetaSq);
                }
                else
                {
                    double invTheta = 1.0D / theta;
                    kA = Math.Sin(theta) * invTheta;
                    kB = (1.0D - Math.Cos(theta)) * (invTheta * invTheta);
                }
            }
            rodriguesSo3Exp(w, kA, kB, result);
        }
Beispiel #2
0
        public /*synchronized*/ void setHeadingDegrees(double heading)
        {
            double currentHeading = getHeadingDegrees();
            double deltaHeading   = heading - currentHeading;
            double s = Math.Sin(deltaHeading / 180.0D * 3.141592653589793D);
            double c = Math.Cos(deltaHeading / 180.0D * 3.141592653589793D);

            double[][] deltaHeadingRotationVals = new double[][] { new[] { c, -s, 0.0D }, new[] { s, c, 0.0D }, new[] { 0.0D, 0.0D, 1.0D } };

            arrayAssign(deltaHeadingRotationVals, setHeadingDegreesTempM1);
            Matrix3x3d.mult(so3SensorFromWorld, setHeadingDegreesTempM1, so3SensorFromWorld);
        }
Beispiel #3
0
            public static IEnumerable <byte> Resample(byte[] samples, int fromSampleRate, int toSampleRate, int quality = 10)
            {
                int srcLength  = samples.Length;
                var destLength = (long)samples.Length * toSampleRate / fromSampleRate;
                var dx         = srcLength / destLength;

                // fmax : nyqist half of destination sampleRate
                // fmax / fsr = 0.5;
                var fmaxDivSR = 0.5;
                var r_g       = 2 * fmaxDivSR;

                // Quality is half the window width
                var wndWidth2 = quality;
                var wndWidth  = quality * 2;

                var    x = 0;
                int    i, j;
                double r_y;
                int    tau;
                double r_w;
                double r_a;
                double r_snc;

                for (i = 0; i < destLength; ++i)
                {
                    r_y = 0.0;
                    for (tau = -wndWidth2; tau < wndWidth2; ++tau)
                    {
                        // input sample index
                        j = x + tau;

                        // Hann Window. Scale and calculate sinc
                        r_w   = 0.5 - 0.5 * Math.Cos(2 * Math.Pi * (0.5 + (j - x) / wndWidth));
                        r_a   = 2 * Math.Pi * (j - x) * fmaxDivSR;
                        r_snc = 1.0;
                        if (r_a != 0)
                        {
                            r_snc = Math.Sin(r_a) / r_a;
                        }

                        if ((j >= 0) && (j < srcLength))
                        {
                            r_y += r_g * r_w * r_snc * samples[j];
                        }
                    }
                    yield return((byte)r_y);

                    x += (int)dx;
                }
            }
Beispiel #4
0
        //get the distance to the closest stop matematicaly
        private double DistFrom(double latStart, double lngStart, double latEnd, double lngEnd)
        {
            double earthRadius = 3958.75;
            double dLat        = Math.ToRadians(latEnd - latStart);
            double dLng        = Math.ToRadians(lngEnd - lngStart);
            double a           = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
                                 Math.Cos(Math.ToRadians(latStart)) * Math.Cos(Math.ToRadians(latEnd)) *
                                 Math.Sin(dLng / 2) * Math.Sin(dLng / 2);
            double c    = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
            double dist = earthRadius * c;

            int meterConversion = 1609;

            //using Java.Lang.Double
            return(new JLD(dist * meterConversion).DoubleValue());
        }