diff options
Diffstat (limited to 'intern/iksolver')
-rw-r--r-- | intern/iksolver/extern/IK_solver.h | 29 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QJacobian.cpp | 9 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QSegment.cpp | 260 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QSegment.h | 19 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QTask.cpp | 3 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_Solver.cpp | 109 |
6 files changed, 281 insertions, 148 deletions
diff --git a/intern/iksolver/extern/IK_solver.h b/intern/iksolver/extern/IK_solver.h index 78633fb0d9a..8626ca22beb 100644 --- a/intern/iksolver/extern/IK_solver.h +++ b/intern/iksolver/extern/IK_solver.h @@ -84,14 +84,13 @@ extern "C" { * - free all segments */ - /** * IK_Segment defines a single segment of an IK tree. * - Individual segments are always defined in local coordinates. * - The segment is assumed to be oriented in the local - * y-direction. + * y-direction. * - start is the start of the segment relative to the end - * of the parent segment. + * of the parent segment. * - rest_basis is a column major matrix defineding the rest * position (w.r.t. which the limits are defined), must * be a pure rotation @@ -100,15 +99,12 @@ extern "C" { * - length is the length of the bone. * * - basis_change and translation_change respectively define - * the change in rotation or translation for rotational joints - * and translational joints. basis_change is a column major 3x3 - * matrix. + * the change in rotation or translation. basis_change is a + * column major 3x3 matrix. * - * For rotational joints the local transformation is then defined as: - * start*rest_basis*basis*basis_change*translate(0,length,0) + * The local transformation is then defined as: + * start * rest_basis * basis * basis_change * translation_change * translate(0,length,0) * - * For translational joints: - * start*rest_basis*basis*translation_change*translate(0,length,0) */ typedef void IK_Segment; @@ -117,13 +113,18 @@ enum IK_SegmentFlag { IK_XDOF = 1, IK_YDOF = 2, IK_ZDOF = 4, - IK_TRANSLATIONAL = 8 + IK_TRANS_XDOF = 8, + IK_TRANS_YDOF = 16, + IK_TRANS_ZDOF = 32 }; typedef enum IK_SegmentAxis { - IK_X, - IK_Y, - IK_Z + IK_X = 0, + IK_Y = 1, + IK_Z = 2, + IK_TRANS_X = 3, + IK_TRANS_Y = 4, + IK_TRANS_Z = 5 } IK_SegmentAxis; extern IK_Segment *IK_CreateSegment(int flag); diff --git a/intern/iksolver/intern/IK_QJacobian.cpp b/intern/iksolver/intern/IK_QJacobian.cpp index 3eb79c89e4d..03c5f9500be 100644 --- a/intern/iksolver/intern/IK_QJacobian.cpp +++ b/intern/iksolver/intern/IK_QJacobian.cpp @@ -296,11 +296,16 @@ void IK_QJacobian::InvertSDLS() MT_Scalar damp = (gamma < max_dtheta)? gamma/max_dtheta: 1.0; - for (j = 0; j < m_d_theta.size(); j++) + for (j = 0; j < m_d_theta.size(); j++) { // slight hack: we do 0.80*, so that if there is some oscillation, // the system can still converge (for joint limits). also, it's // better to go a little to slow than to far - m_d_theta[j] += 0.80*damp*m_d_theta_tmp[j]; + + MT_Scalar dofdamp = damp/m_weight[j]; + if (dofdamp > 1.0) dofdamp = 1.0; + + m_d_theta[j] += 0.80*dofdamp*m_d_theta_tmp[j]; + } if (damp < m_min_damp) m_min_damp = damp; diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp index 310f3708499..cf9e1615d8c 100644 --- a/intern/iksolver/intern/IK_QSegment.cpp +++ b/intern/iksolver/intern/IK_QSegment.cpp @@ -158,11 +158,69 @@ static MT_Vector3 MatrixToAxisAngle(const MT_Matrix3x3& R) return delta; } +static bool EllipseClamp(MT_Scalar& ax, MT_Scalar& az, MT_Scalar *amin, MT_Scalar *amax) +{ + MT_Scalar xlim, zlim, x, z; + + if (ax < 0.0) { + x = -ax; + xlim = -amin[0]; + } + else { + x = ax; + xlim = amax[0]; + } + + if (az < 0.0) { + z = -az; + zlim = -amin[1]; + } + else { + z = az; + zlim = amax[1]; + } + + if (MT_fuzzyZero(xlim) || MT_fuzzyZero(zlim)) { + if (x <= xlim && z <= zlim) + return false; + + if (x > xlim) + x = xlim; + if (z > zlim) + z = zlim; + } + else { + MT_Scalar invx = 1.0/(xlim*xlim); + MT_Scalar invz = 1.0/(zlim*zlim); + + if ((x*x*invx + z*z*invz) <= 1.0) + return false; + + if (MT_fuzzyZero(x)) { + x = 0.0; + z = zlim; + } + else { + MT_Scalar rico = z/x; + MT_Scalar old_x = x; + x = sqrt(1.0/(invx + invz*rico*rico)); + if (old_x < 0.0) + x = -x; + z = rico*x; + } + } + + ax = (ax < 0.0)? -x: x; + az = (az < 0.0)? -z: z; + + return true; +} + // IK_QSegment IK_QSegment::IK_QSegment(int num_DoF, bool translational) -: m_parent(NULL), m_child(NULL), m_sibling(NULL), m_num_DoF(num_DoF), - m_translational(translational) +: m_parent(NULL), m_child(NULL), m_sibling(NULL), m_composite(NULL), + m_num_DoF(num_DoF), m_translational(translational) { m_locked[0] = m_locked[1] = m_locked[2] = false; m_weight[0] = m_weight[1] = m_weight[2] = 1.0; @@ -232,6 +290,11 @@ void IK_QSegment::SetParent(IK_QSegment *parent) m_parent = parent; } +void IK_QSegment::SetComposite(IK_QSegment *seg) +{ + m_composite = seg; +} + void IK_QSegment::RemoveChild(IK_QSegment *child) { if (m_child == NULL) @@ -297,19 +360,15 @@ void IK_QSphericalSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax) lmin = sin(MT_radians(lmin)*0.5); lmax = sin(MT_radians(lmax)*0.5); - // put center of ellispe in the middle between min and max - MT_Scalar offset = 0.5*(lmin + lmax); - lmax = lmax - offset; - if (axis == 0) { + m_min[0] = -lmax; + m_max[0] = -lmin; m_limit_x = true; - m_offset_x = offset; - m_max_x = lmax; } else if (axis == 2) { + m_min[1] = -lmax; + m_max[1] = -lmin; m_limit_z = true; - m_offset_z = offset; - m_max_z = lmax; } } } @@ -391,62 +450,28 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& } if (m_limit_x && m_limit_z) { - /* check in ellipsoid region */ - ax = a.x() + m_offset_x; - az = a.z() + m_offset_z; - - MT_Scalar invX = 1.0/(m_max_x*m_max_x); - MT_Scalar invZ = 1.0/(m_max_z*m_max_z); - - if ((ax*ax*invX + az*az*invZ) > 1.0) { + if (EllipseClamp(ax, az, m_min, m_max)) clamp[0] = clamp[2] = true; - - if (MT_fuzzyZero(ax)) { - ax = 0.0; - az = (az > 0)? m_max_z: -m_max_z; - } - else { - MT_Scalar rico = az/ax; - MT_Scalar old_ax = ax; - ax = sqrt(1.0/(invX + invZ*rico*rico)); - if (old_ax < 0.0) - ax = -ax; - az = rico*ax; - } - } - - ax = ax - m_offset_x; - az = az - m_offset_z; } else if (m_limit_x) { - ax = a.x() + m_offset_x; - - if (ax < -m_max_x) { - ax = -m_max_x; + if (ax < m_min[0]) { + ax = m_min[0]; clamp[0] = true; } - else if (ax > m_max_x) { - ax = m_max_x; + else if (ax > m_max[0]) { + ax = m_max[0]; clamp[0] = true; } - - ax = ax - m_offset_x; - az = a.z(); } else if (m_limit_z) { - az = a.z() + m_offset_z; - - if (az < -m_max_z) { - az = -m_max_z; + if (az < m_min[1]) { + az = m_min[1]; clamp[2] = true; } - else if (az > m_max_z) { - az = m_max_z; + else if (az > m_max[1]) { + az = m_max[1]; clamp[2] = true; } - - ax = a.x(); - az = az - m_offset_z; } if (clamp[0] == false && clamp[1] == false && clamp[2] == false) { @@ -645,62 +670,31 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& del clamp[0] = clamp[1] = false; if (m_limit_x && m_limit_z) { - /* check in ellipsoid region */ - ax = a.x() + m_offset_x; - az = a.z() + m_offset_z; - - MT_Scalar invX = 1.0/(m_max_x*m_max_x); - MT_Scalar invZ = 1.0/(m_max_z*m_max_z); + ax = a.x(); + az = a.z(); - if ((ax*ax*invX + az*az*invZ) > 1.0) { + if (EllipseClamp(ax, az, m_min, m_max)) clamp[0] = clamp[1] = true; - - if (MT_fuzzyZero(ax)) { - ax = 0.0; - az = (az > 0)? m_max_z: -m_max_z; - } - else { - MT_Scalar rico = az/ax; - MT_Scalar old_ax = ax; - ax = sqrt(1.0/(invX + invZ*rico*rico)); - if (old_ax < 0.0) - ax = -ax; - az = rico*ax; - } - } - - ax = ax - m_offset_x; - az = az - m_offset_z; } else if (m_limit_x) { - ax = a.x() + m_offset_x; - - if (ax < -m_max_x) { - ax = -m_max_x; + if (ax < m_min[0]) { + ax = m_min[0]; clamp[0] = true; } - else if (ax > m_max_x) { - ax = m_max_x; + else if (ax > m_max[0]) { + ax = m_max[0]; clamp[0] = true; } - - ax = ax - m_offset_x; - az = a.z(); } else if (m_limit_z) { - az = a.z() + m_offset_z; - - if (az < -m_max_z) { - az = -m_max_z; + if (az < m_min[1]) { + az = m_min[1]; clamp[1] = true; } - else if (az > m_max_z) { - az = m_max_z; + else if (az > m_max[1]) { + az = m_max[1]; clamp[1] = true; } - - ax = a.x(); - az = az - m_offset_z; } if (clamp[0] == false && clamp[1] == false) @@ -740,14 +734,20 @@ void IK_QSwingSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax) // put center of ellispe in the middle between min and max MT_Scalar offset = 0.5*(lmin + lmax); - lmax = lmax - offset; + //lmax = lmax - offset; if (axis == 0) { + m_min[0] = -lmax; + m_max[0] = -lmin; + m_limit_x = true; m_offset_x = offset; m_max_x = lmax; } else if (axis == 2) { + m_min[1] = -lmax; + m_max[1] = -lmin; + m_limit_z = true; m_offset_z = offset; m_max_z = lmax; @@ -907,6 +907,8 @@ IK_QTranslateSegment::IK_QTranslateSegment(int axis1) m_axis_enabled[axis1] = true; m_axis[0] = axis1; + + m_limit[0] = m_limit[1] = m_limit[2] = false; } IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2) @@ -918,6 +920,8 @@ IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2) m_axis[0] = axis1; m_axis[1] = axis2; + + m_limit[0] = m_limit[1] = m_limit[2] = false; } IK_QTranslateSegment::IK_QTranslateSegment() @@ -928,6 +932,8 @@ IK_QTranslateSegment::IK_QTranslateSegment() m_axis[0] = 0; m_axis[1] = 1; m_axis[2] = 2; + + m_limit[0] = m_limit[1] = m_limit[2] = false; } MT_Vector3 IK_QTranslateSegment::Axis(int dof) const @@ -935,18 +941,42 @@ MT_Vector3 IK_QTranslateSegment::Axis(int dof) const return m_global_transform.getBasis().getColumn(m_axis[dof]); } -bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&, bool*) +bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp) { - int dof_id = m_DoF_id; + int dof_id = m_DoF_id, dof = 0, i, clamped = false; - MT_Vector3 dx; - dx.x() = (m_axis_enabled[0])? jacobian.AngleUpdate(dof_id++): 0.0; - dx.y() = (m_axis_enabled[1])? jacobian.AngleUpdate(dof_id++): 0.0; - dx.z() = (m_axis_enabled[2])? jacobian.AngleUpdate(dof_id++): 0.0; + MT_Vector3 dx(0.0, 0.0, 0.0); - m_new_translation = m_translation + dx; + for (i = 0; i < 3; i++) { + if (!m_axis_enabled[i]) { + m_new_translation[i] = m_translation[i]; + continue; + } - return false; + clamp[dof] = false; + + if (!m_locked[dof]) { + m_new_translation[i] = m_translation[i] + jacobian.AngleUpdate(dof_id); + + if (m_limit[i]) { + if (m_new_translation[i] > m_max[i]) { + delta[dof] = m_max[i] - m_translation[i]; + m_new_translation[i] = m_max[i]; + clamped = clamp[dof] = true; + } + else if (m_new_translation[i] < m_min[i]) { + delta[dof] = m_min[i] - m_translation[i]; + m_new_translation[i] = m_min[i]; + clamped = clamp[dof] = true; + } + } + } + + dof_id++; + dof++; + } + + return clamped; } void IK_QTranslateSegment::UpdateAngleApply() @@ -954,8 +984,28 @@ void IK_QTranslateSegment::UpdateAngleApply() m_translation = m_new_translation; } +void IK_QTranslateSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta) +{ + m_locked[dof] = true; + jacobian.Lock(m_DoF_id+dof, delta[dof]); +} + void IK_QTranslateSegment::SetWeight(int axis, MT_Scalar weight) { - m_weight[axis] = weight; + int i; + + for (i = 0; i < m_num_DoF; i++) + if (m_axis[i] == axis) + m_weight[i] = weight; +} + +void IK_QTranslateSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax) +{ + if (lmax < lmin) + return; + + m_min[axis]= lmin; + m_max[axis]= lmax; + m_limit[axis]= true; } diff --git a/intern/iksolver/intern/IK_QSegment.h b/intern/iksolver/intern/IK_QSegment.h index d6082e205ae..e406585bc8b 100644 --- a/intern/iksolver/intern/IK_QSegment.h +++ b/intern/iksolver/intern/IK_QSegment.h @@ -91,6 +91,12 @@ public: IK_QSegment *Parent() const { return m_parent; } + // for combining two joints into one from the interface + void SetComposite(IK_QSegment *seg); + + IK_QSegment *Composite() const + { return m_composite; } + // number of degrees of freedom int NumberOfDoF() const { return m_num_DoF; } @@ -171,6 +177,7 @@ protected: IK_QSegment *m_parent; IK_QSegment *m_child; IK_QSegment *m_sibling; + IK_QSegment *m_composite; // full transform = // start * rest_basis * basis * translation @@ -217,6 +224,7 @@ public: private: MT_Matrix3x3 m_new_basis; bool m_limit_x, m_limit_y, m_limit_z; + MT_Scalar m_min[2], m_max[2]; MT_Scalar m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z; MT_Scalar m_locked_ax, m_locked_ay, m_locked_az; }; @@ -252,7 +260,7 @@ public: private: int m_axis; MT_Scalar m_angle, m_new_angle; - MT_Scalar m_limit; + bool m_limit; MT_Scalar m_min, m_max; }; @@ -275,6 +283,7 @@ public: private: MT_Matrix3x3 m_new_basis; bool m_limit_x, m_limit_z; + MT_Scalar m_min[2], m_max[2]; MT_Scalar m_max_x, m_max_z, m_offset_x, m_offset_z; }; @@ -308,7 +317,7 @@ private: class IK_QTranslateSegment : public IK_QSegment { public: - // Revolute, 2DOF or 3DOF translational segments + // 1DOF, 2DOF or 3DOF translational segments IK_QTranslateSegment(int axis1); IK_QTranslateSegment(int axis1, int axis2); IK_QTranslateSegment(); @@ -316,15 +325,17 @@ public: MT_Vector3 Axis(int dof) const; bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp); - void Lock(int, IK_QJacobian&, MT_Vector3&) {}; + void Lock(int, IK_QJacobian&, MT_Vector3&); void UpdateAngleApply(); void SetWeight(int axis, MT_Scalar weight); + void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax); private: int m_axis[3]; - bool m_axis_enabled[3]; + bool m_axis_enabled[3], m_limit[3]; MT_Vector3 m_new_translation; + MT_Scalar m_min[3], m_max[3]; }; #endif diff --git a/intern/iksolver/intern/IK_QTask.cpp b/intern/iksolver/intern/IK_QTask.cpp index b0e51aec371..c72f146b36e 100644 --- a/intern/iksolver/intern/IK_QTask.cpp +++ b/intern/iksolver/intern/IK_QTask.cpp @@ -82,7 +82,6 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian) jacobian.SetBetas(m_id, m_size, m_weight*d_pos); - // compute derivatives int i; const IK_QSegment *seg; @@ -92,7 +91,7 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian) for (i = 0; i < seg->NumberOfDoF(); i++) { MT_Vector3 axis = seg->Axis(i)*m_weight; - + if (seg->Translational()) jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis); else { diff --git a/intern/iksolver/intern/IK_Solver.cpp b/intern/iksolver/intern/IK_Solver.cpp index 7e9d00f8655..23960a9550b 100644 --- a/intern/iksolver/intern/IK_Solver.cpp +++ b/intern/iksolver/intern/IK_Solver.cpp @@ -45,7 +45,7 @@ typedef struct { std::list<IK_QTask*> tasks; } IK_QSolver; -IK_Segment *IK_CreateSegment(int flag) +IK_QSegment *CreateSegment(int flag, bool translate) { int ndof = 0; ndof += (flag & IK_XDOF)? 1: 0; @@ -55,7 +55,7 @@ IK_Segment *IK_CreateSegment(int flag) IK_QSegment *seg; if (ndof == 0) - seg = new IK_QNullSegment(); + return NULL; else if (ndof == 1) { int axis; @@ -63,7 +63,7 @@ IK_Segment *IK_CreateSegment(int flag) else if (flag & IK_YDOF) axis = 1; else axis = 2; - if (flag & IK_TRANSLATIONAL) + if (translate) seg = new IK_QTranslateSegment(axis); else seg = new IK_QRevoluteSegment(axis); @@ -80,7 +80,7 @@ IK_Segment *IK_CreateSegment(int flag) axis2 = 2; } - if (flag & IK_TRANSLATIONAL) + if (translate) seg = new IK_QTranslateSegment(axis1, axis2); else { if (axis1 + axis2 == 2) @@ -90,27 +90,58 @@ IK_Segment *IK_CreateSegment(int flag) } } else { - if (flag & IK_TRANSLATIONAL) + if (translate) seg = new IK_QTranslateSegment(); else seg = new IK_QSphericalSegment(); } - return (IK_Segment*)seg; + return seg; +} + +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_FreeSegment(IK_Segment *seg) { IK_QSegment *qseg = (IK_QSegment*)seg; + if (qseg->Composite()) + delete qseg->Composite(); delete qseg; } void IK_SetParent(IK_Segment *seg, IK_Segment *parent) { IK_QSegment *qseg = (IK_QSegment*)seg; + IK_QSegment *qparent = (IK_QSegment*)parent; - qseg->SetParent((IK_QSegment*)parent); + if (qparent && qparent->Composite()) + qseg->SetParent(qparent->Composite()); + else + qseg->SetParent(qparent); } void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length) @@ -127,19 +158,35 @@ void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float bas rest[0][2], rest[1][2], rest[2][2]); MT_Scalar mlength(length); - qseg->SetTransform(mstart, mrest, mbasis, mlength); + 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_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax) { IK_QSegment *qseg = (IK_QSegment*)seg; - if (axis == IK_X) - qseg->SetLimit(0, lmin, lmax); - else if (axis == IK_Y) - qseg->SetLimit(1, lmin, lmax); - else if (axis == IK_Z) - qseg->SetLimit(2, lmin, lmax); + 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); } void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness) @@ -153,12 +200,20 @@ void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness) IK_QSegment *qseg = (IK_QSegment*)seg; MT_Scalar weight = 1.0-stiffness; - if (axis == IK_X) - qseg->SetWeight(0, weight); - else if (axis == IK_Y) - qseg->SetWeight(1, weight); - else if (axis == IK_Z) - qseg->SetWeight(2, weight); + + 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_GetBasisChange(IK_Segment *seg, float basis_change[][3]) @@ -166,6 +221,9 @@ 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]; @@ -181,6 +239,10 @@ void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3]) void IK_GetTranslationChange(IK_Segment *seg, float *translation_change) { IK_QSegment *qseg = (IK_QSegment*)seg; + + if (!qseg->Translational() && qseg->Composite()) + qseg = qseg->Composite(); + const MT_Vector3& change = qseg->TranslationChange(); translation_change[0] = (float)change[0]; @@ -222,9 +284,11 @@ void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float w IK_QSolver *qsolver = (IK_QSolver*)solver; IK_QSegment *qtip = (IK_QSegment*)tip; + if (qtip->Composite()) + qtip = qtip->Composite(); + MT_Vector3 pos(goal); - // qsolver->tasks.empty() IK_QTask *ee = new IK_QPositionTask(true, qtip, pos); ee->SetWeight(weight); qsolver->tasks.push_back(ee); @@ -238,6 +302,9 @@ void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[ 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], |