static int rotate_quaternion_into_fundamental_zone(int num_generators, double (*generator)[4], double* q)
{
double max = 0.0;
int i = 0, bi = -1;
for (i=0;i<num_generators;i++)
{
double* g = generator[i];
double t = fabs(q[0] * g[0] - q[1] * g[1] - q[2] * g[2] - q[3] * g[3]);
if (t > max)
{
max = t;
bi = i;
}
}
double f[4];
quat_rot(q, generator[bi], f);
memcpy(q, &f, 4 * sizeof(double));
if (q[0] < 0)
{
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
return bi;
}
void va_rotate(Va *va, Quat q)
{
int i;
assert(va);
for (i = 0; i < va->count; i++) {
V3f *original = (V3f *)(va->v + i * 3);
V3f rotated = quat_rot(q, *original);
*original = rotated;
}
}
void TemplateMatcher::publishTF(const Eigen::Matrix4f &transformation,const std::string &frame_id, const std::string &child_frame_id)
{
Eigen::Matrix3f Rotation= transformation.block<3,3>(0,0);
Eigen::Quaternion<float> quat_rot(Rotation);
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(0,0,0) );//transformation_result(0,3), transformation_result(1,3), transformation_result(2,3)) );
transform.setRotation( tf::Quaternion(quat_rot.x(),quat_rot.y(),quat_rot.z(),quat_rot.w()));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame_id, child_frame_id));
}
double quat_quick_disorientation_icosahedral(double* q0, double* q1)
{
double qrot[4];
double qinv[4] = {q0[0], -q0[1], -q0[2], -q0[3]};
quat_rot(qinv, q1, qrot);
rotate_quaternion_into_icosahedral_fundamental_zone(qrot);
double t = qrot[0];
t = MIN(1, MAX(-1, t));
return 2 * t * t - 1;
}
void md5_build_joints(Md5_anim *a)
{
int i;
for (i = 0; i < a->num_joints; i++) {
Md5_joint *j = a->joints + i;
Md5_joint *p = j->parent;
if (p != NULL) {
j->pos = v3f_plus(p->pos, quat_rot(p->orient, j->pos));
j->orient = quat_mul(p->orient, j->orient);
}
}
}
V3f md5_joint_transform(Md5_joint *j, V3f v)
{
return v3f_plus(quat_rot(j->orient, v), j->pos);
}
