Exemplo n.º 1
0
 /**
  * Sets segment current orientation
  *
  * @param sensor sensor of corresponding sensor
  */
 public void setSegmentOrientation(Sensor sensor) {
   float[] n = new float[3]; // rotation axis
   float[] vec = {0, 0, 1}; // Z axis
   float fi = 0; // rotation angle
   float[] q = new float[4]; // quaternion
   float[] accnorm = sensor.getAccNorm();
   SensorDataProcessing.crossProduct(sensor.getAccNorm(), vec, n); // rotation axis
   SensorDataProcessing.normalizeVector(n); // normalize rotation axis			
   fi =
       (float)
           Math.acos(
               SensorDataProcessing.dotProduct(vec, sensor.getAccNorm())); // get rotation angle
   SensorDataProcessing.quaternion(n, fi, q); // get quaternion
   for (int i = 0; i < 4; i++) {
     SensorDataProcessing.quatRotate(q, initialCross[i], cross[i]);
   }
 }
Exemplo n.º 2
0
  /**
   * method for compensating for tilt
   *
   * @param refferenceStateInitial
   * @param currentState
   * @param refferenceState
   * @param referenceRow
   * @param referenceCol
   */
  public static void compansateCentersForTilt(
      Vector<Vector<Segment>> refferenceStateInitial,
      Vector<Vector<Segment>> currentState,
      Vector<Vector<Segment>> refferenceState,
      int referenceRow,
      int referenceCol) {
    float n[] = new float[3];
    float n2[] = new float[3];
    float fi;
    float fi2;
    float[] q = new float[4];
    float[] q2 = new float[4];

    SensorDataProcessing.crossProduct(
        refferenceStateInitial.get(referenceRow).get(referenceCol).cross[0],
        currentState.get(referenceRow).get(referenceCol).cross[0],
        n);

    SensorDataProcessing.normalizeVector(n);

    fi =
        (float)
            Math.acos(
                SensorDataProcessing.dotProduct(
                        refferenceStateInitial.get(referenceRow).get(referenceCol).cross[0],
                        currentState.get(referenceRow).get(referenceCol).cross[0])
                    / (SensorDataProcessing.getVectorLength(
                            refferenceStateInitial.get(referenceRow).get(referenceCol).cross[0])
                        * SensorDataProcessing.getVectorLength(
                            currentState.get(referenceRow).get(referenceCol).cross[0])));

    SensorDataProcessing.quaternion(n, fi, q);

    for (int i = 0; i < refferenceState.size(); i++) {
      for (int j = 0; j < refferenceState.get(0).size(); j++) {
        SensorDataProcessing.quatRotate(
            q, refferenceStateInitial.get(i).get(j).center, refferenceState.get(i).get(j).center);
        for (int k = 0; k < 4; k++) {
          SensorDataProcessing.quatRotate(
              q,
              refferenceStateInitial.get(i).get(j).cross[k],
              refferenceState.get(i).get(j).cross[k]);
        }
      }
    }
    SensorDataProcessing.crossProduct(
        refferenceState.get(referenceRow).get(referenceCol).cross[1],
        currentState.get(referenceRow).get(referenceCol).cross[1],
        n2);
    SensorDataProcessing.normalizeVector(n2);

    fi2 =
        (float)
            Math.acos(
                SensorDataProcessing.dotProduct(
                        refferenceState.get(referenceRow).get(referenceCol).cross[1],
                        currentState.get(referenceRow).get(referenceCol).cross[1])
                    / (SensorDataProcessing.getVectorLength(
                            refferenceState.get(referenceRow).get(referenceCol).cross[1])
                        * SensorDataProcessing.getVectorLength(
                            currentState.get(referenceRow).get(referenceCol).cross[1])));
    SensorDataProcessing.quaternion(n2, fi2, q2);

    for (int i = 0; i < refferenceState.size(); i++) {
      for (int j = 0; j < refferenceState.get(0).size(); j++) {
        Segment temp = new Segment();
        temp.setInitialCross2(
            refferenceStateInitial.get(0).get(0).getVerticalDistance(),
            refferenceStateInitial.get(0).get(0).getHorizontalDistance());
        for (int k = 0; k < 3; k++) {
          temp.center[k] = refferenceState.get(i).get(j).center[k];
        }
        SensorDataProcessing.quatRotate(q2, temp.center, refferenceState.get(i).get(j).center);
      }
    }
  }
Exemplo n.º 3
0
  /**
   * method for compensating for tilt
   *
   * @param refferenceStateInitial
   * @param currentState
   * @param refferenceState
   * @param referenceRow
   * @param referenceCol
   */
  public static void compansateCentersForTilt(
      Segment[][] refferenceStateInitial,
      Segment[][] currentState,
      Segment[][] refferenceState,
      int referenceRow,
      int referenceCol) {
    float n[] = new float[3];
    float n2[] = new float[3];
    float fi;
    float fi2;
    float[] q = new float[4];
    float[] q2 = new float[4];

    SensorDataProcessing.crossProduct(
        refferenceStateInitial[referenceRow][referenceCol].cross[0],
        currentState[referenceRow][referenceCol].cross[0],
        n);

    SensorDataProcessing.normalizeVector(n);

    fi =
        (float)
            Math.acos(
                SensorDataProcessing.dotProduct(
                        refferenceStateInitial[referenceRow][referenceCol].cross[0],
                        currentState[referenceRow][referenceCol].cross[0])
                    / (SensorDataProcessing.getVectorLength(
                            refferenceStateInitial[referenceRow][referenceCol].cross[0])
                        * SensorDataProcessing.getVectorLength(
                            currentState[referenceRow][referenceCol].cross[0])));

    SensorDataProcessing.quaternion(n, fi, q);

    for (int i = 0; i < refferenceState.length; i++) {
      for (int j = 0; j < refferenceState[0].length; j++) {
        SensorDataProcessing.quatRotate(
            q, refferenceStateInitial[i][j].center, refferenceState[i][j].center);
        for (int k = 0; k < 4; k++) {
          SensorDataProcessing.quatRotate(
              q, refferenceStateInitial[i][j].cross[k], refferenceState[i][j].cross[k]);
        }
      }
    }
    SensorDataProcessing.crossProduct(
        refferenceState[referenceRow][referenceCol].cross[1],
        currentState[referenceRow][referenceCol].cross[1],
        n2);
    SensorDataProcessing.normalizeVector(n2);

    fi2 =
        (float)
            Math.acos(
                SensorDataProcessing.dotProduct(
                        refferenceState[referenceRow][referenceCol].cross[1],
                        currentState[referenceRow][referenceCol].cross[1])
                    / (SensorDataProcessing.getVectorLength(
                            refferenceState[referenceRow][referenceCol].cross[1])
                        * SensorDataProcessing.getVectorLength(
                            currentState[referenceRow][referenceCol].cross[1])));
    SensorDataProcessing.quaternion(n2, fi2, q2);

    for (int i = 0; i < refferenceState.length; i++) {
      for (int j = 0; j < refferenceState[0].length; j++) {
        Segment temp = new Segment();
        for (int k = 0; k < 3; k++) {
          temp.center[k] = refferenceState[i][j].center[k];
        }
        SensorDataProcessing.quatRotate(q2, temp.center, refferenceState[i][j].center);
      }
    }
  }