/**
   * Calculates the min and max boundaries of the structure after it has been transformed into its
   * canonical orientation.
   */
  private void calcBoundaries() {
    minBoundary.x = Double.MAX_VALUE;
    maxBoundary.x = Double.MIN_VALUE;
    minBoundary.y = Double.MAX_VALUE;
    maxBoundary.x = Double.MIN_VALUE;
    minBoundary.z = Double.MAX_VALUE;
    maxBoundary.z = Double.MIN_VALUE;
    xzRadiusMax = Double.MIN_VALUE;

    Point3d probe = new Point3d();

    for (Point3d[] list : subunits.getTraces()) {
      for (Point3d p : list) {
        probe.set(p);
        transformationMatrix.transform(probe);

        minBoundary.x = Math.min(minBoundary.x, probe.x);
        maxBoundary.x = Math.max(maxBoundary.x, probe.x);
        minBoundary.y = Math.min(minBoundary.y, probe.y);
        maxBoundary.y = Math.max(maxBoundary.y, probe.y);
        minBoundary.z = Math.min(minBoundary.z, probe.z);
        maxBoundary.z = Math.max(maxBoundary.z, probe.z);
        xzRadiusMax = Math.max(xzRadiusMax, Math.sqrt(probe.x * probe.x + probe.z * probe.z));
      }
    }
    //		System.out.println("MinBoundary: " + minBoundary);
    //		System.out.println("MaxBoundary: " + maxBoundary);
    //		System.out.println("zxRadius: " + xzRadiusMax);
  }
  /**
   * Checks for blocks around the entity.
   *
   * @param origin The original position of the entity
   * @return True if the entity is colliding horizontally
   */
  private boolean horizontalHitTest(Vector3d origin) {
    boolean result = false;
    ArrayList<BlockPosition> blockPositions = gatherAdjacentBlockPositions(origin);

    // Check each block position for collision
    for (int i = 0; i < blockPositions.size(); i++) {
      BlockPosition p = blockPositions.get(i);
      byte blockType = _parent.getWorldProvider().getBlockAtPosition(new Vector3d(p.x, p.y, p.z));
      AABB blockAABB = Block.AABBForBlockAt(p.x, p.y, p.z);

      if (!BlockManager.getInstance().getBlock(blockType).isPenetrable()) {
        if (getAABB().overlaps(blockAABB)) {
          result = true;

          // Calculate the direction from the origin to the current position
          Vector3d direction = new Vector3d(getPosition().x, 0f, getPosition().z);
          direction.x -= origin.x;
          direction.z -= origin.z;

          // Calculate the point of intersection on the block's AABB
          Vector3d blockPoi = blockAABB.closestPointOnAABBToPoint(origin);
          Vector3d entityPoi = generateAABBForPosition(origin).closestPointOnAABBToPoint(blockPoi);

          Vector3d planeNormal =
              blockAABB.normalForPlaneClosestToOrigin(blockPoi, origin, true, false, true);

          // Find a vector parallel to the surface normal
          Vector3d slideVector = new Vector3d(planeNormal.z, 0, -planeNormal.x);
          Vector3d pushBack = new Vector3d();

          pushBack.sub(blockPoi, entityPoi);

          // Calculate the intensity of the diversion alongside the block
          double length = slideVector.dot(direction);

          Vector3d newPosition = new Vector3d();
          newPosition.z = origin.z + pushBack.z * 0.2 + length * slideVector.z;
          newPosition.x = origin.x + pushBack.x * 0.2 + length * slideVector.x;
          newPosition.y = origin.y;

          // Update the position
          getPosition().set(newPosition);
        }
      }
    }

    return result;
  }
Exemple #3
0
 // Right hand rule for camera:
 // index finger: cameraPos to cameraFocus
 // thumb: up
 // middle finger:c cameraPos x up
 private Vector3d getCameraDir() {
   Vector3d camDir = new Vector3d();
   camDir.x = cameraFocus.x - cameraPos.x;
   camDir.y = cameraFocus.y - cameraPos.y;
   camDir.z = cameraFocus.z - cameraPos.z;
   return camDir;
 }
 // initializes alignment axis
 void initAlignmentAxis(float x, float y, float z) {
   this.axis.set(x, y, z);
   double invMag;
   invMag = 1.0 / Math.sqrt(axis.x * axis.x + axis.y * axis.y + axis.z * axis.z);
   nAxis.x = (double) axis.x * invMag;
   nAxis.y = (double) axis.y * invMag;
   nAxis.z = (double) axis.z * invMag;
 }
