예제 #1
0
/*
This function sets rot_parent_current data member.
Rotation from this bone local coordinate system 
to the coordinate system of its parent
*/
void Skeleton::compute_rotation_parent_child(Bone *parent, Bone *child)
{

  double Rx[4][4], Ry[4][4], Rz[4][4], tmp[4][4], tmp1[4][4], tmp2[4][4];

  if(child != NULL)
  { 

    // The following openGL rotations are pre-calculated and saved in the orientation matrix. 
    //
    // glRotatef(-inboard->axis_x, 1., 0., 0.);
    // glRotatef(-inboard->axis_y, 0., 1,  0.);
    // glRotatef(-inboard->axis_z, 0., 0., 1.);
    // glRotatef(outboard->axis_z, 0., 0., 1.);
    // glRotatef(outboard->axis_y, 0., 1,  0.);
    // glRotatef(outboard->axis_x, 1., 0., 0.);

    rotationZ(Rz, -parent->axis_z);      
    rotationY(Ry, -parent->axis_y);  
    rotationX(Rx, -parent->axis_x);      
    matrix_mult(Rx, Ry, tmp);
    matrix_mult(tmp, Rz, tmp1);

    rotationZ(Rz, child->axis_z);
    rotationY(Ry, child->axis_y);
    rotationX(Rx, child->axis_x);
    matrix_mult(Rz, Ry, tmp);
    matrix_mult(tmp, Rx, tmp2);

    matrix_mult(tmp1, tmp2, tmp);
    matrix_transpose(tmp, child->rot_parent_current);    
  }
}
예제 #2
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);
		}

	}



}
예제 #3
0
void RenderLine2D(Shader *sh, Primitive prim, const float3 &v1, const float3 &v2, float thickness)
{
    auto v = (v2 - v1) / 2;
    auto len = length(v);
    auto vnorm = v / len;
    auto trans = translation(v1 + v) * 
                 rotationZ(vnorm.xy()) *
                 float4x4(float4(len, thickness / 2, 1, 1));
    RenderQuad(sh, prim, true, trans);
}
예제 #4
0
파일: physics.cpp 프로젝트: DawidvC/lobster
	STARTDECL(ph_render) ()
	{
		CheckPhysics();
		auto oldobject2view = object2view;
		auto oldcolor = curcolor;
		for (b2Body *body = world->GetBodyList(); body; body = body->GetNext())
		{
			auto pos = body->GetPosition();
			auto mat = translation(float3(pos.x, pos.y, 0)) * rotationZ(body->GetAngle());
			object2view = oldobject2view * mat;

			for (b2Fixture *fixture = body->GetFixtureList(); fixture; fixture = fixture->GetNext())
			{
				auto shapetype = fixture->GetType();
				auto r = (Renderable *)fixture->GetUserData();
				curcolor = r->color;
				r->Set();
				switch (shapetype)
				{
					case b2Shape::e_polygon:
					{
						auto polyshape = (b2PolygonShape *)fixture->GetShape();
						RenderArray(PRIM_FAN, polyshape->m_count, "pn", sizeof(b2Vec2), polyshape->m_vertices, NULL, 
							                                              sizeof(b2Vec2), polyshape->m_normals);
						break;
					}
					case b2Shape::e_circle:
					{
						// FIXME: instead maybe cache a circle verts somewhere.. though should maxverts be changable?
						const int maxverts = 20;
						struct PhVert { float2 pos; float2 norm; } phverts[maxverts];
						auto polyshape = (b2CircleShape *)fixture->GetShape();
						float step = PI * 2 / maxverts;
						for (int i = 0; i < maxverts; i++)
						{
							auto pos = float2(sinf(i * step + 1), cosf(i * step + 1));
							phverts[i].pos = pos * polyshape->m_radius + *(float2 *)&polyshape->m_p;
							phverts[i].norm = pos;
						}
						RenderArray(PRIM_FAN, maxverts, "pn", sizeof(PhVert), phverts, NULL);
						break;
					}
					case b2Shape::e_edge:
					case b2Shape::e_chain:
          case b2Shape::e_typeCount:
						assert(0);
						break;
				}
			}
		}
		object2view = oldobject2view;
		curcolor = oldcolor;
		return Value();
	}
