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;
}
}