void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
Vector3d mstart(start[0], start[1], start[2]);
// convert from blender column major
Matrix3d mbasis = CreateMatrix(basis[0][0], basis[1][0], basis[2][0],
basis[0][1], basis[1][1], basis[2][1],
basis[0][2], basis[1][2], basis[2][2]);
Matrix3d mrest = CreateMatrix(rest[0][0], rest[1][0], rest[2][0],
rest[0][1], rest[1][1], rest[2][1],
rest[0][2], rest[1][2], rest[2][2]);
double mlength(length);
if (qseg->Composite()) {
Vector3d cstart(0, 0, 0);
Matrix3d cbasis;
cbasis.setIdentity();
qseg->SetTransform(mstart, mrest, mbasis, 0.0);
qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
}
else
qseg->SetTransform(mstart, mrest, mbasis, mlength);
}
void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
MT_Vector3 mstart(start);
// convert from blender column major to moto row major
MT_Matrix3x3 mbasis(basis[0][0], basis[1][0], basis[2][0],
basis[0][1], basis[1][1], basis[2][1],
basis[0][2], basis[1][2], basis[2][2]);
MT_Matrix3x3 mrest(rest[0][0], rest[1][0], rest[2][0],
rest[0][1], rest[1][1], rest[2][1],
rest[0][2], rest[1][2], rest[2][2]);
MT_Scalar mlength(length);
if (qseg->Composite()) {
MT_Vector3 cstart(0, 0, 0);
MT_Matrix3x3 cbasis;
cbasis.setIdentity();
qseg->SetTransform(mstart, mrest, mbasis, 0.0);
qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
}
else
qseg->SetTransform(mstart, mrest, mbasis, mlength);
}
void IK_FreeSegment(IK_Segment *seg)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
if (qseg->Composite())
delete qseg->Composite();
delete qseg;
}
void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
{
m_segments.push_back(seg);
IK_QSegment *child;
for (child = seg->Child(); child; child = child->Sibling())
AddSegmentList(child);
}
void IK_QSegment::Reset()
{
m_locked[0] = m_locked[1] = m_locked[2] = false;
m_basis = m_orig_basis;
m_translation = m_orig_translation;
SetBasis(m_basis);
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
seg->Reset();
}
void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
if (!qseg->Translational() && qseg->Composite())
qseg = qseg->Composite();
const Vector3d& change = qseg->TranslationChange();
translation_change[0] = (float)change[0];
translation_change[1] = (float)change[1];
translation_change[2] = (float)change[2];
}
void IK_QSegment::UpdateTransform(const MT_Transform& global)
{
// compute the global transform at the end of the segment
m_global_start = global.getOrigin() + global.getBasis()*m_start;
m_global_transform.setOrigin(m_global_start);
m_global_transform.setBasis(global.getBasis() * m_rest_basis * m_basis);
m_global_transform.translate(m_translation);
// update child transforms
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
seg->UpdateTransform(m_global_transform);
}
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_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle)
{
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 qgoal(goal[0], goal[1], goal[2]);
Vector3d qpolegoal(polegoal[0], polegoal[1], polegoal[2]);
qsolver->solver.SetPoleVectorConstraint(
qtip, qgoal, qpolegoal, poleangle, getangle);
}
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_GetBasisChange(IK_Segment *seg, float basis_change[][3])
{
IK_QSegment *qseg = (IK_QSegment*)seg;
const MT_Matrix3x3& change = qseg->BasisChange();
if (qseg->Translational() && qseg->Composite())
qseg = qseg->Composite();
// convert from moto row major to blender column major
basis_change[0][0] = (float)change[0][0];
basis_change[1][0] = (float)change[0][1];
basis_change[2][0] = (float)change[0][2];
basis_change[0][1] = (float)change[1][0];
basis_change[1][1] = (float)change[1][1];
basis_change[2][1] = (float)change[1][2];
basis_change[0][2] = (float)change[2][0];
basis_change[1][2] = (float)change[2][1];
basis_change[2][2] = (float)change[2][2];
}
void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
{
IK_QSegment *qseg = (IK_QSegment *)seg;
if (qseg->Translational() && qseg->Composite())
qseg = qseg->Composite();
const Matrix3d& change = qseg->BasisChange();
// convert to blender column major
basis_change[0][0] = (float)change(0, 0);
basis_change[1][0] = (float)change(0, 1);
basis_change[2][0] = (float)change(0, 2);
basis_change[0][1] = (float)change(1, 0);
basis_change[1][1] = (float)change(1, 1);
basis_change[2][1] = (float)change(1, 2);
basis_change[0][2] = (float)change(2, 0);
basis_change[1][2] = (float)change(2, 1);
basis_change[2][2] = (float)change(2, 2);
}
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);
}
void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
{
if (stiffness < 0.0f)
return;
if (stiffness > (1.0 - IK_STRETCH_STIFF_EPS))
stiffness = (1.0 - IK_STRETCH_STIFF_EPS);
IK_QSegment *qseg = (IK_QSegment *)seg;
double weight = 1.0f - stiffness;
if (axis >= IK_TRANS_X) {
if (!qseg->Translational()) {
if (qseg->Composite() && qseg->Composite()->Translational())
qseg = qseg->Composite();
else
return;
}
if (axis == IK_TRANS_X) axis = IK_X;
else if (axis == IK_TRANS_Y) axis = IK_Y;
else axis = IK_Z;
}
qseg->SetWeight(axis, weight);
}
void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
{
if (stiffness < 0.0)
return;
if (stiffness > 0.999)
stiffness = 0.999;
IK_QSegment *qseg = (IK_QSegment*)seg;
MT_Scalar weight = 1.0-stiffness;
if (axis >= IK_TRANS_X) {
if(!qseg->Translational()) {
if(qseg->Composite() && qseg->Composite()->Translational())
qseg = qseg->Composite();
else
return;
}
if(axis == IK_TRANS_X) axis = IK_X;
else if(axis == IK_TRANS_Y) axis = IK_Y;
else axis = IK_Z;
}
qseg->SetWeight(axis, weight);
}
IK_Segment *IK_CreateSegment(int flag)
{
IK_QSegment *rot = CreateSegment(flag, false);
IK_QSegment *trans = CreateSegment(flag >> 3, true);
IK_QSegment *seg;
if (rot == NULL && trans == NULL)
seg = new IK_QNullSegment();
else if (rot == NULL)
seg = trans;
else {
seg = rot;
// make it seem from the interface as if the rotation and translation
// segment are one
if (trans) {
seg->SetComposite(trans);
trans->SetParent(seg);
}
}
return seg;
}
void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
IK_QSegment *qparent = (IK_QSegment *)parent;
if (qparent && qparent->Composite())
qseg->SetParent(qparent->Composite());
else
qseg->SetParent(qparent);
}
void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
if (axis >= IK_TRANS_X) {
if (!qseg->Translational()) {
if (qseg->Composite() && qseg->Composite()->Translational())
qseg = qseg->Composite();
else
return;
}
if (axis == IK_TRANS_X) axis = IK_X;
else if (axis == IK_TRANS_Y) axis = IK_Y;
else axis = IK_Z;
}
qseg->SetLimit(axis, lmin, lmax);
}
