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);
}
}
kmQuaternion *sls_trackball_calc_quat(kmQuaternion *out, float trackball_radius,
float trackball_speed, kmVec2 const *p1,
kmVec2 const *p2) {
//sls_log_info("p1 %f %f, p2 %f %f", p1->x, p1->y, p2->x, p2->y);
kmVec3 axis;
kmVec3 _p1, _p2, dir;
float phi;
float t;
float epsilon = 0.001;
if (sls_vec2_near(p1, p2, epsilon)) {
// zero rotation
kmQuaternionIdentity(out);
return out;
}
_p1 = (kmVec3){p1->x, p1->y, sls_tb_project_to_sphere(trackball_radius, p1)};
_p2 = (kmVec3){p2->x, p2->y, sls_tb_project_to_sphere(trackball_radius, p2)};
kmVec3Subtract(&dir, &_p1, &_p2);
kmVec3Cross(&axis, &_p2, &_p1);
t = kmVec3Length(&dir) / (2.0f * trackball_radius);
t = fminf(fmaxf(-1.f, t), 1.f);
phi = 2.0f * asinf(t) * trackball_speed;
kmQuaternion tmp_a, tmp_b;
kmQuaternionRotationAxisAngle(&tmp_a, &axis, phi);
tmp_b = *out;
return kmQuaternionMultiply(out, &tmp_a, &tmp_b);
;
}
lite3d_scene_node *lite3d_scene_node_rotate_by(lite3d_scene_node *node, const kmQuaternion *quat)
{
SDL_assert(node && quat);
kmQuaternionMultiply(&node->rotation, quat, &node->rotation);
node->recalc = LITE3D_TRUE;
return node;
}
void AnimationTarget::convertQuaternionByValues(float* from, float* by)
{
kmQuaternion qFrom = quaIdentity;
kmQuaternionSet(&qFrom, from[0], from[1], from[2], from[3]);
kmQuaternion qBy = quaIdentity;
kmQuaternionSet(&qBy, by[0], by[1], by[2], by[3]);
//qBy.multiply(qFrom);
kmQuaternionMultiply(&qBy, &qBy, &qFrom);
memcpy(by, (float*)&qBy, sizeof(float) * 4);
}