/*
Transform the direction vector (dir),
which is defined in character's global coordinate system in the ASF file,
to local coordinate
*/
void Skeleton::RotateBoneDirToLocalCoordSystem()
{
int i;
for(i=1; i<NUM_BONES_IN_ASF_FILE; i++)
{
//Transform dir vector into local coordinate system
vector_rotationXYZ(&m_pBoneList[i].dir[0], -m_pBoneList[i].axis_x, -m_pBoneList[i].axis_y, -m_pBoneList[i].axis_z);
}
}
//Computes the angular momentum about the general center of mass
void Computation::computeAngularMomentum() {
double translation[3], rotation[3];
double R[4][4],Rx[4][4],Ry[4][4],Rz[4][4];
double mean_w[3];
Bone * root;
Mass * mass;
if(m_pSkeletonList != NULL)
{
for (int i = 0; i < numOfSkeletons && i < MAX_SKELS; i++)
{
double transform[4][4];
identity(transform);
double r_i_cm[m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE][3]; //stores previous position of local center of mass in global cs
root = m_pSkeletonList[i]->getRoot();
if(m_pMassDistributionList[i] != NULL) {
//reset angular momentum
m_pSkeletonList[i]->H[0] = 0.;
m_pSkeletonList[i]->H[1] = 0.;
m_pSkeletonList[i]->H[2] = 0.;
//store old local cm in global cs of each bone:
for( int k = 0; k < m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE; k++)
{
//store previous position of local cm in global cs in r_i_cm
r_i_cm[k][0] = m_pSkeletonList[i]->getBone(root, k)->r_i[0];
r_i_cm[k][1] = m_pSkeletonList[i]->getBone(root, k)->r_i[1];
r_i_cm[k][2] = m_pSkeletonList[i]->getBone(root, k)->r_i[2];
}
//compute current position of lcm in global cs
m_pSkeletonList[i]->GetTranslation(translation);
m_pSkeletonList[i]->GetRotationAngle(rotation);
//creating Rotation matrix for initial rotation of Skeleton
rotationX(Rx, rotation[0]);
rotationY(Ry, rotation[1]);
rotationZ(Rz, rotation[2]);
matrix4_mult(Rz, Ry, R);
matrix4_mult(R, Rx, R);
matrix4_mult(transform, R, transform);
transform[0][3] += (MOCAP_SCALE*translation [0]);
transform[1][3] += (MOCAP_SCALE*translation [1]);
transform[2][3] += (MOCAP_SCALE*translation [2]);
traverse(root, i, transform, 'h');
//TODO delete
mean_w[0] = 0;
mean_w[1] = 0;
mean_w[2] = 0;
for(int j = 0; j < m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE; j++) { //for every bone compute: (r_i - r_cm) x m_i(v_i - v_cm) + I_i*w_i
double rel_pos[3], v_i[3], v_cm[3], v_rel[3], I_G[3][3], w_i[3], local_inertia[3], cross[3], S[3][3], S_transpose[3][3];
Bone * bone;
bone = m_pSkeletonList[i]->getBone(root, j);
if(m_pMassDistributionList[i]->getMass(bone->name) != NULL) {
mass = m_pMassDistributionList[i]->getMass(bone->name);
//rel_pos = r_i - r_cm
rel_pos[0] = bone->r_i[0] - m_pSkeletonList[i]->cm[0];
rel_pos[1] = bone->r_i[1] - m_pSkeletonList[i]->cm[1];
rel_pos[2] = bone->r_i[2] - m_pSkeletonList[i]->cm[2];
//v_i = r_i - r_i_cm
v_i[0] = (bone->r_i[0] - r_i_cm[j][0]);
v_i[1] = (bone->r_i[1] - r_i_cm[j][1]);
v_i[2] = (bone->r_i[2] - r_i_cm[j][2]);
//if v_i is zero switch flag
checkLegSwing(v_i, bone, i);
// if(mass->mass != 0 && ((strcmp(mass->segName, "rfoot") == 0) || (strcmp(mass->segName, "lfoot") == 0))) printf("v_i_%s: %f %f %f\n",mass->segName, v_i[0], v_i[1], v_i[2]);
//v_cm = r_cm - r_cm_prev
v_cm[0] = (m_pSkeletonList[i]->cm[0] - m_pSkeletonList[i]->cm_prev[0]);
v_cm[1] = (m_pSkeletonList[i]->cm[1] - m_pSkeletonList[i]->cm_prev[1]);
v_cm[2] = (m_pSkeletonList[i]->cm[2] - m_pSkeletonList[i]->cm_prev[2]);
//v_rel = v_i - v_cm
v_rel[0] = v_i[0] - v_cm[0];
v_rel[1] = v_i[1] - v_cm[1];
v_rel[2] = v_i[2] - v_cm[2];
//Inertia tensor
double Ri[4][4], Ri_transpose[4][4], I_i[4][4];
//rotation from global to local
rotationX(Rx, -bone->axis_x);
rotationY(Ry, -bone->axis_y);
rotationZ(Rz, -bone->axis_z);
matrix4_mult(Rz, Ry, Ri);
matrix4_mult(Ri, Rx, Ri);
matrix4_transpose(Ri, Ri_transpose);
I_i[0][0] = mass->Ixx;
I_i[0][1] = I_i[1][0] =(-1)*mass->Ixy;
I_i[0][2] = I_i[2][0] = (-1)*mass->Ixz;
I_i[1][1] = mass->Iyy;
I_i[1][2] = I_i[2][1] = (-1)*mass->Iyz;
I_i[2][2] = mass->Izz;
I_i[0][3] = I_i[1][3] = I_i[2][3] = 0;
I_i[3][0] = I_i[3][1] = I_i[3][2] = 0;
I_i[3][3] = 1.0;
//~I_G = R_i^T * I_i * R_i
//~I_G is inertia tensor relative to bone's cm in rotation of global cs.
matrix4_mult(Ri_transpose, I_i, I_i);
matrix4_mult(I_i, Ri, I_i);
//parallel axes theorem
// I_G = I_i + m_i * S^T(rel_pos)* S(rel_pos) with S is skew matrix for cross product
cross_matrix(rel_pos, S); //works
matrix3_transpose(S, S_transpose);//works
matrix3_mult(S_transpose, S, S); //works
matrix3_scalar_mult(S, mass->mass); //works
//copy entries into I_G
identity3(I_G);
for (int x = 0; x < 3; x++)
for (int y = 0; y < 3; y++)
I_G[x][y] = I_i[x][y] + S[x][y];
//w_i = R_i^T * ({rx,ry,rz} - {rx_prev, ry_prev, rz_prev});
w_i[0] = (bone->rx - bone->rx_prev);
w_i[1] = (bone->ry - bone->ry_prev);
w_i[2] = (bone->rz - bone->rz_prev);
//clamping
if(absolute_value(w_i[0]) > 1.5) w_i[0] = 0;
if(absolute_value(w_i[1]) > 1.5) w_i[1] = 0;
if(absolute_value(w_i[2]) > 1.5) w_i[2] = 0;
mean_w[0] += absolute_value(w_i[0]);
mean_w[1] += absolute_value(w_i[1]);
mean_w[2] += absolute_value(w_i[2]);
vector_rotationXYZ(w_i, bone->axis_x, bone->axis_y, bone->axis_z);
//local_inertia = I_i*w_i
matrix3_v3_mult(I_G,w_i, local_inertia);
//cross = (r_i - r_cm) x m_i(v_i - v_cm)
v_rel[0] *= m_pMassDistributionList[i]->getMass(bone->name)->mass;
v_rel[1] *= m_pMassDistributionList[i]->getMass(bone->name)->mass;
v_rel[2] *= m_pMassDistributionList[i]->getMass(bone->name)->mass;
v3_cross(rel_pos, v_rel, cross);
//H is sum of angular momentum of every bone
m_pSkeletonList[i]->H[0] += (cross[0] + local_inertia[0]);
m_pSkeletonList[i]->H[1] += (cross[1] + local_inertia[1]);
m_pSkeletonList[i]->H[2] += (cross[2] + local_inertia[2]);
} //if end
}//for bones end
setLegSwing(i);
mean_w[0] /= m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE;
mean_w[1] /= m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE;
mean_w[2] /= m_pSkeletonList[i]->NUM_BONES_IN_ASF_FILE;
//normalization N=M*H*V
// to make H dimensionless, divide by subject's height (in m), subject's mass (in kg) and subject's average velocity(0.012 m/frame or 1.44 m/s)
double n = m_pSkeletonList[i]->totalMass*m_pSkeletonList[i]->height*(1.44);
m_pSkeletonList[i]->H[0] /= n;
m_pSkeletonList[i]->H[1] /= n;
m_pSkeletonList[i]->H[2] /= n;
//printf("mean w values: %f %f %f\n", mean_w[0], mean_w[1], mean_w[2]);
//printf("angular momentum: %f %f %f\n", m_pSkeletonList[i]->H[0],m_pSkeletonList[i]->H[1],m_pSkeletonList[i]->H[2]);
//printf("position cm: %f %f %f\n", m_pSkeletonList[i]->cm[0], m_pSkeletonList[i]->cm[1], m_pSkeletonList[i]->cm[2]);
}//if end
}// for Skeletons end
}
}
