Пример #1
0
static void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3], float weight)
{
	if (solver == NULL || root == NULL)
		return;

	IK_QSolver *qsolver = (IK_QSolver *)solver;
	IK_QSegment *qroot = (IK_QSegment *)root;

	// convert from blender column major
	Vector3d center(goal);

	IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
	com->SetWeight(weight);
	qsolver->tasks.push_back(com);
}
Пример #2
0
void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
{
	if (solver == NULL || tip == NULL)
		return;

	IK_QSolver *qsolver = (IK_QSolver*)solver;
	IK_QSegment *qtip = (IK_QSegment*)tip;

	if (qtip->Composite())
		qtip = qtip->Composite();

	MT_Vector3 pos(goal);

	IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
	ee->SetWeight(weight);
	qsolver->tasks.push_back(ee);
}
Пример #3
0
void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
{
	if (solver == NULL || tip == NULL)
		return;

	IK_QSolver *qsolver = (IK_QSolver *)solver;
	IK_QSegment *qtip = (IK_QSegment *)tip;

	// in case of composite segment the second segment is the tip
	if (qtip->Composite())
		qtip = qtip->Composite();

	Vector3d pos(goal[0], goal[1], goal[2]);

	IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
	ee->SetWeight(weight);
	qsolver->tasks.push_back(ee);
}
Пример #4
0
void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
{
	if (solver == NULL || tip == NULL)
		return;

	IK_QSolver *qsolver = (IK_QSolver*)solver;
	IK_QSegment *qtip = (IK_QSegment*)tip;

	if (qtip->Composite())
		qtip = qtip->Composite();

	// convert from blender column major to moto row major
	MT_Matrix3x3 rot(goal[0][0], goal[1][0], goal[2][0],
	                 goal[0][1], goal[1][1], goal[2][1],
	                 goal[0][2], goal[1][2], goal[2][2]);

	IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
	orient->SetWeight(weight);
	qsolver->tasks.push_back(orient);
}
Пример #5
0
void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
{
	if (solver == NULL || tip == NULL)
		return;

	IK_QSolver *qsolver = (IK_QSolver *)solver;
	IK_QSegment *qtip = (IK_QSegment *)tip;

	// in case of composite segment the second segment is the tip
	if (qtip->Composite())
		qtip = qtip->Composite();

	// convert from blender column major
	Matrix3d rot = CreateMatrix(goal[0][0], goal[1][0], goal[2][0],
	                            goal[0][1], goal[1][1], goal[2][1],
	                            goal[0][2], goal[1][2], goal[2][2]);

	IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
	orient->SetWeight(weight);
	qsolver->tasks.push_back(orient);
}
Пример #6
0
bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *>& tasks)
{
	m_segments.clear();
	AddSegmentList(root);

	// assign each segment a unique id for the jacobian
	std::vector<IK_QSegment *>::iterator seg;
	int num_dof = 0;

	for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
		(*seg)->SetDoFId(num_dof);
		num_dof += (*seg)->NumberOfDoF();
	}

	if (num_dof == 0)
		return false;

	// compute task id's and assing weights to task
	int primary_size = 0, primary = 0;
	int secondary_size = 0, secondary = 0;
	MT_Scalar primary_weight = 0.0, secondary_weight = 0.0;
	std::list<IK_QTask *>::iterator task;

	for (task = tasks.begin(); task != tasks.end(); task++) {
		IK_QTask *qtask = *task;

		if (qtask->Primary()) {
			qtask->SetId(primary_size);
			primary_size += qtask->Size();
			primary_weight += qtask->Weight();
			primary++;
		}
		else {
			qtask->SetId(secondary_size);
			secondary_size += qtask->Size();
			secondary_weight += qtask->Weight();
			secondary++;
		}
	}

	if (primary_size == 0 || MT_fuzzyZero(primary_weight))
		return false;

	m_secondary_enabled = (secondary > 0);
	
	// rescale weights of tasks to sum up to 1
	MT_Scalar primary_rescale = 1.0 / primary_weight;
	MT_Scalar secondary_rescale;
	if (MT_fuzzyZero(secondary_weight))
		secondary_rescale = 0.0;
	else
		secondary_rescale = 1.0 / secondary_weight;
	
	for (task = tasks.begin(); task != tasks.end(); task++) {
		IK_QTask *qtask = *task;

		if (qtask->Primary())
			qtask->SetWeight(qtask->Weight() * primary_rescale);
		else
			qtask->SetWeight(qtask->Weight() * secondary_rescale);
	}

	// set matrix sizes
	m_jacobian.ArmMatrices(num_dof, primary_size);
	if (secondary > 0)
		m_jacobian_sub.ArmMatrices(num_dof, secondary_size);

	// set dof weights
	int i;

	for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
		for (i = 0; i < (*seg)->NumberOfDoF(); i++)
			m_jacobian.SetDoFWeight((*seg)->DoFId() + i, (*seg)->Weight(i));

	return true;
}