예제 #5
0
/*
Rotate vector v by a, b, c in X,Y,Z order.
v_out = Rz(c)*Ry(b)*Rx(a)*v_in
*/
void vector_rotationXYZ(double *v, double a, double b, double c)
{
  double Rx[4][4], Ry[4][4], Rz[4][4];

  //Rz is a rotation matrix about Z axis by angle c, same for Ry and Rx
  rotationZ(Rz, c);
  rotationY(Ry, b);
  rotationX(Rx, a);

  //Matrix vector multiplication to generate the output vector v.
  matrix_transform_affine(Rz, v[0], v[1], v[2], v);
  matrix_transform_affine(Ry, v[0], v[1], v[2], v);
  matrix_transform_affine(Rx, v[0], v[1], v[2], v);
}
예제 #6
0
void ToyRotationMatrix::rotateZ(float degree) {
  // TODO: Throw exception here
  if (Rows!=4 || Columns!=4) {
    assert(false);
  }
  DegreeZ=degree;
  float rotationElements[16] = { cos(degree), -sin(degree), 0.0, 0.0,
                                 sin(degree),  cos(degree), 0.0, 0.0,
                                         0.0,          0.0, 1.0, 0.0,
                                         0.0,          0.0, 0.0, 1.0};
  float *rotationElementsP = new float[16];
  memcpy(rotationElementsP, &rotationElements, 16*sizeof(float));
  ToyMatrix<float> rotationZ(4, 4, rotationElementsP);
  (*this) *= rotationZ;
}
예제 #7
0
파일: raytracer.cpp 프로젝트: 3dct/open_iA
void Engine::Transform( iAVec3f * vec )
{
	//!@note rotations and translations are inversed, because we rotating plane and origin instead of object
	float 
		irotX = -rotX,
		irotY = -rotY,
		irotZ = -rotZ;
	iAVec3f iposition = -position;
	iAMat4 mrotx, mroty, mrotz;
	mrotz = rotationZ(irotZ);
	mroty = rotation(mrotz*iAVec3f(0,1,0), irotY);
	mrotx = rotation(mrotz*iAVec3f(1,0,0), irotX);
	iAMat4 rot_mat = mrotx*mroty*mrotz;
	//if plate rotation is desired, should do translation after rotation
	(*vec) = rot_mat*((*vec)+iposition);
}
예제 #8
0
파일: raytracer.cpp 프로젝트: 3dct/open_iA
// -----------------------------------------------------------
// Engine::InitRender
// Initializes the renderer, by resetting the line / tile
// counters and precalculating some values
// -----------------------------------------------------------
void Engine::InitRender(iAVec3f * vp_corners, iAVec3f * vp_delta, iAVec3f * o)
{
	//!@note rotations and translations are inversed, because we rotating plane and origin instead of object
	float 
		irotX = -rotX,
		irotY = -rotY,
		irotZ = -rotZ;
	iAVec3f iposition = -position;
	iAMat4 mrotx, mroty, mrotz;
	mrotz = rotationZ(irotZ);
	mroty = rotation(mrotz*iAVec3f(0,1,0), irotY);
	mrotx = rotation(mrotz*iAVec3f(1,0,0), irotX);
	iAMat4 rot_mat = mrotx*mroty*mrotz;
	//iAMat4 rot_mat = rotationX(irotX)*rotationY(irotY)*rotationZ(irotZ);
	//if plate rotation is desired, should do translation after rotation
	vp_corners[0] = rot_mat*(iAVec3f(m_WX1, m_WY1, m_PLANE_Z)+iposition);
	vp_corners[1] = rot_mat*(iAVec3f(m_WX2, m_WY2, m_PLANE_Z)+iposition);
	(*o) = rot_mat*(iAVec3f( 0, 0, m_ORIGIN_Z )+iposition);
	//no translations here
	vp_delta[0] = rot_mat*iAVec3f(m_DX, 0, 0);
	vp_delta[1] = rot_mat*iAVec3f(0, m_DY, 0);
}
예제 #9
0
// 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;
      }
    }
  }
}
예제 #10
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);


	}

}
예제 #11
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
	}
}
Eigen::MatrixXd MainWindow::rpy2rotation( double roll, double pitch, double yaw )
{
  Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll );

  return _rotation;
}
예제 #13
0
 STARTDECL(gl_rotate_z) (Value &angle, Value &body)
 {
     auto a = transangle(angle);
     return pushtrans(rotationZ(a), rotationZ(a * float2(1, -1)), body);
 }
예제 #14
0
	//  [7/30/2008 zhangxiang]
	void sgNode::roll(const Radian &aAngle, int aRelativeTo /* = TS_LOCAL */){
		rotationZ(aAngle, aRelativeTo);
	}
예제 #15
0
파일: Matrix.cpp 프로젝트: Dehax/DWin3D
Matrix Matrix::rotation(const long double &angleX, const long double &angleY, const long double &angleZ)
{
    return rotationX(angleX) * rotationY(angleY) * rotationZ(angleZ);
}