예제 #1
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;
  }
 /** 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;
 }
 /**
  * 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;
 }
 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);
 }
  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);
  }
  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;
  }
예제 #7
0
  /** 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();
      }
    }
  }
  protected void updateClosedJoint() {
    super.updateClosedJoint();

    integralForce.scale(
        integralStiffness.getDoubleValue(), yoConnectionPositionIntegratedError.getFrameTuple());

    connectionPointA.getForce(tempForce);
    tempForce.add(integralForce.getVector());

    connectionPointA.setForce(tempForce);
    tempForce.scale(-1.0);
    connectionPointB.setForce(tempForce);
  }
  /**
   * Calculate new point(s) X in a B-A-C system. It forms form a B-A(-C)-X system.
   *
   * <p>(2) 2 ligands(B, C) of refAtom A (i) 1 points required; vector in ABC plane bisecting AB,
   * AC. If ABC is linear, no points (ii) 2 points: 2 points X1, X2, X1-A-X2 = angle about 2i vector
   *
   * @param aPoint to which substituents are added
   * @param bPoint first ligand of A
   * @param cPoint second ligand of A
   * @param nwanted number of points to calculate (1-2)
   * @param length A-X length
   * @param angle B-A-X angle
   * @return Point3d[] nwanted points (or zero if failed)
   */
  public static Point3d[] calculate3DCoordinates2(
      Point3d aPoint, Point3d bPoint, Point3d cPoint, int nwanted, double length, double angle) {
    Point3d newPoints[] = new Point3d[0];
    double ang2 = angle / 2.0;

    Vector3d ba = new Vector3d(aPoint);
    ba.sub(bPoint);
    Vector3d ca = new Vector3d(aPoint);
    ca.sub(cPoint);
    Vector3d baxca = new Vector3d();
    baxca.cross(ba, ca);
    if (baxca.length() < 0.00000001) {; // linear
    } else if (nwanted == 1) {
      newPoints = new Point3d[1];
      Vector3d ax = new Vector3d(ba);
      ax.add(ca);
      ax.normalize();
      ax.scale(length);
      newPoints[0] = new Point3d(aPoint);
      newPoints[0].add(ax);
    } else if (nwanted == 2) {
      newPoints = new Point3d[2];
      Vector3d ax = new Vector3d(ba);
      ax.add(ca);
      ax.normalize();
      baxca.normalize();
      baxca.scale(Math.sin(ang2) * length);
      ax.scale(Math.cos(ang2) * length);
      newPoints[0] = new Point3d(aPoint);
      newPoints[0].add(ax);
      newPoints[0].add(baxca);
      newPoints[1] = new Point3d(aPoint);
      newPoints[1].add(ax);
      newPoints[1].sub(baxca);
    }
    return newPoints;
  }
  @SuppressWarnings("all")
  protected void updateTargetPositionBasedOnCollision(
      Vector3d hitNormal, double tangentMag, double normalMag) {
    Vector3d movementDirection = new Vector3d();
    movementDirection.sub(targetPosition, currentPosition);
    double movementLength = movementDirection.length();
    if (movementLength > BulletGlobals.SIMD_EPSILON) {
      movementDirection.normalize();

      Vector3d reflectDir =
          computeReflectionDirection(movementDirection, hitNormal, new Vector3d());
      reflectDir.normalize();

      Vector3d parallelDir = parallelComponent(reflectDir, hitNormal, new Vector3d());
      Vector3d perpindicularDir = perpindicularComponent(reflectDir, hitNormal, new Vector3d());

      targetPosition.set(currentPosition);

      if (false) // tangentMag != 0.0)
      {
        Vector3d parComponent = new Vector3d();
        parComponent.scale(tangentMag * movementLength, parallelDir);
        // printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
        targetPosition.add(parComponent);
      }

      if (normalMag != 0.0f) {
        Vector3d perpComponent = new Vector3d();
        perpComponent.scale(normalMag * movementLength, perpindicularDir);
        // printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
        targetPosition.add(perpComponent);
      }
    } else {
      // printf("movementLength don't normalize a zero vector\n");
    }
  }
예제 #11
0
  void translate(final int xNew, final int yNew) {
    if (content == null || content.isLocked()) return;
    final int dx = xNew - xLast;
    final int dy = yNew - yLast;
    translateTG.getTransform(translateOld);
    v1.scale(dx, translationPerDx);
    v2.scale(-dy, translationPerDy);
    translation.add(v1, v2);
    translateNew.set(translation);
    translateNew.mul(translateOld);

    translateTG.setTransform(translateNew);
    transformChanged(BehaviorCallback.TRANSLATE, translateNew);

    xLast = xNew;
    yLast = yNew;
  }
