Beispiel #1
0
  /**
   * <code>rotateUpTo</code> is a utility function that alters the local rotation to point the Y
   * axis in the direction given by newUp.
   *
   * @param newUp the up vector to use - assumed to be a unit vector.
   */
  public void rotateUpTo(Vector3f newUp) {
    TempVars vars = TempVars.get();

    Vector3f compVecA = vars.vect1;
    Quaternion q = vars.quat1;

    // First figure out the current up vector.
    Vector3f upY = compVecA.set(Vector3f.UNIT_Y);
    Quaternion rot = localTransform.getRotation();
    rot.multLocal(upY);

    // get angle between vectors
    float angle = upY.angleBetween(newUp);

    // figure out rotation axis by taking cross product
    Vector3f rotAxis = upY.crossLocal(newUp).normalizeLocal();

    // Build a rotation quat and apply current local rotation.
    q.fromAngleNormalAxis(angle, rotAxis);
    q.mult(rot, rot);

    vars.release();

    setTransformRefresh();
  }
Beispiel #2
0
  public float getHeadingAtWP(int index) {
    float heading = 0;
    Waypoint nextWayPoint = getNextWayPoint(index);

    // if next way point available, compute heading towards it
    if (nextWayPoint != null) {
      // compute driving direction by looking at next way point from current position
      Vector3f targetPosition = nextWayPoint.getPosition().clone();
      targetPosition.setY(0);

      Vector3f currentPosition = waypointList.get(index).getPosition().clone();
      currentPosition.setY(0);

      Vector3f drivingDirection = targetPosition.subtract(currentPosition).normalize();

      // compute heading (orientation) from driving direction vector for
      // angle between driving direction and heading "0"
      float angle0 = drivingDirection.angleBetween(new Vector3f(0, 0, -1));
      // angle between driving direction and heading "90"
      float angle90 = drivingDirection.angleBetween(new Vector3f(1, 0, 0));

      // get all candidates for heading
      // find the value from {heading1,heading2} which matches with one of {heading3,heading4}
      float heading1 = (2.0f * FastMath.PI + angle0) % FastMath.TWO_PI;
      float heading2 = (2.0f * FastMath.PI - angle0) % FastMath.TWO_PI;
      float heading3 = (2.5f * FastMath.PI + angle90) % FastMath.TWO_PI;
      float heading4 = (2.5f * FastMath.PI - angle90) % FastMath.TWO_PI;

      float diff_1_3 = FastMath.abs(heading1 - heading3);
      float diff_1_4 = FastMath.abs(heading1 - heading4);
      float diff_2_3 = FastMath.abs(heading2 - heading3);
      float diff_2_4 = FastMath.abs(heading2 - heading4);

      if ((diff_1_3 < diff_1_4 && diff_1_3 < diff_2_3 && diff_1_3 < diff_2_4)
          || (diff_1_4 < diff_1_3 && diff_1_4 < diff_2_3 && diff_1_4 < diff_2_4)) {
        // if diff_1_3 or diff_1_4 are smallest --> the correct heading is heading1
        heading = heading1;
      } else {
        // if diff_2_3 or diff_2_4 are smallest --> the correct heading is heading2
        heading = heading2;
      }
    }
    return heading;
  }