void writeFrameBvh(ostream &os, Character::Pose &pose, vector< int > order) {
Quatd rot = rotation( -(double)M_PI * 0.5, make_vector(1.0, 0.0, 0.0) );
Quatd xrot;
Quatd yrot;
Quatd zrot;
xrot.x = 1/sqrt(2); xrot.y = 0; xrot.z = 0; xrot.w = -1/sqrt(2);
yrot.y = 1/sqrt(2); yrot.x = 0; yrot.z = 0; yrot.w = -1/sqrt(2);
zrot.z = 1/sqrt(2); zrot.x = 0; zrot.y = 0; zrot.w = 1/sqrt(2);
Vector3d root_position = vecd(pose.root_position);
//root_position = rotate(root_position, conjugate(rot));
// GOOD, but root doesn't match
// old
root_position = rotate(root_position, frame_rot());
os << FACTOR * root_position.x << " " << FACTOR * root_position.y << " " << FACTOR * root_position.z;
//os << "0 0 0";
double d[3];
//Quatd root_orientation = multiply(quatd(pose.root_orientation), xrot);
// rotate 180 degrees about the z axis, and -90 about the x-axis
Quatd root_orientation = quatd(pose.root_orientation);
root_orientation = normalize(multiply(frame_rot(), multiply(root_orientation, -frame_rot())));
root_orientation = normalize(multiply(xrot, root_orientation));
put_dof_rot(DOF_ORDER, root_orientation, d, 0);
os << " " << d[0] << " " << d[1] << " " << d[2];
//os << " 0 0 0";
/*Character::Angles angles;
Library::Skeleton trans;
to_euler_angles(pose, angles, trans);*/
//cout << "Root xyz " << d[0] << " " << d[1] << " " << d[2] << endl;
//os << " 0 0 0";
//cout << "Frame start: " << endl;
for (unsigned int i = 0; i < order.size(); i++) {
int b = order[i];
/*os << " " << angles.angles[angles.skeleton->bones[b].frame_offset + 0] << " ";
os << -angles.angles[angles.skeleton->bones[b].frame_offset + 1] << " ";
os << angles.angles[angles.skeleton->bones[b].frame_offset + 2];*/
//local, parent-relative, orientation.
Quatd orientation = quatd(pose.bone_orientations[b]);
orientation = normalize(multiply(frame_rot(), multiply(orientation, -frame_rot())));
put_dof_rot(DOF_ORDER, orientation, d, 0);
Quatd check = get_dof_rot(DOF_ORDER, d, 0);
if (length(orientation.xyzw - check.xyzw) > 0.001 && length(-orientation.xyzw - check.xyzw) > 0.001) {
cout << "Back quaternion conversion: " << orientation << " -> " << check << endl;
cout << " via " << d[0] << " " << d[1] << " " << d[2] << endl;
}
os << " " << d[0] << " " << d[1] << " " << d[2];
// GOOD - don't delete! - xyz order, plain old writing 0 1 2
//os << " " << d[0] << " " << d[1] << " " << d[2];
}
os << endl;
}
void writeVSKToASF(unsigned int motion) {
Library::Motion &m = Library::motion_nonconst(motion);
Library::Skeleton transformer;
get_euler_skeleton(transformer, *(m.skeleton));
string asffilename = m.filename.substr(0, m.filename.rfind('/', m.filename.size())) + "/participant.ASF";
ofstream outASF(asffilename.c_str());
outASF << ":version 1.10" << endl;
outASF << ":name VICON" << endl;
outASF << ":units" << endl;
outASF << " mass 1.0" << endl;
outASF << " length .045" << endl;
outASF << " angle deg" << endl;
outASF << ":documentation" << endl;
outASF << " Carnegie Mellon University Motion Capture Lab" << endl;
outASF << " Written by WriteAsfAmc.cpp" << endl;
outASF << ":root" << endl;
outASF << " order TX TY TZ RX RY RZ" << endl;
outASF << " axis XYZ" << endl;
outASF << " position 0 0 0" << endl;
outASF << " orientation 0 0 0" << endl;
outASF << ":bonedata" << endl;
// write out the BONE DATA
for (unsigned int b = 0; b < transformer.bones.size(); b++) {
outASF << " begin" << endl;
outASF << " id " << b << endl;
outASF << " name " << transformer.bones[b].name << endl;
outASF << " direction "
<< transformer.bones[b].direction.x << " "
<< transformer.bones[b].direction.y << " "
<< transformer.bones[b].direction.z << endl;
outASF << " length " << transformer.bones[b].length << endl;
double d[3];
put_dof_rot(transformer.bones[b].offset_order, transformer.bones[b].global_to_local, d, 0);
outASF << " axis " << d[0] << " " << d[1] << " " << d[2] << " ";
for (unsigned int i = 0; i < transformer.bones[b].offset_order.size(); ++i)
outASF << (char)toupper(transformer.bones[b].offset_order[i]);
outASF << endl;
outASF << " dof";
if (transformer.bones[b].euler_axes.size()==3)
outASF << " rx ry rz";
else if (transformer.bones[b].euler_axes.size()==2)
outASF << " rx rz";
else if (transformer.bones[b].euler_axes.size()==1)
outASF << " rx";
//for (unsigned int i = 0; i < transformer.bones[b].dof.size(); ++i) {
// outASF << " r" << transformer.bones[b].dof[i];
//}
outASF << endl;
//outASF << " limits " << endl;
outASF << " end" << endl;
}
// write out the HIERARCHY
outASF << ":hierarchy" << endl;
outASF << " begin" << endl;
outASF << " root ";
for (unsigned int b = 0; b < m.skeleton->bones.size(); b++) {
if (m.skeleton->bones[b].parent==-1)
outASF << m.skeleton->bones[b].name << " ";
}
outASF << endl;
// loop through all bones...
for (unsigned int b = 0; b < m.skeleton->bones.size(); ++b) {
int id = b;
// ... if they have any children ...
bool foundchildren = false;
for (unsigned int bb = 0; bb < m.skeleton->bones.size(); ++bb)
if (m.skeleton->bones[bb].parent==id)
foundchildren = true;
if (! foundchildren )
continue;
// ... then write the children out
outASF << " " << m.skeleton->bones[b].name;
for (unsigned int bb = 0; bb < m.skeleton->bones.size(); ++bb)
if (m.skeleton->bones[bb].parent==id)
outASF << " " << m.skeleton->bones[bb].name;
outASF << endl;
}
outASF << " end" << endl;
cout << "Wrote ASF: " << asffilename << endl;
}