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);
}
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);
}
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);
}
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);
}
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);
}
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;
}