Exemple #5
0
 void doTranslate(double deltaX, double deltaY) {
   Vector3d v = new Vector3d(deltaX, deltaY, 0d);
   vt.transform(v);
   if (lockZ.isSelected()) {
     v.z = 0d;
   }
   parent.getModel().translateObject(v.x, v.y, v.z);
 }
 public void walkBackwards() {
   if (!_godMode && !_isSwimming) {
     _movementDirection.x -= _activeWalkingSpeed * Math.sin(Math.toRadians(_yaw));
     _movementDirection.z += _activeWalkingSpeed * Math.cos(Math.toRadians(_yaw));
   } else if (!_godMode && _isSwimming) {
     _movementDirection.x -=
         _activeWalkingSpeed * Math.sin(Math.toRadians(_yaw)) * Math.cos(Math.toRadians(_pitch));
     _movementDirection.z +=
         _activeWalkingSpeed * Math.cos(Math.toRadians(_yaw)) * Math.cos(Math.toRadians(_pitch));
     _movementDirection.y += _activeWalkingSpeed * Math.sin(Math.toRadians(_pitch));
   } else {
     _movementDirection.x -=
         _activeWalkingSpeed * Math.sin(Math.toRadians(_yaw)) * Math.cos(Math.toRadians(_pitch));
     _movementDirection.z +=
         _activeWalkingSpeed * Math.cos(Math.toRadians(_yaw)) * Math.cos(Math.toRadians(_pitch));
     _movementDirection.y += _activeWalkingSpeed * Math.sin(Math.toRadians(_pitch));
   }
 }
  void updateCenter(Point3d minCoord, Point3d maxCoord) {

    centerOffset.x = -(maxCoord.x - minCoord.x) / 2.0;
    centerOffset.y = -(maxCoord.y - minCoord.y) / 2.0;
    centerOffset.z = -(maxCoord.z - minCoord.z) / 2.0;

    if (Double.isNaN(centerOffset.x) || Double.isInfinite(centerOffset.x)) {
      centerOffset.x = 0;
    }
    if (Double.isNaN(centerOffset.y) || Double.isInfinite(centerOffset.y)) {
      centerOffset.y = 0;
    }
    if (Double.isNaN(centerOffset.z) || Double.isInfinite(centerOffset.z)) {
      centerOffset.z = 0;
    }
    System.out.println(centerOffset);
    centerXform.setTranslation(centerOffset);
    centerGroup.setTransform(centerXform);
  }
Exemple #8
0
  /**
   * Returns the closest point on the AABB to a given point.
   *
   * @param p The point
   * @return The point on the AABB closest to the given point
   */
  public Vector3d closestPointOnAABBToPoint(Vector3d p) {
    Vector3d r = new Vector3d(p);

    if (p.x < minX()) r.x = minX();
    if (p.x > maxX()) r.x = maxX();
    if (p.y < minY()) r.y = minY();
    if (p.y > maxY()) r.y = maxY();
    if (p.z < minZ()) r.z = minZ();
    if (p.z > maxZ()) r.z = maxZ();

    return r;
  }
  /** Method to place the magnetic field vectors and normals on the disk. */
  public void PlaceBNVectors() {
    // first place the vectors on the top of the cylinder
    Vector3d normalTop = null;
    Vector3d centerTop = new Vector3d(0, 0, 0);
    double compx = Math.cos(angleDisk * Math.PI / 180.);
    double compy = Math.sin(angleDisk * Math.PI / 180.);
    normalTop = new Vector3d(compx, compy, 0.);
    normalTop.scale(heightDisk / 2.);
    centerTop.add(normalTop);
    centerTop.add(posDisk);

    for (int j = 0; j < numRadDisk; j++) {
      double rad = (j + 1) * radiusDisk / (numRadDisk + 1);
      for (int i = 0; i < numAziTopDisk; i++) {
        double aziangle = i * 2. * Math.PI / (numAziTopDisk * 1.);
        Vector3d azipos = new Vector3d(0., Math.cos(aziangle), Math.sin(aziangle));
        Vector3d aziposTrans = new Vector3d(0, 0, 0);
        Vector3d azidirTrans = new Vector3d(1, 0, 0);
        Vector3d azidir = new Vector3d(1., 0., 0.);
        azipos.scale(rad);
        aziposTrans.x = azipos.x * compx - azipos.y * compy;
        aziposTrans.y = azipos.x * compy + azipos.y * compx;
        aziposTrans.z = azipos.z;
        azidirTrans.x = azidir.x * compx - azidir.y * compy;
        azidirTrans.y = azidir.x * compy + azidir.y * compx;
        azidirTrans.z = azidir.z;
        aziposTrans.add(centerTop);
        theFieldDiskTop[i][j].setPosition(aziposTrans);
        theFieldDiskTop[i][j].setScale(arrowScale);
        theNormalDiskTop[i][j].setPosition(aziposTrans);
        theNormalDiskTop[i][j].setValue(azidirTrans);
        theNormalDiskTop[i][j].setDrawn(true);
        theNormalDiskTop[i][j].setScale(arrowScale);
        // here we make the field vector tip be at the location of the arrow if the arrow points
        // inward at the local normal

        if (theEngine != null) theEngine.requestSpatial();
      }
    }
  }
  private boolean projectToPlane(Vector3d projVec, Vector3d planeVec) {
    double dis = planeVec.dot(projVec);
    projVec.x = projVec.x - planeVec.x * dis;
    projVec.y = projVec.y - planeVec.y * dis;
    projVec.z = projVec.z - planeVec.z * dis;

    double length = projVec.length();
    if (length < EPSILON) { // projVec is parallel to planeVec
      return false;
    }
    projVec.scale(1 / length);
    return true;
  }
  private void labelAxes()
        // Place numbers along the X- and Z-axes at the integer positions
      {
    Vector3d pt = new Vector3d();
    for (int i = -FLOOR_LEN / 2; i <= FLOOR_LEN / 2; i++) {
      pt.x = i;
      floorBG.addChild(makeText(pt, "" + i)); // along x-axis
    }

    pt.x = 0;
    for (int i = -FLOOR_LEN / 2; i <= FLOOR_LEN / 2; i++) {
      pt.z = i;
      floorBG.addChild(makeText(pt, "" + i)); // along z-axis
    }
  } // end of labelAxes()
