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;
  }
Exemplo n.º 3
0
  /**
   * 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;
      }
    }
  }
Exemplo n.º 4
0
  @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;
    }
  }