예제 #12
0
  public Vector3d getReaction(Vector3d position, Vector3d velocity, Vector3d action, double mass) {

    Vector3d relativePosition = new Vector3d(position);
    relativePosition.sub(point);
    double x = relativePosition.length();
    Vector3d reaction = new Vector3d(relativePosition);
    if (x > Teal.DoubleZero) {
      reaction.scale(-k1);
    }

    double v = velocity.length();
    Vector3d damping = new Vector3d(velocity);
    if (v > Teal.DoubleZero) {
      // damping.scale(1./v);
      damping.scale(-k2 * (1.0 + 0.0 / v) / (1. + (x / p) * (x / p)));
      reaction.add(damping);
    }
    lastReaction.set(reaction);
    return reaction;
  }
 /**
  * Calculate new point X in a B-A(-D)-C system. It forms a B-A(-D)(-C)-X system.
  *
  * <p>(3) 3 ligands(B, C, D) of refAtom A (i) 1 points required; if A, B, C, D coplanar, no
  * points. else vector is resultant of BA, CA, DA
  *
  * @param aPoint to which substituents are added
  * @param bPoint first ligand of A
  * @param cPoint second ligand of A
  * @param dPoint third ligand of A
  * @param length A-X length
  * @return Point3d nwanted points (or null if failed (coplanar))
  */
 public static Point3d calculate3DCoordinates3(
     Point3d aPoint, Point3d bPoint, Point3d cPoint, Point3d dPoint, double length) {
   Vector3d v1 = new Vector3d(aPoint);
   v1.sub(bPoint);
   Vector3d v2 = new Vector3d(aPoint);
   v2.sub(cPoint);
   Vector3d v3 = new Vector3d(aPoint);
   v3.sub(dPoint);
   Vector3d v = new Vector3d(bPoint);
   v.add(cPoint);
   v.add(dPoint);
   if (v.length() < 0.00001) {
     return null;
   }
   v.normalize();
   v.scale(length);
   Point3d point = new Point3d(aPoint);
   point.add(v);
   return point;
 }
예제 #14
0
  /**
   * Shades a point with the same algorithm used by the {@link <a
   * href="http://surf.sourceforge.net">surf raytracer</a>}.
   *
   * @param hitPoint Intersection point.
   * @param v View vector (from intersection point to eye).
   * @param n Surface normal.
   * @param material Surface material.
   * @return
   */
  protected Color3f shadeWithMaterial(
      Point3d hitPoint,
      Vector3d v,
      Vector3d n,
      Color3f ambientColor,
      LightProducts[] lightProducts) {
    Vector3d l = new Vector3d();
    Vector3d h = new Vector3d();

    Color3f color = new Color3f(ambientColor);

    for (int i = 0; i < dcsd.lightSources.length; i++) {
      LightSource lightSource = dcsd.lightSources[i];

      l.sub(lightSource.getPosition(), hitPoint);
      l.normalize();

      float lambertTerm = (float) n.dot(l);
      if (lambertTerm > 0.0f) {
        // compute diffuse color component
        color.scaleAdd(lambertTerm, lightProducts[i].getDiffuseProduct(), color);

        // compute specular color component
        h.add(l, v);
        h.normalize();

        color.scaleAdd(
            (float)
                Math.pow(Math.max(0.0f, n.dot(h)), lightProducts[i].getMaterial().getShininess()),
            lightProducts[i].getSpecularProduct(),
            color);
      }
    }

    color.clampMax(1.0f);

    return color;
  }
예제 #15
0
  /** performs the gl camera transformations for the right eye */
  public void translateRightEye(GL gl, GLU glu, GLUT glut) {
    // calculate the right vector
    Vector3d right = new Vector3d();
    Vector3d temp1 = (Vector3d) cameraTarget.clone();
    temp1.scale(-1);
    Vector3d temp2 = (Vector3d) cameraPosition.clone();

    temp2.add(temp1);
    right.cross(cameraUp, temp2);

    right.normalize();
    right.scale(eyespread / 2.);
    glu.gluLookAt(
        right.x + cameraPosition.x,
        right.y + cameraPosition.y,
        right.z + cameraPosition.z,
        right.x + cameraTarget.x,
        right.y + cameraTarget.y,
        right.z + cameraTarget.z,
        cameraUp.x,
        cameraUp.y,
        cameraUp.z);
  }
