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