@Override
  public void eyePtChanged() {

    Point3d eyePt = getViewPosInLocal(frontRoot);

    // System.out.println("eye pt = " + eyePt)
    if (eyePt != null) {

      for (int i = 0; i < 6; i++) {
        // if (perspectiveAttr.getValue() == true)
        // {
        // eyeVec.sub(eyePt, faceCenter[i]);
        // } else
        {
          eyeVec.sub(eyePt, volCenter);
        }
        frontFaceBits.set(i);
        backFaceBits.clear(i);
        if (eyeVec.dot(faceNormal[i]) < 0) {
          backFaceBits.set(i);
          frontFaceBits.clear(i);
        } else {
          frontFaceBits.set(i);
          backFaceBits.clear(i);
        }
      }
      frontAnnotations.setChildMask(frontFaceBits);
      backAnnotations.setChildMask(backFaceBits);
    }
  }
示例#2
0
  private void yawCamera(double angle) {
    Vector3d backToZero;
    Point3d center;
    if (choosingPivotPoint && pivotPointAdjustor.getPivotPointBG() != null) {
      center = pivotPointAdjustor.getPivotPointPos();
    } else {
      center = getObjCenter();
    }
    backToZero = new Vector3d(center);
    backToZero.negate();
    Transform3D back = new Transform3D();
    back.set(backToZero);

    Transform3D rotate = new Transform3D();
    rotate.set(new AxisAngle4d(up, angle));

    Transform3D toOri = new Transform3D();
    toOri.set(new Vector3d(center));

    // creating composite transform, reversing the order
    Transform3D fullRotate = new Transform3D(toOri);
    fullRotate.mul(rotate);
    fullRotate.mul(back);

    fullRotate.transform(cameraPos);
    fullRotate.transform(cameraFocus);
  }
示例#3
0
  public void lookAt(Vector3d target) {
    Vector3d targetDirection = new Vector3d();
    targetDirection.sub(target, getPosition());
    targetDirection.normalize();

    setPitchYawFromVector(targetDirection);
  }
  protected boolean recoverFromPenetration(CollisionWorld collisionWorld) {
    boolean penetration = false;

    collisionWorld
        .getDispatcher()
        .dispatchAllCollisionPairs(
            ghostObject.getOverlappingPairCache(),
            collisionWorld.getDispatchInfo(),
            collisionWorld.getDispatcher());

    currentPosition.set(ghostObject.getWorldTransform(new Transform()).origin);

    double maxPen = 0.0f;
    for (int i = 0; i < ghostObject.getOverlappingPairCache().getNumOverlappingPairs(); i++) {
      manifoldArray.clear();

      BroadphasePair collisionPair =
          ghostObject.getOverlappingPairCache().getOverlappingPairArray().getQuick(i);

      if (collisionPair.algorithm != null) {
        collisionPair.algorithm.getAllContactManifolds(manifoldArray);
      }

      for (int j = 0; j < manifoldArray.size(); j++) {
        PersistentManifold manifold = manifoldArray.getQuick(j);
        double directionSign = manifold.getBody0() == ghostObject ? -1.0f : 1.0f;
        for (int p = 0; p < manifold.getNumContacts(); p++) {
          ManifoldPoint pt = manifold.getContactPoint(p);

          double dist = pt.getDistance();
          if (dist < 0.0f) {
            if (dist < maxPen) {
              maxPen = dist;
              touchingNormal.set(pt.normalWorldOnB); // ??
              touchingNormal.scale(directionSign);
            }

            currentPosition.scaleAdd(
                directionSign * dist * 0.2f, pt.normalWorldOnB, currentPosition);

            penetration = true;
          } else {
            // printf("touching %f\n", dist);
          }
        }

        // manifold->clearManifold();
      }
    }

    Transform newTrans = ghostObject.getWorldTransform(new Transform());
    newTrans.origin.set(currentPosition);
    ghostObject.setWorldTransform(newTrans);
    // printf("m_touchingNormal =
    // %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);

    // System.out.println("recoverFromPenetration "+penetration+" "+touchingNormal);

    return penetration;
  }
