Ejemplo n.º 1
0
  /**
   * Draws the phone.
   *
   * @param graphics
   */
  protected void paintComponent(Graphics graphics) {
    super.paintComponent(graphics);
    // g.drawString("This is my custom Panel!",(int)yawDegree,(int)pitch);

    Graphics2D g2 = (Graphics2D) graphics;
    // draw Line2D.Double

    double centerx = 100;
    double centery = 100;
    double centerz = -150;
    for (int i = 0; i < phone.length; i += 2) {
      if (i == 0) g2.setColor(Color.RED);
      if (i == 24) g2.setColor(Color.BLUE);

      Vector v1 = new Vector(phone[i]);
      Vector v2 = new Vector(phone[i + 1]);
      v1.rollpitchyaw(rollDegree, pitchDegree, yawDegree);
      v2.rollpitchyaw(rollDegree, pitchDegree, yawDegree);
      g2.draw(
          new Line2D.Double(
              centerx + (v1.x + movex) * centerz / (centerz - v1.y),
              centery - (v1.z + movez) * centerz / (centerz - v1.y),
              centerx + (v2.x + movex) * centerz / (centerz - v2.y),
              centery - (v2.z + movez) * centerz / (centerz - v2.y)));
    }

    if (mSensorSimulator.isShowAcceleration()) {
      // Now we also draw the acceleration:
      g2.setColor(Color.GREEN);
      Vector v1 = new Vector(0, 0, 0);
      Vector v2 = new Vector(accelx, accely, accelz);
      v2.scale(20 * ginverse);
      // Vector v2 = new Vector(1, 0, 0);
      v1.rollpitchyaw(rollDegree, pitchDegree, yawDegree);
      v2.rollpitchyaw(rollDegree, pitchDegree, yawDegree);
      g2.draw(
          new Line2D.Double(
              centerx + (v1.x + movex) * centerz / (centerz - v1.y),
              centery - (v1.z + movez) * centerz / (centerz - v1.y),
              centerx + (v2.x + movex) * centerz / (centerz - v2.y),
              centery - (v2.z + movez) * centerz / (centerz - v2.y)));
    }
  }
