Пример #1
0
  /**
   * Sets the Quaternion as a rotation from the {@code from} direction to the {@code to} direction.
   *
   * <p><b>Attention:</b> this rotation is not uniquely defined. The selected axis is usually
   * orthogonal to {@code from} and {@code to}, minimizing the rotation angle. This method is robust
   * and can handle small or almost identical vectors.
   *
   * @see #fromAxisAngle(PVector, float)
   */
  public void fromTo(PVector from, PVector to) {
    float fromSqNorm = MathUtils.squaredNorm(from);
    float toSqNorm = MathUtils.squaredNorm(to);
    // Identity Quaternion when one vector is null
    if ((fromSqNorm < 1E-10f) || (toSqNorm < 1E-10f)) {
      this.x = this.y = this.z = 0.0f;
      this.w = 1.0f;
    } else {

      PVector axis = from.cross(to);

      float axisSqNorm = MathUtils.squaredNorm(axis);

      // Aligned vectors, pick any axis, not aligned with from or to
      if (axisSqNorm < 1E-10f) axis = MathUtils.orthogonalVector(from);

      float angle = PApplet.asin(PApplet.sqrt(axisSqNorm / (fromSqNorm * toSqNorm)));

      if (from.dot(to) < 0.0) angle = PI - angle;

      fromAxisAngle(axis, angle);
    }
  }
Пример #2
0
 /**
  * Converts this Quaternion to Euler rotation angles {@code roll}, {@code pitch} and {@code yaw}
  * in radians. {@link #fromEulerAngles(float, float, float)} performs the inverse operation. The
  * code was adapted from: http://www.euclideanspace.com/maths/geometry
  * /rotations/conversions/quaternionToEuler/index.htm.
  *
  * <p><b>Attention:</b> This method assumes that this Quaternion is normalized.
  *
  * @return the PVector holding the roll (x coordinate of the vector), pitch (y coordinate of the
  *     vector) and yaw angles (z coordinate of the vector). <b>Note:</b> The order of the
  *     rotations that would produce this Quaternion (i.e., as with {@code fromEulerAngles(roll,
  *     pitch, yaw)}) is: y->z->x.
  * @see #fromEulerAngles(float, float, float)
  */
 public PVector eulerAngles() {
   float roll, pitch, yaw;
   float test = x * y + z * w;
   if (test > 0.499) { // singularity at north pole
     pitch = 2 * PApplet.atan2(x, w);
     yaw = PApplet.PI / 2;
     roll = 0;
     return new PVector(roll, pitch, yaw);
   }
   if (test < -0.499) { // singularity at south pole
     pitch = -2 * PApplet.atan2(x, w);
     yaw = -PApplet.PI / 2;
     roll = 0;
     return new PVector(roll, pitch, yaw);
   }
   float sqx = x * x;
   float sqy = y * y;
   float sqz = z * z;
   pitch = PApplet.atan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz);
   yaw = PApplet.asin(2 * test);
   roll = PApplet.atan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz);
   return new PVector(roll, pitch, yaw);
 }