Ejemplo n.º 1
0
double PoseIK::energyFunc( const math::vectorN& d )
{
	target_pose->copy( source_pose );
	target_pose->addDisplacement( skeleton, d );

	//
	double dist = 0.0f;

	vector	dv, dq, dc;
	quater	q;

	JointConstraint* joint_constraint = 0;

	for( unsigned int i=0; i < NUM_JOINTS_IN_HUMAN; i++ )
	{
		Joint* joint = skeleton->getHumanJoint( i );
		if( !joint )	continue;

		bool is_constrained = pose_constraint->getConstraint( i, &joint_constraint );
		if( !is_constrained )	continue;

		unsigned int joint_index = joint->getIndex();
		unsigned int joint_dof = joint->getDOF();

		math::transq jT = target_pose->getGlobalTransform( skeleton, joint_index );
		math::transq cT = joint_constraint->getTransform();

		switch( joint_constraint->getType() ) {
			case JointConstraint::Type::POSITION :
			{
				dv = jT.translation - cT.translation;
				dist += dv % dv;
			}
			break;

			case JointConstraint::Type::ORIENTATION :
			{
				dv = difference( jT.rotation, cT.rotation );
				dist += dv % dv;
			}
			break;

			case JointConstraint::Type::TRANSQ :
			{
				dv = jT.translation - cT.translation;
				dist += dv % dv;

				dv = difference( jT.rotation, cT.rotation );
				dist += dv % dv;
			}
			break;

			default :
			{
				assert( false );
			}
			break;
		}
	}

	if( IsDampingEnabled )
	{
		unsigned int jj=0;

		for( unsigned int j=0; j < NUM_JOINTS_IN_HUMAN; j++ )
		{
			Joint* joint = skeleton->getHumanJoint( j );
			if( joint )
			{
				double c = DampingCoeff[ j+1 ];
				unsigned int joint_dof = joint->getDOF();

				switch( joint_dof ) {
				case 6 :
					{
						dist += c*d[jj]*d[jj];	jj++;
						dist += c*d[jj]*d[jj];	jj++;
						dist += c*d[jj]*d[jj];	jj++;

						dist += c*d[jj]*d[jj];	jj++;
						dist += c*d[jj]*d[jj];	jj++;
						dist += c*d[jj]*d[jj];	jj++;
					}
					break;
				case 3 :
					{
						dist += c*d[jj]*d[jj];	jj++;
						dist += c*d[jj]*d[jj];	jj++;
						dist += c*d[jj]*d[jj];	jj++;
					}
					break;
				case 1 :
					{
						dist += c*d[jj]*d[jj];	jj++;
					}
					break;
				default :
					break;
				}
			}
		}
	}

	return dist;

}
Ejemplo n.º 2
0
double PoseIK::computeJacobian()
{
	double dist = 0.0f;

	transq	t;
	vector	dv, dq, dc;
	vector	endTrans;
	quater	endRot;

	unsigned int i, j;
	vector	w1, w2, w3;

	num_dof = skeleton->calcDOF();
	num_equations = MAX( pose_constraint->getDOC(), num_dof );
	num_unknowns  = num_dof;

	J.setSize( num_equations, num_unknowns );
	b.setSize( num_equations );

	for( i=0; i < num_equations; i++ )
	{
		b[i] = 0.0;
		for( j=0; j < num_unknowns; j++ )
		{
			J[i][j] = 0.0;
		}
	}

	unsigned int ii = 0;

	JointConstraint* joint_constraint = 0;

	// 
	for( i=0; i < NUM_JOINTS_IN_HUMAN; i++ )
	{
		bool is_constrained = pose_constraint->getConstraint( i, &joint_constraint );

		if( !is_constrained )
		{
			continue;
		}

		Joint* joint = skeleton->getHumanJoint( i );
		unsigned int joint_id = joint->getIndex();
		unsigned int constraint_type = joint_constraint->getType();

		t = target_pose->getGlobalTransform( skeleton, joint_id );
		endTrans = t.translation;
		endRot   = t.rotation;

		unsigned int temp = ii;

		switch( constraint_type ) {
		case JointConstraint::Type::POSITION :
			{
				dv = joint_constraint->getTransform().translation - endTrans;
				dist += dv % dv;

				b[ii++] = dv.x();
				b[ii++] = dv.y();
				b[ii++] = dv.z();
			}
			break;
		case JointConstraint::Type::ORIENTATION :
			{
				dq = math::difference( joint_constraint->getTransform().rotation, endRot );
				dist += dq % dq;

				b[ii++] = dq.x();
				b[ii++] = dq.y();
				b[ii++] = dq.z();
			}
			break;
		case JointConstraint::Type::TRANSQ :
			{
				dv = joint_constraint->getTransform().translation - endTrans;
				dist += dv % dv;

				b[ii++] = dv.x();
				b[ii++] = dv.y();
				b[ii++] = dv.z();

				dq = math::difference( joint_constraint->getTransform().rotation, endRot );
				dist += dq % dq;

				b[ii++] = dq.x();
				b[ii++] = dq.y();
				b[ii++] = dq.z();
			}
			break;
		default :
			break;
		}

		// calc derivatives of all ancestors of this constrained joint 



		unsigned int jj = 0;

		for( j=0; j < NUM_JOINTS_IN_HUMAN; j++ )
		{
			Joint* other_joint = skeleton->getHumanJoint( j );
			if( !other_joint )
			{
				continue;
			}
			unsigned int other_id = other_joint->getIndex();
			unsigned int other_dof = other_joint->getDOF();

			if( skeleton->isAncestor( other_id, joint_id ) )
			{
				unsigned int iii = temp;

				switch( other_dof ) 
				{
					case 6 :
					{
						switch( joint_constraint->getType() ) 
						{
							case JointConstraint::Type::POSITION :
							{
								J[iii  ][jj  ] = 1;
								J[iii+1][jj+1] = 1;
								J[iii+2][jj+2] = 1;

								t  = target_pose->getGlobalTransform( skeleton, other_id );
								dv = endTrans - t.translation;

								w1 = vector(1,0,0) * t * dv;
								w2 = vector(0,1,0) * t * dv;
								w3 = vector(0,0,1) * t * dv;

								J[iii  ][jj+3] = w1[0];
								J[iii  ][jj+4] = w2[0];
								J[iii  ][jj+5] = w3[0];

								J[iii+1][jj+3] = w1[1];
								J[iii+1][jj+4] = w2[1];
								J[iii+1][jj+5] = w3[1];

								J[iii+2][jj+3] = w1[2];
								J[iii+2][jj+4] = w2[2];
								J[iii+2][jj+5] = w3[2];

								iii += 3;

								break;
							}

							case JointConstraint::Type::ORIENTATION :
							{
								J[iii  ][jj+3] = 1;
								J[iii+1][jj+4] = 1;
								J[iii+2][jj+5] = 1;

								break;
							}

							case JointConstraint::Type::TRANSQ :
							{
								J[iii  ][jj  ] = 1;
								J[iii+1][jj+1] = 1;
								J[iii+2][jj+2] = 1;

								t  = target_pose->getGlobalTransform( skeleton, other_id );
								dv = endTrans - t.translation;

								w1 = vector(1,0,0) * t * dv;
								w2 = vector(0,1,0) * t * dv;
								w3 = vector(0,0,1) * t * dv;

								J[iii  ][jj+3] = w1[0];
								J[iii  ][jj+4] = w2[0];
								J[iii  ][jj+5] = w3[0];

								J[iii+1][jj+3] = w1[1];
								J[iii+1][jj+4] = w2[1];
								J[iii+1][jj+5] = w3[1];

								J[iii+2][jj+3] = w1[2];
								J[iii+2][jj+4] = w2[2];
								J[iii+2][jj+5] = w3[2];

								iii += 3;

								J[iii  ][jj+3] = 1;
								J[iii+1][jj+4] = 1;
								J[iii+2][jj+5] = 1;

								break;
							}
							default :
								break;
						}
					}
					break;

					case 3 :
					{
						switch( joint_constraint->getType() )
						{
							case JointConstraint::Type::POSITION :
							{
								t  = target_pose->getGlobalTransform( skeleton, other_id );
								dv = endTrans - t.translation;

								w1 = vector(1,0,0) * t * dv;
								w2 = vector(0,1,0) * t * dv;
								w3 = vector(0,0,1) * t * dv;

								J[iii  ][jj  ] = w1[0];
								J[iii  ][jj+1] = w2[0];
								J[iii  ][jj+2] = w3[0];

								J[iii+1][jj  ] = w1[1];
								J[iii+1][jj+1] = w2[1];
								J[iii+1][jj+2] = w3[1];

								J[iii+2][jj  ] = w1[2];
								J[iii+2][jj+1] = w2[2];
								J[iii+2][jj+2] = w3[2];

								iii += 3;

								break;
							}

							case JointConstraint::Type::ORIENTATION :
							{
								J[iii  ][jj  ] = 1;
								J[iii+1][jj+1] = 1;
								J[iii+2][jj+2] = 1;

								break;
							}

							case JointConstraint::Type::TRANSQ :
							{
								t  = target_pose->getGlobalTransform( skeleton, other_id );
								dv = endTrans - t.translation;

								w1 = vector(1,0,0) * t * dv;
								w2 = vector(0,1,0) * t * dv;
								w3 = vector(0,0,1) * t * dv;

								J[iii  ][jj  ] = w1[0];
								J[iii  ][jj+1] = w2[0];
								J[iii  ][jj+2] = w3[0];

								J[iii+1][jj  ] = w1[1];
								J[iii+1][jj+1] = w2[1];
								J[iii+1][jj+2] = w3[1];

								J[iii+2][jj  ] = w1[2];
								J[iii+2][jj+1] = w2[2];
								J[iii+2][jj+2] = w3[2];

								iii += 3;

								J[iii  ][jj  ] = 1;
								J[iii+1][jj+1] = 1;
								J[iii+2][jj+2] = 1;

								break;
							}
							default :
								break;
						}
					}
					break;

					case 1 :
					{
						switch( joint_constraint->getType() )
						{
							case JointConstraint::Type::POSITION :
							{
								t  = target_pose->getGlobalTransform( skeleton, other_id );
								dv = vector(1,0,0) * t * ( endTrans - t.translation );

								J[iii  ][jj] = dv[0];
								J[iii+1][jj] = dv[1];
								J[iii+2][jj] = dv[2];

								iii += 3;

								break;
							}
								
							case JointConstraint::Type::ORIENTATION :
							{
								J[iii  ][jj] = 1;
								J[iii+1][jj] = 0;
								J[iii+2][jj] = 0;

								break;
							}

							case JointConstraint::Type::TRANSQ :
							{
								t  = target_pose->getGlobalTransform( skeleton, other_id );
								dv = vector(1,0,0) * t * ( endTrans - t.translation );

								J[iii  ][jj] = dv[0];
								J[iii+1][jj] = dv[1];
								J[iii+2][jj] = dv[2];

								iii += 3;

								J[iii  ][jj] = 1;
								J[iii+1][jj] = 0;
								J[iii+2][jj] = 0;

								break;
							}

							default :
								break;
						}
					}
					break;
				}
			}
			jj += other_dof;
		}
	}

	return dist;
}
Ejemplo n.º 3
0
void segmentMotion()
{
	int num_frames = motion_data.getNumFrames();
	if( num_frames > 0 )
	{
		bool* is_lf_contact = new bool[ num_frames ];
		bool* is_rf_contact = new bool[ num_frames ];

		//
		Joint* lf = motion_data.getHumanJoint( Human::LEFT_FOOT );
		Joint* rf = motion_data.getHumanJoint( Human::RIGHT_FOOT );
		Joint* lt = motion_data.getHumanJoint( Human::LEFT_TOE );
		Joint* rt = motion_data.getHumanJoint( Human::RIGHT_TOE );

		double hl = 10, sl = 0.5;
		int f;

		for( f=0; f < num_frames-1; f++ )
		{
			math::position lf_p = motion_data.getPosition( f, lf->getIndex() );
			math::position rf_p = motion_data.getPosition( f, rf->getIndex() );

			math::vector lf_v0 = motion_data.getLinearVelocity( f, lf->getIndex() );
			math::vector rf_v0 = motion_data.getLinearVelocity( f, rf->getIndex() );

			math::vector lf_v1 = motion_data.getLinearVelocity( f+1, lf->getIndex() );
			math::vector rf_v1 = motion_data.getLinearVelocity( f+1, rf->getIndex() );

			//if( lf_a.length() < sl )
			if( lf_p.y() < hl && lf_v0.y() < 0 && lf_v1.y() > 0 )
			{
				is_lf_contact[ f ] = true;
			}
			else
			{
				is_lf_contact[ f ] = false;
			}

			//if( rf_a.length() < sl )
			if( rf_p.y() < hl && rf_v0.y() < 0 && rf_v1.y() > 0 )
			{
				is_rf_contact[ f ] = true;
			}
			else
			{
				is_rf_contact[ f ] = false;
			}
		}

		//
		unsigned int prev_f = 0;
		unsigned int num_segments = 0;

		for( f=1; f < num_frames; f++ )
		{
			if( ( is_rf_contact[ f-1 ] != is_rf_contact[ f ] || is_lf_contact[ f-1 ] != is_lf_contact[ f ] )
				&& f-prev_f >= MIN_SEGMENT_LENGTH )
			{
				segment_list.push_back( new MotionSegment( prev_f, f-1 ) );
				std::cout << "segment[ " << num_segments++ << " ]: " << f-prev_f << " frames ( " << prev_f << "~" << f-1 << " )\n";

				prev_f = f;
			}
		}
	}
}