示例#5
0
  private void dotest(final Record rec) {
    PolyTree ptree1;
    PolyTree ptree2;

    ptree1 = (PolyTree) bodies.get(rec.poly1);
    if (ptree1 == null) {
      System.err.println("PolyTree " + rec.poly1 + " not found");
      System.exit(1);
    }
    ptree2 = (PolyTree) bodies.get(rec.poly2);
    if (ptree2 == null) {
      System.err.println("PolyTree " + rec.poly2 + " not found");
      System.exit(1);
    }
    double[] dlist;
    final Vector3d offset = new Vector3d();
    if (ptree1.numNodes() > 0 || ptree2.numNodes() > 0)
      dlist = new double[] {-0.1, 0.1, rec.minDist};
    else dlist = new double[] {-0.1, 0.1, 1.0, rec.minDist};
    //	   dlist = new double[] { rec.minDist };
    for (final double element : dlist) {
      offset.scale(element - rec.minDist, rec.minNrml);
      if (exhaustive) exhaustiveCheck(rec, offset, ptree1, ptree2);
      else singleCheck(rec, offset, ptree1, ptree2, null, null);
    }
  }
  /**
   * setLinkStatus
   *
   * <p>Debugようにこのメソッドを叩くと現在のRobotの状態をIntegratorに通知する
   *
   * @param objectName
   */
  private void _setLinkStatus(String objectName) {
    if (robot_ == null) robot_ = (GrxModelItem) manager_.getItem(GrxModelItem.class, objectName);

    TransformGroup tg = robot_.getTransformGroupRoot();
    Transform3D t3d = new Transform3D();
    tg.getTransform(t3d);
    Vector3d pos = new Vector3d();
    Matrix3d mat = new Matrix3d();
    t3d.get(mat, pos);

    double[] value = new double[12];
    pos.get(value);
    value[3] = mat.m00;
    value[4] = mat.m01;
    value[5] = mat.m02;
    value[6] = mat.m10;
    value[7] = mat.m11;
    value[8] = mat.m12;
    value[9] = mat.m20;
    value[10] = mat.m21;
    value[11] = mat.m22;

    integrator_.setCharacterLinkData(
        objectName, robot_.rootLink().getName(), LinkDataType.ABS_TRANSFORM, value);
    integrator_.setCharacterAllLinkData(
        objectName, LinkDataType.JOINT_VALUE, robot_.getJointValues());
    integrator_.calcCharacterForwardKinematics(objectName);
  }
 /**
  * Returns the reflection direction of a ray going 'direction' hitting a surface with normal
  * 'normal'.
  *
  * <p>From: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
  */
 protected Vector3d computeReflectionDirection(Vector3d direction, Vector3d normal, Vector3d out) {
   // return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
   out.set(normal);
   out.scale(-2.0f * direction.dot(normal));
   out.add(direction);
   return out;
 }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 300000)
  public void testTwoIntersectingBoxes() {
    CombinedTerrainObject3D combinedTerrainObject =
        new CombinedTerrainObject3D("Combined Terrain Object to Test");
    Point3d pointToCheck = new Point3d();
    Point3d expectedIntersection = new Point3d();
    Vector3d expectedNormal = new Vector3d();

    Point3d resultIntersection = new Point3d();
    Vector3d resultNormal = new Vector3d();

    setupTwoIntersectingBoxesMadeFromPolygons(combinedTerrainObject);

    pointToCheck.set(0.4, 0.45, 0.25);
    expectedIntersection.set(0.4, 0.5, 0.25);
    expectedNormal.set(0.0, 1.0, 0.0);

    combinedTerrainObject.checkIfInside(
        pointToCheck.getX(),
        pointToCheck.getY(),
        pointToCheck.getZ(),
        resultIntersection,
        resultNormal);
    JUnitTools.assertTuple3dEquals(expectedIntersection, resultIntersection, 1e-4);
    JUnitTools.assertTuple3dEquals(expectedNormal, resultNormal, 1e-4);
  }
示例#9
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;
 }
示例#10
0
 public void setViewingDirection(double yaw, double pitch) {
   _viewingDirection.set(
       Math.sin(Math.toRadians(yaw)) * Math.cos(Math.toRadians(pitch)),
       -Math.sin(Math.toRadians(pitch)),
       -Math.cos(Math.toRadians(pitch)) * Math.cos(Math.toRadians(yaw)));
   _viewingDirection.normalize(_viewingDirection);
 }
 /** Returns the portion of 'direction' that is parallel to 'normal' */
 protected Vector3d parallelComponent(Vector3d direction, Vector3d normal, Vector3d out) {
   // btScalar magnitude = direction.dot(normal);
   // return normal * magnitude;
   out.set(normal);
   out.scale(direction.dot(normal));
   return out;
 }