Ejemplo n.º 2
0
  /**
   * Updates physical model of all sensors by minimum time-step.
   *
   * <p>This internal update provides the close-to-continuum description of the sensors. It does not
   * yet provide the values that are read out by the Sensors class (which are obtained by further
   * time-selection or averaging).
   */
  public void updateSensorPhysics() {
    Vector vec;
    double random;

    // Update the timer if necessary:
    double newdelay;
    newdelay = mSensorSimulator.getUpdateSensors();
    if (newdelay > 0) {
      mSensorSimulator.setDelay((int) newdelay);
    }

    dt = 0.001 * mSensorSimulator.getDelay(); // from ms to s
    g = mSensorSimulator.getGravityConstant();
    if (g != 0) {
      ginverse = 1 / g;
    }
    meterperpixel = 1 / mSensorSimulator.getPixelsPerMeter();
    k = mSensorSimulator.getSpringConstant();
    gamma = mSensorSimulator.getDampingConstant();

    /*
    		// Calculate velocity induced by mouse:
    		double f = meterperpixel / g;
    		vx = f * ((double) (movex - oldx)) / dt;
    		vz = f * ((double) (movez - oldz)) / dt;

    		// Calculate acceleration induced by mouse:
    		ax = (vx - oldvx) / dt;
    		az = (vz - oldvz) / dt;
    */
    // New physical model of acceleration:
    // Have accelerometer be steered by string.
    // We will treat this 2D only, as the rest is a linear
    // transformation, and we assume all three accelerometer
    // directions behave the same.

    // F = m * a
    // F = - k * x

    // First calculate the force acting on the
    // sensor test particle, assuming that
    // the accelerometer is mounted by a string:
    // F = - k * x
    Fx = +k * (movex - accx);
    Fz = +k * (movez - accz);

    // a = F / m
    ax = Fx / m;
    az = Fz / m;

    // Calculate velocity by integrating
    // the current acceleration.
    // Take into account damping
    // by damping constant gamma.
    // integrate dv/dt = a - v*gamma
    // vx += (ax - vx * gamma) * dt;
    // vz += (az - vz * gamma) * dt;

    vx += (ax) * dt;
    vz += (az) * dt;

    // Now this is the force that tries to adjust
    // the accelerometer back
    // integrate dx/dt = v;
    accx += vx * dt;
    accz += vz * dt;

    // We put damping here: We don't want to damp for
    // zero motion with respect to the background,
    // but with respect to the mobile phone:
    accx += gamma * (movex - accx) * dt;
    accz += gamma * (movez - accz) * dt;

    /*
    // Old values:
    oldx = movex;
    oldz = movez;
    oldvx = vx;
    oldvz = vz;
    */

    // Calculate acceleration by gravity:
    double gravityax;
    double gravityay;
    double gravityaz;

    gravityax = mSensorSimulator.getGravityX();
    gravityay = mSensorSimulator.getGravityY();
    gravityaz = mSensorSimulator.getGravityZ();

    ////
    // Now calculate this into mobile phone acceleration:
    // ! Mobile phone's acceleration is just opposite to
    // lab frame acceleration !
    vec = new Vector(-ax * meterperpixel + gravityax, gravityay, -az * meterperpixel + gravityaz);

    // we reverse roll, pitch, and yawDegree,
    // as this is how the mobile phone sees the coordinate system.
    vec.reverserollpitchyaw(rollDegree, pitchDegree, yawDegree);

    if (mSensorSimulator.isEnabledAccelerometer()) {
      if (mSensorSimulator.useRealDeviceWiimtoe()) {
        accelx = mSensorSimulator.getWiiMoteData().getX() * g;
        accely = mSensorSimulator.getWiiMoteData().getY() * g;
        accelz = mSensorSimulator.getWiiMoteData().getZ() * g;
      } else {
        accelx = vec.x;
        accely = vec.y;
        accelz = vec.z;

        if (mSensorSimulator.useRealDeviceThinkpad()) {
          // We will use data directly from sensor instead:

          // Read data from file
          String line = "";
          try {
            // FileReader always assumes default encoding is OK!
            BufferedReader input =
                new BufferedReader(new FileReader(mSensorSimulator.getRealDevicePath()));
            try {
              line = input.readLine();
            } finally {
              input.close();
              // mSensorSimulator.mRealDeviceThinkpadOutputLabel.setBackground(Color.WHITE);
            }
          } catch (IOException ex) {
            ex.printStackTrace();
            // mSensorSimulator.mRealDeviceThinkpadOutputLabel.setBackground(Color.RED);
            line = "Error reading file!";
          }

          // Show the line content:
          mSensorSimulator.setRealDeviceOutput(line);

          // Assign values

          // Create z-component (optional)

        }

        // Add random component:
        random = mSensorSimulator.getRandomAccelerometer();
        if (random > 0) {
          accelx += getRandom(random);
          accely += getRandom(random);
          accelz += getRandom(random);
        }

        // Add accelerometer limit:
        double limit = g * mSensorSimulator.getAccelerometerLimit();
        if (limit > 0) {
          // limit on each component separately, as each is
          // a separate sensor.
          if (accelx > limit) accelx = limit;
          if (accelx < -limit) accelx = -limit;
          if (accely > limit) accely = limit;
          if (accely < -limit) accely = -limit;
          if (accelz > limit) accelz = limit;
          if (accelz < -limit) accelz = -limit;
        }
      }
    } else {
      accelx = 0;
      accely = 0;
      accelz = 0;
    }

    // Calculate magnetic field:
    // Calculate acceleration by gravity:
    double magneticnorth;
    double magneticeast;
    double magneticvertical;

    if (mSensorSimulator.isEnabledMagneticField()) {
      magneticnorth = mSensorSimulator.getMagneticFieldNorth();
      magneticeast = mSensorSimulator.getMagneticFieldEast();
      magneticvertical = mSensorSimulator.getMagneticFieldVertical();

      // Add random component:
      random = mSensorSimulator.getRandomMagneticField();
      if (random > 0) {
        magneticnorth += getRandom(random);
        magneticeast += getRandom(random);
        magneticvertical += getRandom(random);
      }

      // Magnetic vector in phone coordinates:
      vec = new Vector(magneticeast, magneticnorth, -magneticvertical);
      vec.scale(0.001); // convert from nT (nano-Tesla) to �T (micro-Tesla)

      // we reverse roll, pitch, and yawDegree,
      // as this is how the mobile phone sees the coordinate system.
      vec.reverserollpitchyaw(rollDegree, pitchDegree, yawDegree);

      compassx = vec.x;
      compassy = vec.y;
      compassz = vec.z;
    } else {
      compassx = 0;
      compassy = 0;
      compassz = 0;
    }

    // Orientation is currently not affected:
    if (mSensorSimulator.isEnabledOrientation()) {
      // yaw = Math.toRadians(yawDegree);
      // pitch = Math.toRadians(pitchDegree);
      // roll = Math.toRadians(rollDegree);
      // Since OpenGL uses degree as input,
      // and it seems also more user-friendly,
      // let us stick to degree.
      // (it seems, professional sensors also use
      //  degree output.)
      yaw = yawDegree;
      pitch = pitchDegree;
      roll = rollDegree;

      // Add random component:
      random = mSensorSimulator.getRandomOrientation();
      if (random > 0) {
        yaw += getRandom(random);
        pitch += getRandom(random);
        roll += getRandom(random);
      }
    } else {
      yaw = 0;
      pitch = 0;
      roll = 0;
    }

    // Thermometer
    if (mSensorSimulator.isEnabledTemperature()) {
      temperature = mSensorSimulator.getTemperature();

      // Add random component:
      random = mSensorSimulator.getRandomTemperature();
      if (random > 0) {
        temperature += getRandom(random);
      }
    } else {
      temperature = 0;
    }

    // Barcode
    if (mSensorSimulator.isEnabledBarcodeReader()) {
      barcode = mSensorSimulator.getBarcode();
    }

    if (mSensorSimulator.isShowAcceleration()) {
      // We only have to repaint if we show the acceleration,
      // otherwise the phone does not change as long as there is
      // no user interaction.
      repaint();
    }
    ;
  }