public void getYawPitchRoll(DoubleYoVariable yaw, DoubleYoVariable pitch, DoubleYoVariable roll) { double pitchArgument = -2.0 * q_qx.getDoubleValue() * q_qz.getDoubleValue() + 2.0 * q_qs.getDoubleValue() * q_qy.getDoubleValue(); pitch.set(FastMath.asin(pitchArgument)); if (Math.abs(pitch.getDoubleValue()) < 0.49 * Math.PI) { yaw.set( FastMath.atan2( 2.0 * q_qx.getDoubleValue() * q_qy.getDoubleValue() + 2.0 * q_qz.getDoubleValue() * q_qs.getDoubleValue(), 1.0 - 2.0 * q_qy.getDoubleValue() * q_qy.getDoubleValue() - 2.0 * q_qz.getDoubleValue() * q_qz.getDoubleValue())); // Math.asin(q_qs.val * q_qz.val * 2.0); roll.set( FastMath.atan2( 2.0 * q_qy.getDoubleValue() * q_qz.getDoubleValue() + 2.0 * q_qx.getDoubleValue() * q_qs.getDoubleValue(), 1.0 - 2.0 * q_qx.getDoubleValue() * q_qx.getDoubleValue() - 2.0 * q_qy.getDoubleValue() * q_qy.getDoubleValue())); // Math.asin(q_qs.val * q_qx.val * 2.0); } else { yaw.set(2.0 * FastMath.atan2(q_qz.getDoubleValue(), q_qs.getDoubleValue())); roll.set(0.0); } }
public double[] getYawPitchRoll() { double[] yawPitchRollToReturn = new double[3]; double pitchArgument = -2.0 * q_qx.getDoubleValue() * q_qz.getDoubleValue() + 2.0 * q_qs.getDoubleValue() * q_qy.getDoubleValue(); double pitch = 0.0, roll = 0.0, yaw = 0.0; pitch = FastMath.asin(pitchArgument); if (Math.abs(pitch) < 0.49 * Math.PI) { yaw = FastMath.atan2( 2.0 * q_qx.getDoubleValue() * q_qy.getDoubleValue() + 2.0 * q_qz.getDoubleValue() * q_qs.getDoubleValue(), 1.0 - 2.0 * q_qy.getDoubleValue() * q_qy.getDoubleValue() - 2.0 * q_qz.getDoubleValue() * q_qz.getDoubleValue()); // Math.asin(q_qs.val * q_qz.val * 2.0); roll = FastMath.atan2( 2.0 * q_qy.getDoubleValue() * q_qz.getDoubleValue() + 2.0 * q_qx.getDoubleValue() * q_qs.getDoubleValue(), 1.0 - 2.0 * q_qx.getDoubleValue() * q_qx.getDoubleValue() - 2.0 * q_qy.getDoubleValue() * q_qy.getDoubleValue()); // Math.asin(q_qs.val * q_qx.val * 2.0); } else { yaw = 2.0 * FastMath.atan2(q_qz.getDoubleValue(), q_qs.getDoubleValue()); roll = 0.0; } yawPitchRollToReturn[0] = yaw; yawPitchRollToReturn[1] = pitch; yawPitchRollToReturn[2] = roll; return yawPitchRollToReturn; }
/** * Returns 1.0 if the given coordinate is completely inside the ellipse, 0.0 if it is completely * outside, and a number between 0.0 and 1.0 if it is in the blurred area */ public double isOutside(int x, int y) { double dx = x - cx; double dy = y - cy; double dist2 = dx * dx + dy * dy; if (linkedRadius) { // circle if (dist2 > outerRadius2) { // outside return 1.0; } else if (dist2 < innerRadius2) { // innermost region return 0.0; } else { // between the inner and outer radius double distance = Math.sqrt(dist2); double ratio = (distance - innerRadiusY) / yRadiusDifference; // gives a value between 0 and 1 // double trigRatio = (FastMath.cos(ratio * Math.PI) + 1.0) / 2.0; // 1- smooth step is faster than cosine interpolation // http://en.wikipedia.org/wiki/Smoothstep // http://www.wolframalpha.com/input/?i=Plot[{%281+%2B+Cos[a+*+Pi]%29%2F2%2C+1+-+3+*+a+*+a+%2B+2+*+a+*+a+*a}%2C+{a%2C+0%2C+1}] double trigRatio = 1 + ratio * ratio * (2 * ratio - 3); return trigRatio; } } else { // ellipsis double dx2 = dx * dx; double dy2 = dy * dy; if (dy2 >= (outerRadiusY2 - (outerRadiusY2 * dx2) / outerRadiusX2)) { // outside return 1.0; } if (dy2 <= (innerRadiusY2 - (innerRadiusY2 * dx2) / innerRadiusX2)) { // innermost region return 0.0; } else { // between the inner and outer radius // we are on an ellipse with unknown a and b semi major/minor axes // but we know that a/b = outerRadiusX/outerRadiusY double ellipseDistortion = outerRadiusX / outerRadiusY; double b = Math.sqrt(ellipseDistortion * ellipseDistortion * dy2 + dx2) / ellipseDistortion; // now we can calculate how far away we are between the two ellipses double ratio = (b - innerRadiusY) / yRadiusDifference; // gives a value between 0 and 1 double trigRatio = (FastMath.cos(ratio * Math.PI) + 1.0) / 2.0; return trigRatio; } } }
@Override protected void transformInverse(int x, int y, float[] out) { float dx = x - icentreX; float dy = y - icentreY; float distance = dx * dx + dy * dy; if (distance > radius2 || distance == 0) { // out[0] = x; // out[1] = y; double angle = FastMath.atan2(dy, dx); angle += rotateResultAngle; double r = Math.sqrt(distance); double zoomedR = r / zoom; float u = (float) (zoomedR * FastMath.cos(angle)); float v = (float) (zoomedR * FastMath.sin(angle)); out[0] = (u + icentreX); out[1] = (v + icentreY); } else { float scaledDist = (float) Math.sqrt(distance / radius2); float pinchBulgeFactor = (float) FastMath.pow(FastMath.sin(Math.PI * 0.5 * scaledDist), -pinchBulgeAmount); // pinch-bulge dx *= pinchBulgeFactor; dy *= pinchBulgeFactor; // twirl float e = 1 - scaledDist; float a = angle * e * e; a += rotateResultAngle; float sin = (float) FastMath.sin(a); float cos = (float) FastMath.cos(a); float u = (cos * dx - sin * dy) / zoom; float v = (sin * dx + cos * dy) / zoom; out[0] = icentreX + u; out[1] = icentreY + v; } }