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);
}
}
}