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);
}
private float[] computeOffsets(int start, float[] baseline)
{
for (int i = 0; i < SEGMENT_SIZE; i++)
{
float[] point = (float[])mSensorData[start + i];
float[] o = { point[0] - baseline[0], point[1] - baseline[1], point[2] - baseline[2] };
float magnitude = (float)Math.Sqrt(o[0] * o[0] + o[1] * o[1] + o[2] * o[2]);
mOffsets[i] = magnitude;
}
return(mOffsets);
}
//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());
}
public double getHeadingDegrees()
{
double x = so3SensorFromWorld.get(2, 0);
double y = so3SensorFromWorld.get(2, 1);
double mag = Math.Sqrt(x * x + y * y);
if (mag < 0.1D)
{
return(0.0D);
}
double heading = -90.0D - Math.Atan2(y, x) / 3.141592653589793D * 180.0D;
if (heading < 0.0D)
{
heading += 360.0D;
}
if (heading >= 360.0D)
{
heading -= 360.0D;
}
return(heading);
}
public double length()
{
return(Math.Sqrt(x * x + y * y + z * z));
}
public DistortionMesh(EyeParams eye, Distortion distortion, float screenWidthM, float screenHeightM, float xEyeOffsetMScreen, float yEyeOffsetMScreen, float textureWidthM, float textureHeightM, float xEyeOffsetMTexture, float yEyeOffsetMTexture, float viewportXMTexture, float viewportYMTexture, float viewportWidthMTexture, float viewportHeightMTexture)
{
float mPerUScreen = screenWidthM;
float mPerVScreen = screenHeightM;
float mPerUTexture = textureWidthM;
float mPerVTexture = textureHeightM;
float[] vertexData = new float[8000];
int vertexOffset = 0;
for (int row = 0; row < 40; row++)
{
for (int col = 0; col < 40; col++)
{
float uTexture = col / 39.0F * (viewportWidthMTexture / textureWidthM) + viewportXMTexture / textureWidthM;
float vTexture = row / 39.0F * (viewportHeightMTexture / textureHeightM) + viewportYMTexture / textureHeightM;
float xTexture = uTexture * mPerUTexture;
float yTexture = vTexture * mPerVTexture;
float xTextureEye = xTexture - xEyeOffsetMTexture;
float yTextureEye = yTexture - yEyeOffsetMTexture;
float rTexture = (float)Math.Sqrt(xTextureEye * xTextureEye + yTextureEye * yTextureEye);
float textureToScreen = rTexture > 0.0F ? distortion.distortInverse(rTexture) / rTexture : 1.0F;
float xScreen = xTextureEye * textureToScreen + xEyeOffsetMScreen;
float yScreen = yTextureEye * textureToScreen + yEyeOffsetMScreen;
float uScreen = xScreen / mPerUScreen;
float vScreen = yScreen / mPerVScreen;
float vignetteSizeMTexture = 0.002F / textureToScreen;
float dxTexture = xTexture - DistortionRenderer.clamp(xTexture, viewportXMTexture + vignetteSizeMTexture, viewportXMTexture + viewportWidthMTexture - vignetteSizeMTexture);
float dyTexture = yTexture - DistortionRenderer.clamp(yTexture, viewportYMTexture + vignetteSizeMTexture, viewportYMTexture + viewportHeightMTexture - vignetteSizeMTexture);
float drTexture = (float)Math.Sqrt(dxTexture * dxTexture + dyTexture * dyTexture);
float vignette = 1.0F - DistortionRenderer.clamp(drTexture / vignetteSizeMTexture, 0.0F, 1.0F);
vertexData[(vertexOffset + 0)] = (2.0F * uScreen - 1.0F);
vertexData[(vertexOffset + 1)] = (2.0F * vScreen - 1.0F);
vertexData[(vertexOffset + 2)] = vignette;
vertexData[(vertexOffset + 3)] = uTexture;
vertexData[(vertexOffset + 4)] = vTexture;
vertexOffset += 5;
}
}
nIndices = 3158;
int[] indexData = new int[nIndices];
int indexOffset = 0;
vertexOffset = 0;
for (int row = 0; row < 39; row++)
{
if (row > 0)
{
indexData[indexOffset] = indexData[(indexOffset - 1)];
indexOffset++;
}
for (int col = 0; col < 40; col++)
{
if (col > 0)
{
if (row % 2 == 0)
{
vertexOffset++;
}
else
{
vertexOffset--;
}
}
indexData[(indexOffset++)] = vertexOffset;
indexData[(indexOffset++)] = (vertexOffset + 40);
}
vertexOffset += 40;
}
FloatBuffer vertexBuffer = ByteBuffer.AllocateDirect(vertexData.Length * 4).Order(ByteOrder.NativeOrder()).AsFloatBuffer();
vertexBuffer.Put(vertexData).Position(0);
IntBuffer indexBuffer = ByteBuffer.AllocateDirect(indexData.Length * 4).Order(ByteOrder.NativeOrder()).AsIntBuffer();
indexBuffer.Put(indexData).Position(0);
int[] bufferIds = new int[2];
GLES20.GlGenBuffers(2, bufferIds, 0);
mArrayBufferId = bufferIds[0];
mElementBufferId = bufferIds[1];
GLES20.GlBindBuffer(34962, mArrayBufferId);
GLES20.GlBufferData(34962, vertexData.Length * 4, vertexBuffer, 35044);
GLES20.GlBindBuffer(34963, mElementBufferId);
GLES20.GlBufferData(34963, indexData.Length * 4, indexBuffer, 35044);
GLES20.GlBindBuffer(34962, 0);
GLES20.GlBindBuffer(34963, 0);
}
