예제 #1
0
double PoseIK::incorporateDamping( const math::vectorN& d, math::vectorN& dp )
{
	double dist = 0.0f;
	double c1, c2;

	unsigned int jj = 0;

	for( unsigned int j=0; j < NUM_JOINTS_IN_HUMAN; j++ )
	{
		Joint* joint = skeleton->getHumanJoint( j );

		if( joint )
		{
			c1 = PoseIK::DampingCoeff[ j ];
			c2 = 2.0 * c1;

			unsigned int dof = joint->getDOF();

			switch( dof ) {
			case 6 :
			{
				dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++;
				dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++;
				dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++;

				dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++;
				dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++;
				dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++;

				break;
			}
			case 3 :
			{
				dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++;
				dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++;
				dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++;

				break;
			}
			case 1 :
			{
				dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++;

				break;
			}

			default :
				break;
			}
		}
	}

	return dist;
}
예제 #2
0
unsigned int Skeleton::calcDOF()
{
	unsigned int total_dof = 0;

	std::vector< Joint* >::iterator itor_j = joint_list.begin();
	while( itor_j != joint_list.end() )
	{
		Joint* j = ( *itor_j ++ );
		total_dof += j->getDOF();
	}
	return total_dof;
}
예제 #3
0
void PoseIK::scalingCoefficients( math::vectorN& x )
{
	double c = 0.0f;

	unsigned int jj = 0;

	for( unsigned int j=0; j < NUM_JOINTS_IN_HUMAN; j++ )
	{
		Joint* joint = skeleton->getHumanJoint( j );

		if( joint )
		{
			unsigned int dof = joint->getDOF();

			switch( dof ) {
			case 6 :
			{
				c = PoseIK::RigidityCoeff[ j ];
				x[jj++] *= c;
				x[jj++] *= c;
				x[jj++] *= c;

				c = PoseIK::RigidityCoeff[ j ];
				x[jj++] *= c;
				x[jj++] *= c;
				x[jj++] *= c;

				break;
			}
			case 3 :
			{
				c = PoseIK::RigidityCoeff[ j ];
				x[jj++] *= c;
				x[jj++] *= c;
				x[jj++] *= c;

				break;
			}
			case 1 :
			{
				c = PoseIK::RigidityCoeff[ j ];
				x[jj++] *= c;

				break;
			}
			default :
				break;
			}
		}
	}
}
예제 #4
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;

}