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);
}
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);
}
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;
}
}
//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());
}