void MotionRecognizer::normalize(vector<kmVecPair>& data, vector<kmVec3>& norm)
{
kmQuaternion q1, q2, q3;
kmVec3 n;
vector<kmVecPair>::iterator iter;
for (iter = data.begin(); iter != data.end(); iter++) {
q1.x = ((kmVecPair)*iter).accel.x;
q1.y = ((kmVecPair)*iter).accel.y;
q1.z = ((kmVecPair)*iter).accel.z;
q1.w = 0;
if (q1.x * q1.x + q1.y * q1.y + q1.z * q1.z < ACCELERATION_THRESHOLD)
continue;
q2.x = ((kmVecPair)*iter).rot.x;
q2.y = ((kmVecPair)*iter).rot.y;
q2.z = ((kmVecPair)*iter).rot.z;
q2.w = ((kmVecPair)*iter).rot.w;
// For every acclerations, rotate them by rotation quaternion.
// Then, axes of acclerations are adjusted to global earth axes.
kmQuaternionMultiply(&q3, &q2, &q1);
kmQuaternionConjugate(&q1, &q2);
kmQuaternionMultiply(&q2, &q3, &q1);
// Normalize vector's size equal to size of codebook vector (=SQRT6)
kmQuaternionNormalize(&q1, &q2);
n.x = q1.x * SQRT6;
n.y = q1.y * SQRT6;
n.z = q1.z * SQRT6;
norm.push_back(n);
}
}
///< Returns the inverse of the passed Quaternion
kmQuaternion* kmQuaternionInverse(kmQuaternion* pOut,
const kmQuaternion* pIn)
{
kmScalar l = kmQuaternionLength(pIn);
kmQuaternion tmp;
if (fabs(l) > kmEpsilon)
{
pOut->x = 0.0;
pOut->y = 0.0;
pOut->z = 0.0;
pOut->w = 0.0;
return pOut;
}
///Get the conjugute and divide by the length
kmQuaternionScale(pOut,
kmQuaternionConjugate(&tmp, pIn), 1.0f / l);
return pOut;
}