示例#12
0
  /** Updates the status if the entity is currently swimming (in water). */
  private void updateSwimStatus() {
    ArrayList<BlockPosition> blockPositions = gatherAdjacentBlockPositions(getPosition());

    boolean swimming = false, headUnderWater = false;

    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).isLiquid()
          && getAABB().overlaps(blockAABB)) {
        swimming = true;
      }

      Vector3d eyePos = calcEyePosition();
      eyePos.y += 0.25;

      if (BlockManager.getInstance().getBlock(blockType).isLiquid() && blockAABB.contains(eyePos)) {
        headUnderWater = true;
      }
    }

    _headUnderWater = headUnderWater;
    _isSwimming = swimming;
  }
示例#13
0
  /**
   * Returns the normal of the plane closest to the given origin.
   *
   * @param pointOnAABB A point on the AABB
   * @param origin The origin
   * @param testX True if the x-axis should be tested
   * @param testY True if the y-axis should be tested
   * @param testZ True if the z-axis should be tested
   * @return The normal
   */
  public Vector3d normalForPlaneClosestToOrigin(
      Vector3d pointOnAABB, Vector3d origin, boolean testX, boolean testY, boolean testZ) {
    ArrayList<Vector3d> normals = new ArrayList<Vector3d>();

    if (pointOnAABB.z == minZ() && testZ) normals.add(new Vector3d(0, 0, -1));
    if (pointOnAABB.z == maxZ() && testZ) normals.add(new Vector3d(0, 0, 1));
    if (pointOnAABB.x == minX() && testX) normals.add(new Vector3d(-1, 0, 0));
    if (pointOnAABB.x == maxX() && testX) normals.add(new Vector3d(1, 0, 0));
    if (pointOnAABB.y == minY() && testY) normals.add(new Vector3d(0, -1, 0));
    if (pointOnAABB.y == maxY() && testY) normals.add(new Vector3d(0, 1, 0));

    double minDistance = Double.MAX_VALUE;
    Vector3d closestNormal = new Vector3d();

    for (int i = 0; i < normals.size(); i++) {
      Vector3d n = normals.get(i);

      Vector3d diff = new Vector3d(centerPointForNormal(n));
      diff.sub(origin);

      double distance = diff.length();

      if (distance < minDistance) {
        minDistance = distance;
        closestNormal = n;
      }
    }

    return closestNormal;
  }
 /** Returns the portion of 'direction' that is perpindicular to 'normal' */
 protected Vector3d perpindicularComponent(Vector3d direction, Vector3d normal, Vector3d out) {
   // return direction - parallelComponent(direction, normal);
   Vector3d perpendicular = parallelComponent(direction, normal, out);
   perpendicular.scale(-1);
   perpendicular.add(direction);
   return perpendicular;
 }
示例#15
0
  /**
   * Return a midpoint of a helix, calculated from three positions of three adjacent subunit
   * centers.
   *
   * @param p1 center of first subunit
   * @param p2 center of second subunit
   * @param p3 center of third subunit
   * @return midpoint of helix
   */
  private Point3d getMidPoint(Point3d p1, Point3d p2, Point3d p3) {
    Vector3d v1 = new Vector3d();
    v1.sub(p1, p2);
    Vector3d v2 = new Vector3d();
    v2.sub(p3, p2);
    Vector3d v3 = new Vector3d();
    v3.add(v1);
    v3.add(v2);
    v3.normalize();

    // calculat the total distance between to subunits
    double dTotal = v1.length();
    // calculate the rise along the y-axis. The helix axis is aligned with y-axis,
    // therfore, the rise between subunits is the y-distance
    double rise = p2.y - p1.y;
    // use phythagorean theoremm to calculate chord length between two subunit centers
    double chord = Math.sqrt(dTotal * dTotal - rise * rise);
    //		System.out.println("Chord d: " + dTotal + " rise: " + rise + "chord: " + chord);
    double angle = helixLayers.getByLargestContacts().getAxisAngle().angle;

    // using the axis angle and the chord length, we can calculate the radius of the helix
    // http://en.wikipedia.org/wiki/Chord_%28geometry%29
    double radius = chord / Math.sin(angle / 2) / 2; // can this go to zero?
    //		System.out.println("Radius: " + radius);

    // project the radius onto the vector that points toward the helix axis
    v3.scale(radius);
    v3.add(p2);
    //		System.out.println("Angle: " +
    // Math.toDegrees(helixLayers.getByLowestAngle().getAxisAngle().angle));
    Point3d cor = new Point3d(v3);
    return cor;
  }
 public void propertyChange(PropertyChangeEvent pce) {
   Object source = pce.getSource();
   if (source == posSlider_x) {
     double posX = ((Double) pce.getNewValue()).doubleValue();
     posDisk.x = posX;
     Disk.setNode3D(ShapeNodeDisk);
     Disk.setPosition(posDisk);
     PlaceBNVectors();
   } else if (source == posSlider_y) {
     double posY = ((Double) pce.getNewValue()).doubleValue();
     posDisk.y = posY;
     Disk.setNode3D(ShapeNodeDisk);
     Disk.setPosition(posDisk);
     PlaceBNVectors();
   } else if (source == angDisk) {
     angleDisk = ((Double) pce.getNewValue()).doubleValue();
     double angDisk_rad = angleDisk * Math.PI / 180.;
     double compx = Math.cos(angDisk_rad);
     double compy = Math.sin(angDisk_rad);
     Disk.setNode3D(ShapeNodeDisk);
     Disk.setDirection(new Vector3d(compx, compy, 0.));
     flux_plot.setNormalDisk(new Vector3d(compx, compy, 0.));
     PlaceBNVectors();
   } else if (source == radDisk) {
     radiusDisk = ((Double) pce.getNewValue()).doubleValue();
     ShapeNodeDisk.setGeometry(Cylinder.makeGeometry(32, radiusDisk, heightDisk));
     Disk.setNode3D(ShapeNodeDisk);
     arrowScale = arrowScaleInitial * radiusDisk / radiusDiskInitial;
     flux_plot.setRadiusDisk(radiusDisk);
     PlaceBNVectors();
   } else {
     super.propertyChange(pce);
   }
 }
 // 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;
 }