Exemple #12
0
  private MyPickResult getMouseIntersection(MouseEvent e, boolean withSnap) {

    MyPickResult myResult = new MyPickResult();

    Point3d closestObjectPos = null;
    Point3d closestMarkPos = null;
    Node closestObject = null;
    Node closestMark = null;

    // 0: closest object; 1: closest mark
    pickCanvas.setShapeLocation(e);
    PickResult[] allResults = null;
    try {
      allResults = pickCanvas.pickAllSorted();
    } catch (AssertionError ex) {
    }
    if (allResults != null) {
      for (PickResult result : allResults) {
        Shape3D s = (Shape3D) result.getNode(PickResult.SHAPE3D);
        if (s != null) { // only pick a Shape3D object
          if (closestObjectPos != null && closestMarkPos != null) {
            break;
          }
          try {
            PickIntersection intersection = result.getClosestIntersection(cameraPos);
            if (!(s.getParent().getParent().getParent() instanceof Mark)
                && !((s.getParent().getParent().getParent() instanceof Marker))) {
              // markdot/markline/label -> TG -> BG -> Mark/Marker
              try {
                closestObjectPos = intersection.getPointCoordinatesVW();
                closestObject = s;
              } catch (NullPointerException ex) {
              }
            } else if ((s.getParent().getParent().getParent() instanceof Mark)) {
              try {
                closestMarkPos = intersection.getPointCoordinatesVW();
                closestMark = s;
              } catch (NullPointerException ex) {
              }
            }
          } catch (NullPointerException ex) {
          }
        }
      }

      Point3d resultPos = null;
      Node resultNode = null;

      if (closestMark != null) {
        resultPos = closestMarkPos; // priority of result -> mark
        resultNode = closestMark;
      } else if (closestObject != null) {
        resultPos = closestObjectPos;
        resultNode = closestObject;
      }

      if (withSnap) { // snap resultPos to correct position

        // start trying to if cursor are inside object bounding sphere
        if ((closestObjectPos != null && bs.intersect(closestObjectPos))
            || closestMarkPos != null && bs.intersect(closestMarkPos)) {

          Point3d closestObjectPos2 = null;
          Point3d closestMarkPos2 = null;
          Node closestObject2 = null;
          Node closestMark2 = null;

          // initiate snap bounds if hasn't been initiated before
          // "snappable" targets to be considered are the ones inside threshold
          if (cursorBound == null) {
            cursorBound = new BoundingSphere(resultPos, CURSOR_THRESHOLD);
          }
          if (thresholdBound == null) {
            thresholdBound = new BoundingSphere(resultPos, SNAP_THRESHOLD);
          }

          // update the bounds
          // only if the cursor is no longer inside current cursor bounds
          if (!cursorBound.intersect(resultPos)) {
            cursorBound.setCenter(resultPos);
            thresholdBound.setCenter(resultPos);
          }

          // snap to nearest point in the list of snap targets
          double nearestDistance = FARTHEST;
          double distance;
          Point3d nearestTarget = null;

          // snap to vertex. all vertexes listed in snapList
          if (snapList.size() > 0) {
            for (Point3d snapTarget : snapList) {
              if (thresholdBound.intersect(snapTarget)) {
                distance = snapTarget.distance(resultPos);
                if (distance < nearestDistance) {
                  nearestDistance = distance;
                  nearestTarget = snapTarget;
                }

                if (nearestTarget != null) {
                  resultPos.set(nearestTarget); // snap!

                  // pick the snapped node
                  Vector3d camToSnapped = new Vector3d();
                  camToSnapped.x = resultPos.x - cameraPos.x;
                  camToSnapped.y = resultPos.y - cameraPos.y;
                  camToSnapped.z = resultPos.z - cameraPos.z;
                  pickCanvas.setShapeRay(cameraPos, camToSnapped);

                  PickResult[] allResultsSnapped = null;
                  try {
                    allResultsSnapped = pickCanvas.pickAllSorted();
                  } catch (AssertionError ex) {
                  }
                  if (allResultsSnapped != null) {
                    for (PickResult result : allResultsSnapped) {
                      Shape3D s = (Shape3D) result.getNode(PickResult.SHAPE3D);
                      if (s != null) { // only pick a Shape3D object
                        if (closestObjectPos2 != null && closestMarkPos2 != null) {
                          break;
                        }
                        try {
                          PickIntersection intersection = result.getClosestIntersection(cameraPos);
                          if (!(s.getParent().getParent().getParent() instanceof Mark)
                              && !((s.getParent().getParent().getParent() instanceof Marker))) {
                            // markdot/markline/label -> TG -> BG -> Mark/Marker
                            try {
                              closestObjectPos2 = intersection.getPointCoordinatesVW();
                              closestObject2 = s;
                            } catch (NullPointerException ex) {
                            }
                          } else if ((s.getParent().getParent().getParent() instanceof Mark)) {
                            try {
                              closestMarkPos2 = intersection.getPointCoordinatesVW();
                              closestMark2 = s;
                            } catch (NullPointerException ex) {
                            }
                          }
                        } catch (NullPointerException ex) {
                        }
                      }
                    }

                    if (closestMark2 != null) {
                      resultPos = closestMarkPos2; // priority of result -> mark
                      resultNode = closestMark2;
                    } else if (closestObject2 != null) {
                      resultPos = closestObjectPos2;
                      resultNode = closestObject2;
                    }
                  }
                }
              }
            }
          }
        }
      }

      myResult.setPoint(resultPos);
      myResult.setNode(resultNode);
    }
    return myResult;
  }
  // called on the parent object
  // Should be synchronized so that the user thread does not modify the
  // OrientedShape3D params while computing the transform
  synchronized void updateOrientedTransform(Canvas3D canvas, int viewIndex) {
    double angle = 0.0;
    double sign;
    boolean status;

    Transform3D orientedxform = getOrientedTransform(viewIndex);
    //  get viewplatforms's location in virutal world
    if (mode == OrientedShape3D.ROTATE_ABOUT_AXIS) { // rotate about axis
      canvas.getCenterEyeInImagePlate(viewPosition);
      canvas.getImagePlateToVworld(xform); // xform is imagePlateToLocal
      xform.transform(viewPosition);

      // get billboard's transform
      xform.set(getCurrentLocalToVworld());
      xform.invert(); // xform is now vWorldToLocal

      // transform the eye position into the billboard's coordinate system
      xform.transform(viewPosition);

      // eyeVec is a vector from the local origin to the eye pt in local
      eyeVec.set(viewPosition);
      eyeVec.normalize();

      // project the eye into the rotation plane
      status = projectToPlane(eyeVec, nAxis);

      if (status) {
        // project the z axis into the rotation plane
        zAxis.x = 0.0;
        zAxis.y = 0.0;
        zAxis.z = 1.0;
        status = projectToPlane(zAxis, nAxis);
      }
      if (status) {

        // compute the sign of the angle by checking if the cross product
        // of the two vectors is in the same direction as the normal axis
        vector.cross(eyeVec, zAxis);
        if (vector.dot(nAxis) > 0.0) {
          sign = 1.0;
        } else {
          sign = -1.0;
        }

        // compute the angle between the projected eye vector and the
        // projected z

        double dot = eyeVec.dot(zAxis);
        if (dot > 1.0f) {
          dot = 1.0f;
        } else if (dot < -1.0f) {
          dot = -1.0f;
        }

        angle = sign * Math.acos(dot);

        // use -angle because xform is to *undo* rotation by angle
        aa.x = nAxis.x;
        aa.y = nAxis.y;
        aa.z = nAxis.z;
        aa.angle = -angle;
        orientedxform.set(aa);
      } else {
        orientedxform.setIdentity();
      }

    } else if (mode == OrientedShape3D.ROTATE_ABOUT_POINT) { // rotate about point
      // Need to rotate Z axis to point to eye, and Y axis to be
      // parallel to view platform Y axis, rotating around rotation pt

      // get the eye point
      canvas.getCenterEyeInImagePlate(viewPosition);

      // derive the yUp point
      yUpPoint.set(viewPosition);
      yUpPoint.y += 0.01; // one cm in Physical space

      // transform the points to the Billboard's space
      canvas.getImagePlateToVworld(xform); // xform is ImagePlateToVworld
      xform.transform(viewPosition);
      xform.transform(yUpPoint);

      // get billboard's transform
      xform.set(getCurrentLocalToVworld());
      xform.invert(); // xform is vWorldToLocal

      // transfom points to local coord sys
      xform.transform(viewPosition);
      xform.transform(yUpPoint);

      // Make a vector from viewPostion to 0,0,0 in the BB coord sys
      eyeVec.set(viewPosition);
      eyeVec.normalize();

      // create a yUp vector
      yUp.set(yUpPoint);
      yUp.sub(viewPosition);
      yUp.normalize();

      // find the plane to rotate z
      zAxis.x = 0.0;
      zAxis.y = 0.0;
      zAxis.z = 1.0;

      // rotation axis is cross product of eyeVec and zAxis
      vector.cross(eyeVec, zAxis); // vector is cross product

      // if cross product is non-zero, vector is rotation axis and
      // rotation angle is acos(eyeVec.dot(zAxis)));
      double length = vector.length();
      if (length > 0.0001) {
        double dot = eyeVec.dot(zAxis);
        if (dot > 1.0f) {
          dot = 1.0f;
        } else if (dot < -1.0f) {
          dot = -1.0f;
        }
        angle = Math.acos(dot);
        aa.x = vector.x;
        aa.y = vector.y;
        aa.z = vector.z;
        aa.angle = -angle;
        zRotate.set(aa);
      } else {
        // no rotation needed, set to identity (scale = 1.0)
        zRotate.set(1.0);
      }

      // Transform the yAxis by zRotate
      yAxis.x = 0.0;
      yAxis.y = 1.0;
      yAxis.z = 0.0;
      zRotate.transform(yAxis);

      // project the yAxis onto the plane perp to the eyeVec
      status = projectToPlane(yAxis, eyeVec);

      if (status) {
        // project the yUp onto the plane perp to the eyeVec
        status = projectToPlane(yUp, eyeVec);
      }

      if (status) {
        // rotation angle is acos(yUp.dot(yAxis));
        double dot = yUp.dot(yAxis);

        // Fix numerical error, otherwise acos return NULL
        if (dot > 1.0f) {
          dot = 1.0f;
        } else if (dot < -1.0f) {
          dot = -1.0f;
        }

        angle = Math.acos(dot);

        // check the sign by looking a the cross product vs the eyeVec
        vector.cross(yUp, yAxis); // vector is cross product
        if (eyeVec.dot(vector) < 0) {
          angle *= -1;
        }
        aa.x = eyeVec.x;
        aa.y = eyeVec.y;
        aa.z = eyeVec.z;
        aa.angle = -angle;
        xform.set(aa); // xform is now yRotate

        // rotate around the rotation point
        vector.x = rotationPoint.x;
        vector.y = rotationPoint.y;
        vector.z = rotationPoint.z; // vector to translate to RP
        orientedxform.set(vector); // translate to RP
        orientedxform.mul(xform); // yRotate
        orientedxform.mul(zRotate); // zRotate
        vector.scale(-1.0); // vector to translate back
        xform.set(vector); // xform to translate back
        orientedxform.mul(xform); // translate back
      } else {
        orientedxform.setIdentity();
      }
    }
    // Scale invariant computation
    if (constantScale) {
      // Back Xform a unit vector to local world coords
      canvas.getInverseVworldProjection(left_xform, right_xform);

      // the two endpts of the vector have to be transformed
      // individually because the Xform is not affine
      im_vec[0].set(0.0, 0.0, 0.0, 1.0);
      im_vec[1].set(1.0, 0.0, 0.0, 1.0);
      left_xform.transform(im_vec[0]);
      left_xform.transform(im_vec[1]);

      left_xform.set(getCurrentLocalToVworld());
      left_xform.invert();
      left_xform.transform(im_vec[0]);
      left_xform.transform(im_vec[1]);
      lvec.set(im_vec[1]);
      lvec.sub(im_vec[0]);

      // We simply need the direction of this vector
      lvec.normalize();
      im_vec[0].set(0.0, 0.0, 0.0, 1.0);
      im_vec[1].set(lvec);
      im_vec[1].w = 1.0;

      // Forward Xfrom to clip coords
      left_xform.set(getCurrentLocalToVworld());
      left_xform.transform(im_vec[0]);
      left_xform.transform(im_vec[1]);

      canvas.getVworldProjection(left_xform, right_xform);
      left_xform.transform(im_vec[0]);
      left_xform.transform(im_vec[1]);

      // Perspective division
      im_vec[0].x /= im_vec[0].w;
      im_vec[0].y /= im_vec[0].w;
      im_vec[0].z /= im_vec[0].w;

      im_vec[1].x /= im_vec[1].w;
      im_vec[1].y /= im_vec[1].w;
      im_vec[1].z /= im_vec[1].w;

      lvec.set(im_vec[1]);
      lvec.sub(im_vec[0]);

      // Use the length of this vector to determine the scaling
      // factor
      double scale = 1 / lvec.length();

      // Convert to meters
      scale *= scaleFactor * canvas.getPhysicalWidth() / 2;

      // Scale object so that it appears the same size
      scaleXform.setScale(scale);
      orientedxform.mul(scaleXform);
    }
  }
