示例#1
0
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);
		}

	}



}
示例#2
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;
}
示例#3
0
/******************************************************************************
** 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;
}
示例#4
0
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);


	}

}
示例#5
0
//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
	}
}