예제 #1
0
 public Quaternion(final Quaternion q) {
   set(q);
 }
예제 #2
0
 public Quaternion(final float x, final float y, final float z, final float w) {
   set(x, y, z, w);
 }
예제 #3
0
  /**
   * Set this quaternion to a spherical linear interpolation between the given start and end
   * quaternions by the given change amount.
   *
   * <p>Note: Method <i>does not</i> normalize this quaternion!
   *
   * @param a start quaternion
   * @param b end quaternion
   * @param changeAmnt float between 0 and 1 representing interpolation.
   * @return this quaternion for chaining.
   * @see <a
   *     href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/">euclideanspace.com-QuaternionSlerp</a>
   */
  public final Quaternion setSlerp(final Quaternion a, final Quaternion b, final float changeAmnt) {
    // System.err.println("Slerp.0: A "+a+", B "+b+", t "+changeAmnt);
    if (changeAmnt == 0.0f) {
      set(a);
    } else if (changeAmnt == 1.0f) {
      set(b);
    } else {
      float bx = b.x;
      float by = b.y;
      float bz = b.z;
      float bw = b.w;

      // Calculate angle between them (quat dot product)
      float cosHalfTheta = a.x * bx + a.y * by + a.z * bz + a.w * bw;

      final float scale0, scale1;

      if (cosHalfTheta >= 0.95f) {
        // quaternions are close, just use linear interpolation
        scale0 = 1.0f - changeAmnt;
        scale1 = changeAmnt;
        // System.err.println("Slerp.1: Linear Interpol; cosHalfTheta "+cosHalfTheta);
      } else if (cosHalfTheta <= -0.99f) {
        // the quaternions are nearly opposite,
        // we can pick any axis normal to a,b to do the rotation
        scale0 = 0.5f;
        scale1 = 0.5f;
        // System.err.println("Slerp.2: Any; cosHalfTheta "+cosHalfTheta);
      } else {
        // System.err.println("Slerp.3: cosHalfTheta "+cosHalfTheta);
        if (cosHalfTheta
            <= -FloatUtil.EPSILON) { // FIXME: .. or shall we use the upper bound 'cosHalfTheta <
          // FloatUtil.EPSILON' ?
          // Negate the second quaternion and the result of the dot product (Inversion)
          bx *= -1f;
          by *= -1f;
          bz *= -1f;
          bw *= -1f;
          cosHalfTheta *= -1f;
          // System.err.println("Slerp.4: Inverted cosHalfTheta "+cosHalfTheta);
        }
        final float halfTheta = FloatUtil.acos(cosHalfTheta);
        final float sinHalfTheta = FloatUtil.sqrt(1.0f - cosHalfTheta * cosHalfTheta);
        // if theta = 180 degrees then result is not fully defined
        // we could rotate around any axis normal to qa or qb
        if (Math.abs(sinHalfTheta) < 0.001f) { // fabs is floating point absolute
          scale0 = 0.5f;
          scale1 = 0.5f;
          // throw new InternalError("XXX"); // FIXME should not be reached due to above inversion ?
        } else {
          // Calculate the scale for q1 and q2, according to the angle and
          // it's sine value
          scale0 = FloatUtil.sin((1f - changeAmnt) * halfTheta) / sinHalfTheta;
          scale1 = FloatUtil.sin(changeAmnt * halfTheta) / sinHalfTheta;
        }
      }

      x = a.x * scale0 + bx * scale1;
      y = a.y * scale0 + by * scale1;
      z = a.z * scale0 + bz * scale1;
      w = a.w * scale0 + bw * scale1;
    }
    // System.err.println("Slerp.X: Result "+this);
    return this;
  }