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;
}
//BVH専用
void Pose::BuildPose(double const *frame_data, Skeleton* skl)
{
//clear out the destination pose.
Clear(skl->bones.size());
//make sure we have the right amount of storage.
assert(skl->bones.size() == boneOrientations_.size());
//also, make sure the pose knows its skeleton.
this->skeleton = skl;
rootPosition_ = get_dof_trans(skl->positionEulerOrder, frame_data, 0);
rootPosition_ /= 100.0;
//rootOrientation_ = multiply(ordered_rotation( skl->offset_order, skl->axis_offset ), get_dof_rot(skl->order, frame_data, 0));
rootOrientation_ = get_dof_rot(skl->bones[0]->eulerOrder, frame_data, skl->positionEulerOrder.size());
//rootは回転0とみなす
for (unsigned int b = 1; b < skl->bones.size(); ++b) {
//親関節の回転が現在のBoneの向きとなる
Bone* parent = skl->bones[skl->bones[b]->parent];
if(parent->parent >= 0)
{//親関節がルートでない場合
klQuat rot = get_dof_rot(parent->eulerOrder, frame_data, parent->frameOffset);
boneOrientations_[b] = rot;
}
//rot = multiply(conjugate(skl->bones[b].global_to_local), rot);
//Quatd mul = get_dof_rot(skl->bones[b].dof, frame_data, skl->bones[b].frame_offset);
//rot = multiply(mul, rot);
//rot = multiply(skl->bones[b].global_to_local, rot);
//rot = normalize(rot);
}
//for (unsigned int b = 0; b < skl->bones.size(); ++b) {
// Quatd rot;
// rot.clear();
// rot = multiply(conjugate(skl->bones[b].global_to_local), rot);
// Quatd mul = get_dof_rot(skl->bones[b].dof, frame_data, skl->bones[b].frame_offset);
// rot = multiply(mul, rot);
// rot = multiply(skl->bones[b].global_to_local, rot);
// rot = normalize(rot);
// this->bone_orientations[b] = rot;
//}
//if (skl->rot_is_glob) {
// for (unsigned int b = skl->bones.size()-1; b < skl->bones.size(); --b) { //neg wraps
// if (skl->bones[b].parent==-1) {
// Quatf f;
// f = get_dof_rot(skl->order, frame_data, 0);
// this->bone_orientations[b] = multiply(conjugate(f), this->bone_orientations[b]);
// } else {
// this->bone_orientations[b] = multiply(conjugate(this->bone_orientations[skl->bones[b].parent]), this->bone_orientations[b]);
// }
// this->bone_orientations[b] = normalize(this->bone_orientations[b]);
// assert(skl->bones[b].parent < int(b));
// }
//}
//if (skl->z_is_up) {
// Quatf rot;
// rot = rotation( -(float)M_PI * 0.5f, make_vector(1.0f, 0.0f, 0.0f) );
// this->root_position = rotate(this->root_position, rot);
// this->root_orientation = multiply(rot, this->root_orientation);
//
//}
}
