Example #1
0
  public Quaternion fromRotationMatrix(
      float m00,
      float m01,
      float m02,
      float m10,
      float m11,
      float m12,
      float m20,
      float m21,
      float m22) {
    // Use the Graphics Gems code, from
    // ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z
    // *NOT* the "Matrix and Quaternions FAQ", which has errors!

    // the trace is the sum of the diagonal elements; see
    // http://mathworld.wolfram.com/MatrixTrace.html
    float t = m00 + m11 + m22;

    // we protect the division by s by ensuring that s>=1
    if (t >= 0) { // |w| >= .5
      float s = FastMath.sqrt(t + 1); // |s|>=1 ...
      w = 0.5f * s;
      s = 0.5f / s; // so this division isn't bad
      x = (m21 - m12) * s;
      y = (m02 - m20) * s;
      z = (m10 - m01) * s;
    } else if ((m00 > m11) && (m00 > m22)) {
      float s = FastMath.sqrt(1.0f + m00 - m11 - m22); // |s|>=1
      x = s * 0.5f; // |x| >= .5
      s = 0.5f / s;
      y = (m10 + m01) * s;
      z = (m02 + m20) * s;
      w = (m21 - m12) * s;
    } else if (m11 > m22) {
      float s = FastMath.sqrt(1.0f + m11 - m00 - m22); // |s|>=1
      y = s * 0.5f; // |y| >= .5
      s = 0.5f / s;
      x = (m10 + m01) * s;
      z = (m21 + m12) * s;
      w = (m02 - m20) * s;
    } else {
      float s = FastMath.sqrt(1.0f + m22 - m00 - m11); // |s|>=1
      z = s * 0.5f; // |z| >= .5
      s = 0.5f / s;
      x = (m02 + m20) * s;
      y = (m21 + m12) * s;
      w = (m10 - m01) * s;
    }

    return this;
  }
  /*
   * (non-Javadoc)
   * @see com.jme.bounding.BoundingVolume#intersectsWhere(com.jme.math.Ray)
   */
  public int collideWithRay(Ray ray, CollisionResults results) {
    Vector3f vect1 = Vector3f.newInstance();
    Vector3f diff = vect1.set(ray.getOrigin()).subtractLocal(center);
    float a = diff.dot(diff) - (getRadius() * getRadius());
    float a1, discr, root;
    if (a <= 0.0) {
      // inside sphere
      a1 = ray.direction.dot(diff);
      discr = (a1 * a1) - a;
      root = FastMath.sqrt(discr);

      float distance = root - a1;
      Vector3f point = new Vector3f(ray.direction).multLocal(distance).addLocal(ray.origin);

      CollisionResult result = new CollisionResult(point, distance);
      results.addCollision(result);
      Vector3f.recycle(vect1);
      return 1;
    }

    a1 = ray.direction.dot(diff);
    if (a1 >= 0.0) {
      Vector3f.recycle(vect1);
      return 0;
    }

    discr = a1 * a1 - a;
    if (discr < 0.0) {
      Vector3f.recycle(vect1);
      return 0;
    } else if (discr >= FastMath.ZERO_TOLERANCE) {
      root = FastMath.sqrt(discr);
      float dist = -a1 - root;
      Vector3f point = new Vector3f(ray.direction).multLocal(dist).addLocal(ray.origin);
      results.addCollision(new CollisionResult(point, dist));

      dist = -a1 + root;
      point = new Vector3f(ray.direction).multLocal(dist).addLocal(ray.origin);
      results.addCollision(new CollisionResult(point, dist));
      Vector3f.recycle(vect1);
      return 2;
    } else {
      float dist = -a1;
      Vector3f point = new Vector3f(ray.direction).multLocal(dist).addLocal(ray.origin);
      results.addCollision(new CollisionResult(point, dist));
      Vector3f.recycle(vect1);
      return 1;
    }
  }
 /**
  * Calculates the minimum bounding sphere of 2 points. Used in welzl's algorithm.
  *
  * @param O The 1st point inside the sphere.
  * @param A The 2nd point inside the sphere.
  * @see #calcWelzl(java.nio.FloatBuffer)
  */
 private void setSphere(Vector3f O, Vector3f A) {
   radius =
       FastMath.sqrt(
               ((A.x - O.x) * (A.x - O.x) + (A.y - O.y) * (A.y - O.y) + (A.z - O.z) * (A.z - O.z))
                   / 4f)
           + RADIUS_EPSILON
           - 1f;
   center.interpolate(O, A, .5f);
 }
Example #4
0
  /**
   * <code>toAngleAxis</code> sets a given angle and axis to that represented by the current
   * quaternion. The values are stored as following: The axis is provided as a parameter and built
   * by the method, the angle is returned as a float.
   *
   * @param axisStore the object we'll store the computed axis in.
   * @return the angle of rotation in radians.
   */
  public float toAngleAxis(Vector3f axisStore) {
    float sqrLength = x * x + y * y + z * z;
    float angle;
    if (sqrLength == 0.0f) {
      angle = 0.0f;
      if (axisStore != null) {
        axisStore.x = 1.0f;
        axisStore.y = 0.0f;
        axisStore.z = 0.0f;
      }
    } else {
      angle = (2.0f * FastMath.acos(w));
      if (axisStore != null) {
        float invLength = (1.0f / FastMath.sqrt(sqrLength));
        axisStore.x = x * invLength;
        axisStore.y = y * invLength;
        axisStore.z = z * invLength;
      }
    }

    return angle;
  }
Example #5
0
 public float distance(Vector3f point) {
   return FastMath.sqrt(distanceSquared(point));
 }