//Set the aspect ratio of each bone
void CMUSkeleton::set_bone_shape(Bone *bone)
{
int root = CMUSkeleton::getRootIndex();
bone[root].aspx=1;
bone[root].aspy=1;
printf("READ %d\n",numBonesInSkel(bone[0]));
printf("MOV %d\n",movBonesInSkel(bone[0]));
int numbones = numBonesInSkel(bone[0]);
for(int j=1;j<numbones;j++)
{
bone[j].aspx=0.25;
bone[j].aspy=0.25;
}
}
int CMUSkeleton::numBonesInSkel(Bone bone)
{
Bone * tmp = bone.sibling;
int numBones = 0;
while (tmp != NULL)
{
if (tmp->child != NULL)
numBones += numBonesInSkel(*(tmp->child));
numBones++;
tmp = tmp->sibling;
}
if (bone.child != NULL)
return numBones + 1 + numBonesInSkel(*bone.child);
else
return numBones + 1;
}
// loop through all bones to calculate local coordinate's direction vector and relative orientation
void Skeleton::ComputeRotationToParentCoordSystem(Bone *bone)
{
//Compute tmp2, a matrix containing root
//joint local coordinate system orientation
int root = Skeleton::getRootIndex();
MATRIX4 Rz = MATRIX4::RotationZ(bone[root].axis_z * M_PI / 180.0);
MATRIX4 Ry = MATRIX4::RotationY(bone[root].axis_y * M_PI / 180.0);
MATRIX4 Rx = MATRIX4::RotationX(bone[root].axis_x * M_PI / 180.0);
MATRIX4 tmp2 = Rz * Ry * Rx;
tmp2 = tmp2.transpose();
for (int x = 0; x < 4; x++)
for (int y = 0; y < 4; y++)
bone[root].rot_parent_current[x][y] = tmp2[x][y];
//Compute rot_parent_current for all other bones
int numbones = numBonesInSkel(bone[0]);
for(int i=0; i<numbones; i++)
{
if(bone[i].child != NULL)
{
compute_rotation_parent_child(&bone[i], bone[i].child);
// compute parent child siblings...
Bone * tmp = NULL;
if (bone[i].child != NULL) tmp = (bone[i].child)->sibling;
while (tmp != NULL)
{
compute_rotation_parent_child(&bone[i], tmp);
tmp = tmp->sibling;
}
}
}
}
// loop through all bones to calculate local coordinate's direction vector and relative orientation
void Skeleton::ComputeRotationToParentCoordSystem(Bone *bone)
{
int i;
double Rx[4][4], Ry[4][4], Rz[4][4], tmp[4][4], tmp2[4][4];
//Compute rot_parent_current for the root
//Compute tmp2, a matrix containing root
//joint local coordinate system orientation
int root = Skeleton::getRootIndex();
rotationZ(Rz, bone[root].axis_z);
rotationY(Ry, bone[root].axis_y);
rotationX(Rx, bone[root].axis_x);
matrix_mult(Rz, Ry, tmp);
matrix_mult(tmp, Rx, tmp2);
//set bone[root].rot_parent_current to transpose of tmp2
matrix_transpose(tmp2, bone[root].rot_parent_current);
//Compute rot_parent_current for all other bones
int numbones = numBonesInSkel(bone[0]);
for(i=0; i<numbones; i++)
{
if(bone[i].child != NULL)
{
compute_rotation_parent_child(&bone[i], bone[i].child);
// compute parent child siblings...
Bone * tmp = NULL;
if (bone[i].child != NULL) tmp = (bone[i].child)->sibling;
while (tmp != NULL)
{
compute_rotation_parent_child(&bone[i], tmp);
tmp = tmp->sibling;
}
}
}
}