public Segment(float angle) { float[] q = new float[4]; float[] n = {0, 0, 1}; float tempCross[][] = new float[4][3]; for (int i = 0; i < 4; i++) { for (int j = 0; j < 3; j++) { tempCross[i][j] = initialCross[i][j]; } } SensorDataProcessing.quaternion(n, angle, q); for (int i = 0; i < 4; i++) { SensorDataProcessing.quatRotate(q, tempCross[i], initialCross[i]); } }
/** * 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]); } }
/** * 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); } } }
/** * 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); } } }