Exemple #14
0
 @Override
 @SuppressWarnings("rawtypes")
 public void processStimulus(Enumeration criteria) { // examiner criteria �
   while (criteria.hasMoreElements()) {
     WakeupOnAWTEvent w = (WakeupOnAWTEvent) criteria.nextElement();
     AWTEvent events[] = w.getAWTEvent();
     synchronized (deltaT) {
       synchronized (deltaR) {
         for (int i = 0; i < events.length; i++) {
           if (events[i].getID() == KeyEvent.KEY_PRESSED) {
             int k = ((KeyEvent) events[i]).getKeyCode();
             if (k == KeyEvent.VK_UP) {
               deltaT.z = -0.1;
             } else if (k == KeyEvent.VK_DOWN) {
               deltaT.z = 0.1;
             } else if (k == KeyEvent.VK_PAGE_UP) {
               deltaT.y = 0.1;
             } else if (k == KeyEvent.VK_PAGE_DOWN) {
               deltaT.y = -0.1;
             } else if (k == KeyEvent.VK_LEFT) {
               deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), 0.02));
             } else if (k == KeyEvent.VK_RIGHT) {
               deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), -0.02));
             }
           } else if (events[i].getID() == KeyEvent.KEY_RELEASED) {
             int k = ((KeyEvent) events[i]).getKeyCode();
             if (k == KeyEvent.VK_UP) {
               deltaT.z = 0;
             } else if (k == KeyEvent.VK_DOWN) {
               deltaT.z = 0;
             } else if (k == KeyEvent.VK_PAGE_UP) {
               deltaT.y = 0;
             } else if (k == KeyEvent.VK_PAGE_DOWN) {
               deltaT.y = 0;
             } else if (k == KeyEvent.VK_LEFT) {
               deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), 0.0));
             } else if (k == KeyEvent.VK_RIGHT) {
               deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), 0.0));
             }
           } else if (events[i].getID() == MouseEvent.MOUSE_WHEEL) {
             MouseWheelEvent mwe = ((MouseWheelEvent) events[i]);
             // deltaT.y = deltaT.y + mwe.getPreciseWheelRotation () / 10 ;
             //                  } else if ((events [i].getID () == MouseEvent.MOUSE_DRAGGED) ||
             //                             (events [i].getID () == MouseEvent.MOUSE_PRESSED)) {
             //                     MouseEvent me  =  ((MouseEvent)events [i]) ;
             //                     int width = me.getComponent ().getWidth () ;
             //                     int height = me.getComponent ().getHeight () ;
             //                     int x = me.getX () ;
             //                     int y = me.getY () ;
             //                     double azimuth = Math.atan2 (width / 2.0 - x, 500.0) ;
             //                     double elevation = Math.atan2 (height / 2.0 - y, 500.0) ;
             //                     Quat4d azimuthR = new Quat4d () ;
             //                     azimuthR.set (new AxisAngle4d (0, 1, 0, azimuth)) ;
             //                     Quat4d elevationR = new Quat4d () ;
             //                     elevationR.set (new AxisAngle4d (1, 0, 0, elevation)) ;
             //                     absoluteR.mul (azimuthR, elevationR) ;
             //                     navigator.setHeadOrientationInSupportFrame (absoluteR.x,
             // absoluteR.y, absoluteR.z, absoluteR.w);
           } else if (events[i].getID() == MouseEvent.MOUSE_PRESSED) {
             initHeadR = navigator.getHeadRotationInSupportFrame();
             MouseEvent me = ((MouseEvent) events[i]);
             xInit = me.getX();
             yInit = me.getY();
           } else if (events[i].getID() == MouseEvent.MOUSE_DRAGGED) {
             MouseEvent me = ((MouseEvent) events[i]);
             int dx = me.getX() - xInit;
             int dy = me.getY() - yInit;
             double azimuth = Math.atan2(-dx, 500.0);
             double elevation = Math.atan2(-dy, 500.0);
             Quat4d azimuthR = new Quat4d();
             azimuthR.set(new AxisAngle4d(0, 1, 0, azimuth));
             Quat4d elevationR = new Quat4d();
             elevationR.set(new AxisAngle4d(1, 0, 0, elevation));
             Quat4d relativeR = new Quat4d();
             relativeR.mul(azimuthR, elevationR);
             absoluteR.mul(initHeadR, relativeR);
             navigator.setHeadOrientationInSupportFrame(
                 absoluteR.x, absoluteR.y, absoluteR.z, absoluteR.w);
           }
         }
       }
     }
     wakeupOn(wEvents);
   }
 }
  public void dragged(int dx, int dy, boolean isControlDown, boolean isShiftDown) {
    if (pickedTransformable != null) {
      boolean controlDown = isControlDown;
      boolean shiftDown = isShiftDown;

      double deltaFactor;
      if (camera instanceof edu.cmu.cs.stage3.alice.core.camera.OrthographicCamera) {
        edu.cmu.cs.stage3.alice.core.camera.OrthographicCamera orthoCamera =
            (edu.cmu.cs.stage3.alice.core.camera.OrthographicCamera) camera;
        double nearClipHeightInScreen =
            renderTarget
                .getAWTComponent()
                .getHeight(); // TODO: should be viewport, but not working right now
        double nearClipHeightInWorld =
            orthoCamera.getSceneGraphOrthographicCamera().getPlane()[3]
                - orthoCamera.getSceneGraphOrthographicCamera().getPlane()[1];
        deltaFactor = nearClipHeightInWorld / nearClipHeightInScreen;

        if (controlDown) {
          if (shiftDown) {
            helper.setTransformationRightNow(
                edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), camera);
            helper.setPositionRightNow(zeroVec, pickedTransformable);
            pickedTransformable.rotateRightNow(
                edu.cmu.cs.stage3.math.MathUtilities.getXAxis(), -dy * .01, helper);
            pickedTransformable.rotateRightNow(
                edu.cmu.cs.stage3.math.MathUtilities.getYAxis(), -dx * .01, pickedTransformable);
          } else {
            helper.setTransformationRightNow(
                edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), camera);
            helper.setPositionRightNow(zeroVec, pickedTransformable);
            pickedTransformable.rotateRightNow(
                edu.cmu.cs.stage3.math.MathUtilities.getZAxis(), -dx * .01, helper);
          }
        } else if (shiftDown) {
          helper.setTransformationRightNow(
              edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), camera);
          helper.setPositionRightNow(zeroVec, pickedTransformable);
          tempVec.x = 0.0;
          tempVec.y = -dy * deltaFactor;
          tempVec.z = 0.0;
          pickedTransformable.moveRightNow(tempVec, helper);
        } else {
          helper.setTransformationRightNow(
              edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), camera);
          helper.setPositionRightNow(zeroVec, pickedTransformable);
          tempVec.x = dx * deltaFactor;
          tempVec.y = -dy * deltaFactor;
          tempVec.z = 0.0;
          pickedTransformable.moveRightNow(tempVec, helper);
        }
      } else {
        double projectionMatrix11 =
            renderTarget.getProjectionMatrix(camera.getSceneGraphCamera()).getElement(1, 1);
        double nearClipDist = camera.nearClippingPlaneDistance.doubleValue();
        double nearClipHeightInWorld = 2 * (nearClipDist / projectionMatrix11);
        double nearClipHeightInScreen =
            renderTarget
                .getAWTComponent()
                .getHeight(); // TODO: should be viewport, but not working right now
        double pixelHeight = nearClipHeightInWorld / nearClipHeightInScreen;
        //				double pixelHeight = nearClipHeightInWorld/300;
        double objectDist = pickedTransformable.getPosition(camera).getLength();
        deltaFactor = (objectDist * pixelHeight) / nearClipDist;

        if (controlDown) {
          if (shiftDown) {
            helper.setTransformationRightNow(
                edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), camera);
            helper.setPositionRightNow(zeroVec, pickedTransformable);
            pickedTransformable.rotateRightNow(
                edu.cmu.cs.stage3.math.MathUtilities.getXAxis(), -dy * .01, helper);
            pickedTransformable.rotateRightNow(
                edu.cmu.cs.stage3.math.MathUtilities.getYAxis(), -dx * .01, pickedTransformable);
          } else {
            helper.setTransformationRightNow(
                edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), world);
            helper.setPositionRightNow(zeroVec, pickedTransformable);
            pickedTransformable.rotateRightNow(
                edu.cmu.cs.stage3.math.MathUtilities.getYAxis(), -dx * .01, helper);
          }
        } else if (shiftDown) {
          helper.setTransformationRightNow(
              edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), world);
          helper.setPositionRightNow(zeroVec, pickedTransformable);
          tempVec.x = 0.0;
          tempVec.y = -dy * deltaFactor;
          tempVec.z = 0.0;
          pickedTransformable.moveRightNow(tempVec, helper);
        } else {
          javax.vecmath.Matrix4d cameraTransformation =
              camera.getSceneGraphTransformable().getAbsoluteTransformation();
          cameraUp.x = cameraTransformation.m10;
          cameraUp.y = cameraTransformation.m11;
          cameraUp.z = cameraTransformation.m12;
          cameraForward.x = cameraTransformation.m20;
          cameraForward.y = cameraTransformation.m21;
          cameraForward.z = cameraTransformation.m22;

          helper.setPositionRightNow(zeroVec, pickedTransformable);
          if (Math.abs(cameraForward.y) < Math.abs(cameraUp.y)) { // if we're looking mostly level
            cameraForward.y = 0.0;
            helper.setOrientationRightNow(cameraForward, cameraUp, world);
          } else { // if we're looking mostly up or down
            cameraUp.y = 0.0;
            cameraForward.negate();
            helper.setOrientationRightNow(cameraUp, cameraForward, world);
          }

          tempVec.x = dx * deltaFactor;
          tempVec.y = 0.0;
          tempVec.z = -dy * deltaFactor;
          pickedTransformable.moveRightNow(tempVec, helper);
        }
      }
    }
  }
 public void strafeRight() {
   _movementDirection.x += _activeWalkingSpeed * Math.sin(Math.toRadians(_yaw + 90));
   _movementDirection.z -= _activeWalkingSpeed * Math.cos(Math.toRadians(_yaw + 90));
 }
  /**
   * Sets the skipNode for each frame. The skipNode should be the smallest node in which the
   * camera's rays start from (its position)
   *
   * @param cameraPos The position of the camera in space.
   */
  private void setSkipNode(Vector3d cameraPos) {

    // if the skipNode has not been set yet, check if the camera is inside the bounding box
    if (skipNode == null) {
      if (cameraPos.x < 1
          && cameraPos.x > -1
          && cameraPos.y < 1
          && cameraPos.y > -1
          && cameraPos.z < 1
          && cameraPos.z > -1) {

        // if it is, descend hierarchy from root node until leaf reached
        OctreeNode node = rootNode;
        double boxDim = 2d;
        Vector3d boxMin = new Vector3d(-1, -1, -1);

        while (!node.isLeaf()) {
          boxDim /= 2d;
          boolean[] s = {
            (cameraPos.x >= (boxMin.x + boxDim)) ? true : false,
            (cameraPos.y >= (boxMin.y + boxDim)) ? true : false,
            (cameraPos.z >= (boxMin.z + boxDim)) ? true : false
          };
          boxMin.x += s[0] ? boxDim : 0;
          boxMin.y += s[1] ? boxDim : 0;
          boxMin.z += s[2] ? boxDim : 0;
          node = node.getChild(s[0] ? 1 : 0, s[1] ? 1 : 0, s[2] ? 1 : 0);
        }

        // once skipNode has been found for next frame, set it and record it's position and
        // dimensions
        skipNode = node;
        skipNodeBoxMin = boxMin;
        skipNodeBoxDim = boxDim;
      }
    } else {

      // otherwise check the camera's position is still within the skipNode, if it is, possibly
      // descend further down the hierarchy
      if (cameraPos.x > skipNodeBoxMin.x
          && cameraPos.x < (skipNodeBoxMin.x + skipNodeBoxDim)
          && cameraPos.y > skipNodeBoxMin.y
          && cameraPos.y < (skipNodeBoxMin.y + skipNodeBoxDim)
          && cameraPos.z > skipNodeBoxMin.z
          && cameraPos.z < (skipNodeBoxMin.z + skipNodeBoxDim)) {

        if (!skipNode.isLeaf()) {
          // take current skipNode values and descend hierarchy
          OctreeNode node = skipNode;
          double boxDim = skipNodeBoxDim;
          Vector3d boxMin = new Vector3d(skipNodeBoxMin);

          while (!node.isLeaf()) {
            boxDim /= 2d;
            boolean[] s = {
              (cameraPos.x >= (boxMin.x + boxDim)) ? true : false,
              (cameraPos.y >= (boxMin.y + boxDim)) ? true : false,
              (cameraPos.z >= (boxMin.z + boxDim)) ? true : false
            };
            boxMin.x += s[0] ? boxDim : 0;
            boxMin.y += s[1] ? boxDim : 0;
            boxMin.z += s[2] ? boxDim : 0;
            node = node.getChild(s[0] ? 1 : 0, s[1] ? 1 : 0, s[2] ? 1 : 0);
          }

          // once new skipNode has been found inside old one, set it and record it's position and
          // dimensions
          skipNode = node;
          skipNodeBoxMin = boxMin;
          skipNodeBoxDim = boxDim;
        }

      } else {

        // otherwise the camera has left the current skipNode and a new one must be found
        // first find the possibly correct neighbor
        OctreeNode oldSkipNode = skipNode;
        if (cameraPos.x < skipNodeBoxMin.x) {
          skipNode = skipNode.getNeighbor(0);
          skipNodeBoxMin.x -= skipNodeBoxDim;
        } else if (cameraPos.x > (skipNodeBoxMin.x + skipNodeBoxDim)) {
          skipNode = skipNode.getNeighbor(1);
          skipNodeBoxMin.x += skipNodeBoxDim;
        } else if (cameraPos.y < skipNodeBoxMin.y) {
          skipNode = skipNode.getNeighbor(2);
          skipNodeBoxMin.y -= skipNodeBoxDim;
        } else if (cameraPos.y > (skipNodeBoxMin.y + skipNodeBoxDim)) {
          skipNode = skipNode.getNeighbor(3);
          skipNodeBoxMin.y += skipNodeBoxDim;
        } else if (cameraPos.z < skipNodeBoxMin.z) {
          skipNode = skipNode.getNeighbor(4);
          skipNodeBoxMin.z -= skipNodeBoxDim;
        } else if (cameraPos.z > (skipNodeBoxMin.z + skipNodeBoxDim)) {
          skipNode = skipNode.getNeighbor(5);
          skipNodeBoxMin.z += skipNodeBoxDim;
        }

        // if no neighbor node found, just retun null skipNode for worst case scenario
        if (skipNode != null) {

          // if coarser neighbor node found, snap skipNodeBoxMin onto coarser grid
          if (skipNode.getDepth() != oldSkipNode.getDepth()) {
            skipNodeBoxDim = Math.pow(2d, -skipNode.getDepth() + 1d);
            skipNodeBoxMin.x = Math.floor(skipNodeBoxMin.x / skipNodeBoxDim) * skipNodeBoxDim;
            skipNodeBoxMin.y = Math.floor(skipNodeBoxMin.y / skipNodeBoxDim) * skipNodeBoxDim;
            skipNodeBoxMin.z = Math.floor(skipNodeBoxMin.z / skipNodeBoxDim) * skipNodeBoxDim;
          }

          // now do one last check that the camera is inside the new skipNode. If not, set it to
          // null
          if (!(cameraPos.x > skipNodeBoxMin.x
              && cameraPos.x < (skipNodeBoxMin.x + skipNodeBoxDim)
              && cameraPos.y > skipNodeBoxMin.y
              && cameraPos.y < (skipNodeBoxMin.y + skipNodeBoxDim)
              && cameraPos.z > skipNodeBoxMin.z
              && cameraPos.z < (skipNodeBoxMin.z + skipNodeBoxDim))) skipNode = null;
        }
      }
    }
  }
  /** Updates the position of the entity. */
  protected void updatePosition() {
    // Save the previous position before changing any of the values
    Vector3d oldPosition = new Vector3d(getPosition());

    double friction =
        (Double) ConfigurationManager.getInstance().getConfig().get("Player.friction");

    /*
     * Slowdown the speed of the entity each time this method is called.
     */
    if (MathHelper.fastAbs(_velocity.y) > 0f) {
      _velocity.y += -1f * _velocity.y * friction;
    }

    if (MathHelper.fastAbs(_velocity.x) > 0f) {
      _velocity.x += -1f * _velocity.x * friction;
    }

    if (MathHelper.fastAbs(_velocity.z) > 0f) {
      _velocity.z += -1f * _velocity.z * friction;
    }

    /*
     * Apply friction.
     */
    if (MathHelper.fastAbs(_velocity.x) > _activeWalkingSpeed
        || MathHelper.fastAbs(_velocity.z) > _activeWalkingSpeed
        || MathHelper.fastAbs(_velocity.y) > _activeWalkingSpeed) {
      double max =
          Math.max(
              Math.max(MathHelper.fastAbs(_velocity.x), MathHelper.fastAbs(_velocity.z)),
              MathHelper.fastAbs(_velocity.y));
      double div = max / _activeWalkingSpeed;

      _velocity.x /= div;
      _velocity.z /= div;
      _velocity.y /= div;
    }

    /*
     * Increase the speed of the entity by adding the movement
     * vector to the acceleration vector.
     */
    _velocity.x += _movementDirection.x;
    _velocity.y += _movementDirection.y;
    _velocity.z += _movementDirection.z;

    double maxGravity =
        (Double) ConfigurationManager.getInstance().getConfig().get("Player.maxGravity");
    double maxGravitySwimming =
        (Double) ConfigurationManager.getInstance().getConfig().get("Player.maxGravitySwimming");
    double gravitySwimming =
        (Double) ConfigurationManager.getInstance().getConfig().get("Player.gravitySwimming");
    double gravity = (Double) ConfigurationManager.getInstance().getConfig().get("Player.gravity");

    // Normal gravity
    if (_gravity > -maxGravity && !_godMode && !_isSwimming) {
      _gravity -= gravity;
    }

    if (_gravity < -maxGravity && !_godMode && !_isSwimming) {
      _gravity = -maxGravity;
    }

    // Gravity under water
    if (_gravity > -maxGravitySwimming && !_godMode && _isSwimming) {
      _gravity -= gravitySwimming;
    }

    if (_gravity < -maxGravitySwimming && !_godMode && _isSwimming) {
      _gravity = -maxGravitySwimming;
    }

    getPosition().y += _velocity.y;
    getPosition().y += _gravity;

    if (!_godMode) {
      if (verticalHitTest(oldPosition)) {
        handleVerticalCollision();

        double oldGravity = _gravity;
        _gravity = 0;

        if (oldGravity <= 0) {
          // Jumping is only possible, if the entity is standing on ground
          if (_jump) {
            _jump = false;
            _gravity = _jumpIntensity;
          }

          // Entity reaches the ground
          if (!_touchingGround) {
            Vector3d playerDirection = directionOfReferencePoint();
            _footstepSounds[
                MathHelper.fastAbs(_parent.getWorldProvider().getRandom().randomInt()) % 5]
                .playAsSoundEffect(
                    0.7f
                        + (float)
                                MathHelper.fastAbs(
                                    _parent.getWorldProvider().getRandom().randomDouble())
                            * 0.3f,
                    0.2f
                        + (float)
                                MathHelper.fastAbs(
                                    _parent.getWorldProvider().getRandom().randomDouble())
                            * 0.3f,
                    false,
                    (float) playerDirection.x,
                    (float) playerDirection.y,
                    (float) playerDirection.z);
            _touchingGround = true;
          }
        } else {
          _touchingGround = false;
        }
      } else {
        _touchingGround = false;
      }
    } else {
      _gravity = 0f;
    }

    oldPosition.set(getPosition());

    /*
     * Update the position of the entity
     * according to the acceleration vector.
     */
    getPosition().x += _velocity.x;
    getPosition().z += _velocity.z;

    _stepCounter += Math.max(MathHelper.fastAbs(_velocity.x), MathHelper.fastAbs(_velocity.z));

    /*
     * Check for horizontal collisions __after__ checking for vertical
     * collisions.
     */
    if (!_godMode) {
      if (horizontalHitTest(oldPosition)) {
        handleHorizontalCollision();
      }
    }
  }