void Computation::computeGeneralCenterOfMass() {
double translation[3], rotation[3];
double R[4][4],Rx[4][4],Ry[4][4],Rz[4][4];
for (int i = 0; i < numOfSkeletons && i < MAX_SKELS; i++)
{
totalMass = 0.0; //reset total Mass
double transform[4][4];
identity(transform); // reset transform matrix to identity
//store previous position of gcm
m_pSkeletonList[i]->cm_prev[0] = m_pSkeletonList[i]->cm[0];
m_pSkeletonList[i]->cm_prev[1] = m_pSkeletonList[i]->cm[1];
m_pSkeletonList[i]->cm_prev[2] = m_pSkeletonList[i]->cm[2];
m_pSkeletonList[i]->cm[0] = 0.0;
m_pSkeletonList[i]->cm[1] = 0.0;
m_pSkeletonList[i]->cm[2] = 0.0;
if (m_pMassDistributionList[i] != NULL) {
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(m_pSkeletonList[i]->getRoot(), i, transform, 'g');
m_pSkeletonList[i]->cm[0] /= (totalMass);
m_pSkeletonList[i]->cm[1] /= (totalMass);
m_pSkeletonList[i]->cm[2] /= (totalMass);
m_pSkeletonList[i]->totalMass = totalMass;
} else {
printf("Entry of m_pMotionList[%d] is empty.\n", i);
//exit(0);
}
}
}
/******************************************************************************
** Name: get_r_matrix4
** Params:
** Return:
** Descriptions:
** This function rotate a matrix m in 3d
** about a arbitrary vector U(x, y, z)
*******************************************************************************/
son_bool_t matrix4_rotate(son_matrix4x4_t *m,
const son_vector3d_t *v,
double theta)
{
son_matrix4x4_t r, rm;
get_r_matrix4(v, theta, &r);
matrix4_mult(m, &r, &rm);
matrix4_cpy(&rm, m);
return son_TRUE;
}
/******************************************************************************
** Name: matrix4_scale
** Params:
** Return:
** Descriptions:
** This function scale the matrix m4x4 with s
*******************************************************************************/
son_bool_t matrix4_scale(son_matrix4x4_t *m,
double s)
{
son_matrix4x4_t sm;
son_matrix4x4_t s_matrix = {s, 0.0, 0.0, 0.0,
0.0, s, 0.0, 0.0,
0.0, 0.0, s, 0.0,
0.0, 0.0, 0.0, 1.0
};
matrix4_mult(m, &s_matrix, &sm);
matrix4_cpy(&sm, m);
return son_TRUE;
}
void Computation::traverse(Bone * ptr, int skelNum, double transform[4][4], char c){
if (ptr != NULL) {
double Rx[4][4], Ry[4][4], Rz[4][4], M[4][4]; //store rotation matrices.
double transformBackUp[4][4];
double translation[4][4];
double C[4][4], Cinv[4][4];
matrix4_copy(transform, transformBackUp);
//create homogeneous transformation to next frame.
identity(M);
identity(translation);
identity(Rx);
identity(Ry);
identity(Rz);
// compute C
rotationZ(Rz, ptr->axis_z);
rotationY(Ry, ptr->axis_y);
rotationX(Rx, ptr->axis_x);
matrix4_mult(Rz, Ry, C);
matrix4_mult(C, Rx, C);
// compute M
rotationX(Rx, (ptr->rx));
rotationY(Ry, (ptr->ry));
rotationZ(Rz, (ptr->rz));
matrix4_mult(Rz, Ry, M);
matrix4_mult(M, Rx, M);
M[0][3] += ptr->tx;
M[1][3] += ptr->ty;
M[2][3] += ptr->tz;
matrix4_mult(transform, C, transform);
matrix4_mult(transform, M, transform);
if(c == 'g') computeCM(ptr, skelNum, transform);
if(c == 'h') computePos(ptr, transform);
translation[0][3] = ptr->dir[0]*ptr->length;
translation[1][3] = ptr->dir[1]*ptr->length;
translation[2][3] = ptr->dir[2]*ptr->length;
matrix4_mult(transform, translation, transform);
matrix4_transpose(C, Cinv);
matrix4_mult(transform, Cinv, transform);
traverse(ptr->child, skelNum, transform, c);
traverse(ptr->sibling, skelNum, transformBackUp, c);
}
}
//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
}
}