예제 #16
0
  public void addChargesToList2(int number) {
    for (int k = 0; k < number; k++) {
      Vector3d position = new Vector3d();
      int count = 0;
      do {
        count++;
        double x = (2. * Math.random() - 1.) * (plate_length / 2. - pc_radius);
        double y = (2. * Math.random() - 1.) * (plate_height / 2. - pc_radius);
        double z = (2. * Math.random() - 1.) * (plate_width / 2. - pc_radius);
        position.set(x, y, z);
        position.add(plate2_position);
      } while (!isValidList2Position(position));

      double charge = 0.;
      if (!pcList2.isEmpty()) charge = ((PointCharge) pcList2.get(0)).getCharge();
      else charge = -pc_charge;

      PointCharge pc = new PointCharge();
      pc.setPosition(position);
      pc.setCharge(charge);
      pc.setRadius(pc_radius);

      pc.setMass(1.0);
      pc.setID("pcList2_charge" + pcList2.size());
      pc.setPickable(false);
      pc.setColliding(true);
      pc.setGeneratingP(false);

      SphereCollisionController scc = new SphereCollisionController(pc);
      scc.setRadius(pc_radius);
      scc.setTolerance(0.5);
      scc.setMode(SphereCollisionController.WALL_SPHERE);
      pc.setCollisionController(scc);
      addElement(pc);
      pcList2.add(pc);
    }
  }
예제 #17
0
 @Override
 public void convertToAbsolutePosition(Vector3d point) {
   M.transform(point);
   point.add(origin);
 }
  protected void stepForwardAndStrafe(CollisionWorld collisionWorld, Vector3d walkMove) {
    // printf("m_normalizedDirection=%f,%f,%f\n",
    // 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
    // phase 2: forward and strafe
    Transform start = new Transform();
    Transform end = new Transform();
    targetPosition.add(currentPosition, walkMove);
    start.setIdentity();
    end.setIdentity();

    double fraction = 1.0f;
    Vector3d distance2Vec = new Vector3d();
    distance2Vec.sub(currentPosition, targetPosition);
    double distance2 = distance2Vec.lengthSquared();
    // printf("distance2=%f\n",distance2);

    /*if (touchingContact) {
    	if (normalizedDirection.dot(touchingNormal) > 0.0f) {
    		updateTargetPositionBasedOnCollision(touchingNormal);
    	}
    }*/

    int maxIter = 10;

    while (fraction > 0.01f && maxIter-- > 0) {
      start.origin.set(currentPosition);
      end.origin.set(targetPosition);

      KinematicClosestNotMeConvexResultCallback callback =
          new KinematicClosestNotMeConvexResultCallback(
              ghostObject, upAxisDirection[upAxis], -1.0f);
      callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
      callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;

      double margin = convexShape.getMargin();
      convexShape.setMargin(margin + addedMargin);

      if (useGhostObjectSweepTest) {
        ghostObject.convexSweepTest(
            convexShape,
            start,
            end,
            callback,
            collisionWorld.getDispatchInfo().allowedCcdPenetration);
      } else {
        collisionWorld.convexSweepTest(convexShape, start, end, callback);
      }

      convexShape.setMargin(margin);

      fraction -= callback.closestHitFraction;

      if (callback.hasHit()) {
        // we moved only a fraction
        Vector3d hitDistanceVec = new Vector3d();
        hitDistanceVec.sub(callback.hitPointWorld, currentPosition);
        // double hitDistance = hitDistanceVec.length();

        // if the distance is farther than the collision margin, move
        // if (hitDistance > addedMargin) {
        //	//printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
        //	currentPosition.interpolate(currentPosition, targetPosition,
        // callback.closestHitFraction);
        // }

        updateTargetPositionBasedOnCollision(callback.hitNormalWorld);

        Vector3d currentDir = new Vector3d();
        currentDir.sub(targetPosition, currentPosition);
        distance2 = currentDir.lengthSquared();
        if (distance2 > BulletGlobals.SIMD_EPSILON) {
          currentDir.normalize();
          // see Quake2: "If velocity is against original velocity, stop ead to avoid tiny
          // oscilations in sloping corners."
          if (currentDir.dot(normalizedDirection) <= 0.0f) {
            break;
          }
        } else {
          // printf("currentDir: don't normalize a zero vector\n");
          break;
        }
      } else {
        // we moved whole way
        currentPosition.set(targetPosition);
      }

      // if (callback.m_closestHitFraction == 0.f)
      //    break;
    }
  }
예제 #19
0
  public Vector3d calcEyePosition() {
    Vector3d eyePosition = new Vector3d(getPosition());
    eyePosition.add(calcEyeOffset());

    return eyePosition;
  }