示例#18
0
 /* (non-Javadoc)
  * @see org.biojava.nbio.structure.quaternary.core.AxisAligner#getDimension()
  */
 @Override
 public Vector3d getDimension() {
   run();
   Vector3d dimension = new Vector3d();
   dimension.sub(maxBoundary, minBoundary);
   dimension.scale(0.5);
   return dimension;
 }
 public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthWall(
     int nthWall) {
   double alpha = ((double) nthWall) / 7.0;
   Vector3d startingLocation = morph(firstPlatform, lastPlatform, alpha);
   startingLocation.add(new Vector3d(-1.0 - 0.1, 1.0 - 0.1, 0.0));
   return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(
       startingLocation, yaw - Math.PI / 2.0);
 }
 @Override
 public GroundProfile3D getGroundProfile() {
   Vector3d surfaceNormal = new Vector3d(0.03, 0.27, 1.0);
   surfaceNormal.normalize();
   Point3d intersectionPoint = new Point3d(0.22, 2.2, -0.4);
   double maxXY = 10.0;
   return new SlopedPlaneGroundProfile(surfaceNormal, intersectionPoint, maxXY);
 }
示例#21
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 perturb(Vector3d direction) {
   Vector3d force = new Vector3d(direction);
   if (direction.lengthSquared() > 0.0) {
     force.normalize();
     force.scale(disturbanceMagnitude.getDoubleValue());
     forcePerturbable.setForcePerturbance(force, disturbanceDuration.getDoubleValue());
   }
 }
 // static helper method
 private static Vector3d getNormalizedVector(Vector3d v, Vector3d out) {
   out.set(v);
   out.normalize();
   if (out.length() < BulletGlobals.SIMD_EPSILON) {
     out.set(0, 0, 0);
   }
   return out;
 }
示例#24
0
    boolean check(final ClosestPointPair cpair) {
      final Vector3d del = new Vector3d();

      del.sub(cpair.pnt2, cpair.pnt1);
      if (Math.abs(del.length() - d) > TOL) return false;
      del.normalize();
      if (!vec.epsilonEquals(del, TOL)) return false;
      return true;
    }
  public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup
      createInitialSetupAtNthPlatformToesTouching(int nthPlatform) {
    double alpha = ((double) nthPlatform) / 7.0;
    Vector3d startingLocation = morph(firstPlatform, lastPlatform, alpha);

    startingLocation.add(new Vector3d(-0.11, -0.16, 0.0));

    return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw);
  }
 // gets a point not on vector a...b; this can be used to define a plan or cross products
 private static Vector3d getNonColinearVector(Vector3d ab) {
   Vector3d cr = new Vector3d();
   cr.cross(ab, XV);
   if (cr.length() > 0.00001) {
     return XV;
   } else {
     return YV;
   }
 }
  /**
   * @param datasets input 2D dataset
   * @return one 2D dataset
   */
  @Override
  public List<Dataset> value(IDataset... datasets) {
    if (datasets.length == 0) return null;

    List<Dataset> result = new ArrayList<Dataset>();

    for (IDataset ids : datasets) {
      Dataset ds = DatasetUtils.convertToDataset(ids);
      // check if input is 2D
      int[] s = ds.getShape();
      if (s.length != 2) return null;

      // find extent of projected dataset
      int[] pos = new int[2];
      int[] start = pshape.clone();
      int[] stop = new int[2];

      qToPixel(qspace.qFromPixelPosition(0, 0), pos);
      adjustBoundingBox(start, stop, pos);
      qToPixel(qspace.qFromPixelPosition(s[0], 0), pos);
      adjustBoundingBox(start, stop, pos);
      qToPixel(qspace.qFromPixelPosition(0, s[1]), pos);
      adjustBoundingBox(start, stop, pos);
      qToPixel(qspace.qFromPixelPosition(s[0], s[1]), pos);
      adjustBoundingBox(start, stop, pos);
      stop[0]++;
      stop[1]++;

      // iterate over bounding box in project dataset
      //     find image point
      //     calculate interpolated value
      //     store running average
      // (could have done this using a slice iterator)
      Vector3d qy = new Vector3d();
      Vector3d q = new Vector3d();
      Vector3d t = new Vector3d();
      double delta, value;
      int i = 0, n;
      for (int y = start[0]; y < stop[0]; y++) {
        qy.scale((y + roff) * cdel, col);
        for (int x = start[1]; x < stop[1]; x++) {
          q.scaleAdd((x + coff) * rdel, row, qy);
          qspace.pixelPosition(q, t);
          n = (int) (count.getElementLongAbs(i) + 1);
          value = image.getElementDoubleAbs(i);
          delta = Maths.interpolate(ds, t.y, t.x) - value;
          image.setObjectAbs(i, value + (delta / n));
          count.setObjectAbs(i, n);
          i++;
        }
      }

      result.add(image);
      result.add(count);
    }
    return result;
  }
  public static Vector3d morph(Vector3d point1, Vector3d point2, double alpha) {
    Vector3d framePoint1 = new Vector3d(point1);
    Vector3d framePoint2 = new Vector3d(point2);

    framePoint1.scale(1.0 - alpha);
    framePoint2.scale(alpha);
    framePoint1.add(framePoint2);

    return framePoint1;
  }
  /**
   * Convert from q to a pixel coordinate (discretization)
   *
   * @param q
   * @param pos (to be used for a dataset so is row-major: x->2, y->1)
   */
  private void qToPixel(final Vector3d q, int[] pos) {
    pos[0] = -1;
    pos[1] = -1;

    final int x = (int) Math.floor(q.dot(row) / rdel) - roff;
    pos[1] = (x < 0 || x > pshape[1]) ? -1 : x;

    final int y = (int) Math.floor(q.dot(col) / cdel) - coff;
    pos[0] = (y < 0 || y > pshape[0]) ? -1 : y;
  }
  public void playerStep(CollisionWorld collisionWorld, double dt) {
    // printf("playerStep(): ");
    // printf("  dt = %f", dt);

    // quick check...
    if (!useWalkDirection && velocityTimeInterval <= 0.0f) {
      // printf("\n");
      return; // no motion
    }

    wasOnGround = onGround();

    // Update fall velocity.
    verticalVelocity -= gravity * dt;
    if (verticalVelocity > 0.0 && verticalVelocity > jumpSpeed) {
      verticalVelocity = jumpSpeed;
    }
    if (verticalVelocity < 0.0 && Math.abs(verticalVelocity) > Math.abs(fallSpeed)) {
      verticalVelocity = -Math.abs(fallSpeed);
    }
    verticalOffset = verticalVelocity * dt;

    Transform xform = ghostObject.getWorldTransform(new Transform());

    // printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
    // printf("walkSpeed=%f\n",walkSpeed);

    stepUp(collisionWorld);
    if (useWalkDirection) {
      // System.out.println("playerStep 3");
      stepForwardAndStrafe(collisionWorld, walkDirection);
    } else {
      System.out.println("playerStep 4");
      // printf("  time: %f", m_velocityTimeInterval);

      // still have some time left for moving!
      double dtMoving = (dt < velocityTimeInterval) ? dt : velocityTimeInterval;
      velocityTimeInterval -= dt;

      // how far will we move while we are moving?
      Vector3d move = new Vector3d();
      move.scale(dtMoving, walkDirection);

      // printf("  dtMoving: %f", dtMoving);

      // okay, step
      stepForwardAndStrafe(collisionWorld, move);
    }
    stepDown(collisionWorld, dt);

    // printf("\n");

    xform.origin.set(currentPosition);
    ghostObject.setWorldTransform(xform);
  }