diff options
author | Campbell Barton <ideasman42@gmail.com> | 2019-04-17 07:17:24 +0300 |
---|---|---|
committer | Campbell Barton <ideasman42@gmail.com> | 2019-04-17 07:21:24 +0300 |
commit | e12c08e8d170b7ca40f204a5b0423c23a9fbc2c1 (patch) | |
tree | 8cf3453d12edb177a218ef8009357518ec6cab6a /intern/iksolver | |
parent | b3dabc200a4b0399ec6b81f2ff2730d07b44fcaa (diff) |
ClangFormat: apply to source, most of intern
Apply clang format as proposed in T53211.
For details on usage and instructions for migrating branches
without conflicts, see:
https://wiki.blender.org/wiki/Tools/ClangFormat
Diffstat (limited to 'intern/iksolver')
-rw-r--r-- | intern/iksolver/CMakeLists.txt | 28 | ||||
-rw-r--r-- | intern/iksolver/extern/IK_solver.h | 42 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_Math.h | 330 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QJacobian.cpp | 617 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QJacobian.h | 97 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QJacobianSolver.cpp | 585 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QJacobianSolver.h | 96 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QSegment.cpp | 1145 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QSegment.h | 539 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QTask.cpp | 264 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QTask.h | 204 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_Solver.cpp | 547 |
12 files changed, 2292 insertions, 2202 deletions
diff --git a/intern/iksolver/CMakeLists.txt b/intern/iksolver/CMakeLists.txt index 362e6e2bb6b..5e12cdbcc2f 100644 --- a/intern/iksolver/CMakeLists.txt +++ b/intern/iksolver/CMakeLists.txt @@ -19,27 +19,27 @@ # ***** END GPL LICENSE BLOCK ***** set(INC - intern - ../memutil + intern + ../memutil ) set(INC_SYS - ${EIGEN3_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ) set(SRC - intern/IK_QJacobian.cpp - intern/IK_QJacobianSolver.cpp - intern/IK_QSegment.cpp - intern/IK_QTask.cpp - intern/IK_Solver.cpp + intern/IK_QJacobian.cpp + intern/IK_QJacobianSolver.cpp + intern/IK_QSegment.cpp + intern/IK_QTask.cpp + intern/IK_Solver.cpp - extern/IK_solver.h - intern/IK_Math.h - intern/IK_QJacobian.h - intern/IK_QJacobianSolver.h - intern/IK_QSegment.h - intern/IK_QTask.h + extern/IK_solver.h + intern/IK_Math.h + intern/IK_QJacobian.h + intern/IK_QJacobianSolver.h + intern/IK_QSegment.h + intern/IK_QTask.h ) set(LIB diff --git a/intern/iksolver/extern/IK_solver.h b/intern/iksolver/extern/IK_solver.h index 9af6cc6988f..79c57b7f44b 100644 --- a/intern/iksolver/extern/IK_solver.h +++ b/intern/iksolver/extern/IK_solver.h @@ -22,7 +22,6 @@ * \ingroup iksolver */ - /** * \page IK - Blender inverse kinematics module. * @@ -97,28 +96,29 @@ extern "C" { typedef void IK_Segment; enum IK_SegmentFlag { - IK_XDOF = 1, - IK_YDOF = 2, - IK_ZDOF = 4, - IK_TRANS_XDOF = 8, - IK_TRANS_YDOF = 16, - IK_TRANS_ZDOF = 32 + IK_XDOF = 1, + IK_YDOF = 2, + IK_ZDOF = 4, + IK_TRANS_XDOF = 8, + IK_TRANS_YDOF = 16, + IK_TRANS_ZDOF = 32 }; typedef enum IK_SegmentAxis { - IK_X = 0, - IK_Y = 1, - IK_Z = 2, - IK_TRANS_X = 3, - IK_TRANS_Y = 4, - IK_TRANS_Z = 5 + 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); extern void IK_FreeSegment(IK_Segment *seg); extern void IK_SetParent(IK_Segment *seg, IK_Segment *parent); -extern void IK_SetTransform(IK_Segment *seg, float start[3], float rest_basis[][3], float basis[][3], float length); +extern void IK_SetTransform( + IK_Segment *seg, float start[3], float rest_basis[][3], float basis[][3], float length); extern void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax); extern void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness); @@ -144,8 +144,16 @@ IK_Solver *IK_CreateSolver(IK_Segment *root); void IK_FreeSolver(IK_Solver *solver); void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight); -void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight); -void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle); +void IK_SolverAddGoalOrientation(IK_Solver *solver, + IK_Segment *tip, + float goal[][3], + float weight); +void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, + IK_Segment *tip, + float goal[3], + float polegoal[3], + float poleangle, + int getangle); float IK_SolverGetPoleAngle(IK_Solver *solver); int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations); @@ -158,4 +166,4 @@ int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations); } #endif -#endif // __IK_SOLVER_H__ +#endif // __IK_SOLVER_H__ diff --git a/intern/iksolver/intern/IK_Math.h b/intern/iksolver/intern/IK_Math.h index b10aafaae27..ce99c3923f3 100644 --- a/intern/iksolver/intern/IK_Math.h +++ b/intern/iksolver/intern/IK_Math.h @@ -35,219 +35,227 @@ static const double IK_EPSILON = 1e-20; static inline bool FuzzyZero(double x) { - return fabs(x) < IK_EPSILON; + return fabs(x) < IK_EPSILON; } static inline double Clamp(const double x, const double min, const double max) { - return (x < min) ? min : (x > max) ? max : x; + return (x < min) ? min : (x > max) ? max : x; } -static inline Eigen::Matrix3d CreateMatrix(double xx, double xy, double xz, - double yx, double yy, double yz, - double zx, double zy, double zz) +static inline Eigen::Matrix3d CreateMatrix(double xx, + double xy, + double xz, + double yx, + double yy, + double yz, + double zx, + double zy, + double zz) { - Eigen::Matrix3d M; - M(0, 0) = xx; M(0, 1) = xy; M(0, 2) = xz; - M(1, 0) = yx; M(1, 1) = yy; M(1, 2) = yz; - M(2, 0) = zx; M(2, 1) = zy; M(2, 2) = zz; - return M; + Eigen::Matrix3d M; + M(0, 0) = xx; + M(0, 1) = xy; + M(0, 2) = xz; + M(1, 0) = yx; + M(1, 1) = yy; + M(1, 2) = yz; + M(2, 0) = zx; + M(2, 1) = zy; + M(2, 2) = zz; + return M; } static inline Eigen::Matrix3d RotationMatrix(double sine, double cosine, int axis) { - if (axis == 0) - return CreateMatrix(1.0, 0.0, 0.0, - 0.0, cosine, -sine, - 0.0, sine, cosine); - else if (axis == 1) - return CreateMatrix(cosine, 0.0, sine, - 0.0, 1.0, 0.0, - -sine, 0.0, cosine); - else - return CreateMatrix(cosine, -sine, 0.0, - sine, cosine, 0.0, - 0.0, 0.0, 1.0); + if (axis == 0) + return CreateMatrix(1.0, 0.0, 0.0, 0.0, cosine, -sine, 0.0, sine, cosine); + else if (axis == 1) + return CreateMatrix(cosine, 0.0, sine, 0.0, 1.0, 0.0, -sine, 0.0, cosine); + else + return CreateMatrix(cosine, -sine, 0.0, sine, cosine, 0.0, 0.0, 0.0, 1.0); } static inline Eigen::Matrix3d RotationMatrix(double angle, int axis) { - return RotationMatrix(sin(angle), cos(angle), axis); + return RotationMatrix(sin(angle), cos(angle), axis); } - -static inline double EulerAngleFromMatrix(const Eigen::Matrix3d& R, int axis) +static inline double EulerAngleFromMatrix(const Eigen::Matrix3d &R, int axis) { - double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1)); - - if (t > 16.0 * IK_EPSILON) { - if (axis == 0) return -atan2(R(1, 2), R(2, 2)); - else if (axis == 1) return atan2(-R(0, 2), t); - else return -atan2(R(0, 1), R(0, 0)); - } - else { - if (axis == 0) return -atan2(-R(2, 1), R(1, 1)); - else if (axis == 1) return atan2(-R(0, 2), t); - else return 0.0f; - } + double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1)); + + if (t > 16.0 * IK_EPSILON) { + if (axis == 0) + return -atan2(R(1, 2), R(2, 2)); + else if (axis == 1) + return atan2(-R(0, 2), t); + else + return -atan2(R(0, 1), R(0, 0)); + } + else { + if (axis == 0) + return -atan2(-R(2, 1), R(1, 1)); + else if (axis == 1) + return atan2(-R(0, 2), t); + else + return 0.0f; + } } static inline double safe_acos(double f) { - // acos that does not return NaN with rounding errors - if (f <= -1.0) - return M_PI; - else if (f >= 1.0) - return 0.0; - else - return acos(f); + // acos that does not return NaN with rounding errors + if (f <= -1.0) + return M_PI; + else if (f >= 1.0) + return 0.0; + else + return acos(f); } -static inline Eigen::Vector3d normalize(const Eigen::Vector3d& v) +static inline Eigen::Vector3d normalize(const Eigen::Vector3d &v) { - // a sane normalize function that doesn't give (1, 0, 0) in case - // of a zero length vector - double len = v.norm(); - return FuzzyZero(len) ? Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v / len); + // a sane normalize function that doesn't give (1, 0, 0) in case + // of a zero length vector + double len = v.norm(); + return FuzzyZero(len) ? Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v / len); } -static inline double angle(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) +static inline double angle(const Eigen::Vector3d &v1, const Eigen::Vector3d &v2) { - return safe_acos(v1.dot(v2)); + return safe_acos(v1.dot(v2)); } -static inline double ComputeTwist(const Eigen::Matrix3d& R) +static inline double ComputeTwist(const Eigen::Matrix3d &R) { - // qy and qw are the y and w components of the quaternion from R - double qy = R(0, 2) - R(2, 0); - double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1; + // qy and qw are the y and w components of the quaternion from R + double qy = R(0, 2) - R(2, 0); + double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1; - double tau = 2.0 * atan2(qy, qw); + double tau = 2.0 * atan2(qy, qw); - return tau; + return tau; } static inline Eigen::Matrix3d ComputeTwistMatrix(double tau) { - return RotationMatrix(tau, 1); + return RotationMatrix(tau, 1); } -static inline void RemoveTwist(Eigen::Matrix3d& R) +static inline void RemoveTwist(Eigen::Matrix3d &R) { - // compute twist parameter - double tau = ComputeTwist(R); + // compute twist parameter + double tau = ComputeTwist(R); - // compute twist matrix - Eigen::Matrix3d T = ComputeTwistMatrix(tau); + // compute twist matrix + Eigen::Matrix3d T = ComputeTwistMatrix(tau); - // remove twist - R = R * T.transpose(); + // remove twist + R = R * T.transpose(); } -static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d& R) +static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d &R) { - // compute twist parameter - double tau = ComputeTwist(R); + // compute twist parameter + double tau = ComputeTwist(R); - // compute swing parameters - double num = 2.0 * (1.0 + R(1, 1)); + // compute swing parameters + double num = 2.0 * (1.0 + R(1, 1)); - // singularity at pi - if (fabs(num) < IK_EPSILON) - // TODO: this does now rotation of size pi over z axis, but could - // be any axis, how to deal with this i'm not sure, maybe don't - // enforce limits at all then - return Eigen::Vector3d(0.0, tau, 1.0); + // singularity at pi + if (fabs(num) < IK_EPSILON) + // TODO: this does now rotation of size pi over z axis, but could + // be any axis, how to deal with this i'm not sure, maybe don't + // enforce limits at all then + return Eigen::Vector3d(0.0, tau, 1.0); - num = 1.0 / sqrt(num); - double ax = -R(2, 1) * num; - double az = R(0, 1) * num; + num = 1.0 / sqrt(num); + double ax = -R(2, 1) * num; + double az = R(0, 1) * num; - return Eigen::Vector3d(ax, tau, az); + return Eigen::Vector3d(ax, tau, az); } static inline Eigen::Matrix3d ComputeSwingMatrix(double ax, double az) { - // length of (ax, 0, az) = sin(theta/2) - double sine2 = ax * ax + az * az; - double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2); - - // compute swing matrix - Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az)); - - return S; -} - -static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d& R) -{ - Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2), - R(0, 2) - R(2, 0), - R(1, 0) - R(0, 1)); - - double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2); - double l = delta.norm(); - - if (!FuzzyZero(l)) - delta *= c / l; - - return delta; -} - -static inline bool EllipseClamp(double& ax, double& az, double *amin, double *amax) -{ - double 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 (FuzzyZero(xlim) || FuzzyZero(zlim)) { - if (x <= xlim && z <= zlim) - return false; - - if (x > xlim) - x = xlim; - if (z > zlim) - z = zlim; - } - else { - double invx = 1.0 / (xlim * xlim); - double invz = 1.0 / (zlim * zlim); - - if ((x * x * invx + z * z * invz) <= 1.0) - return false; - - if (FuzzyZero(x)) { - x = 0.0; - z = zlim; - } - else { - double rico = z / x; - double 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; + // length of (ax, 0, az) = sin(theta/2) + double sine2 = ax * ax + az * az; + double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2); + + // compute swing matrix + Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az)); + + return S; +} + +static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d &R) +{ + Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); + + double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2); + double l = delta.norm(); + + if (!FuzzyZero(l)) + delta *= c / l; + + return delta; } +static inline bool EllipseClamp(double &ax, double &az, double *amin, double *amax) +{ + double 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 (FuzzyZero(xlim) || FuzzyZero(zlim)) { + if (x <= xlim && z <= zlim) + return false; + + if (x > xlim) + x = xlim; + if (z > zlim) + z = zlim; + } + else { + double invx = 1.0 / (xlim * xlim); + double invz = 1.0 / (zlim * zlim); + + if ((x * x * invx + z * z * invz) <= 1.0) + return false; + + if (FuzzyZero(x)) { + x = 0.0; + z = zlim; + } + else { + double rico = z / x; + double 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; +} diff --git a/intern/iksolver/intern/IK_QJacobian.cpp b/intern/iksolver/intern/IK_QJacobian.cpp index 678486e36f4..7f77968a5d4 100644 --- a/intern/iksolver/intern/IK_QJacobian.cpp +++ b/intern/iksolver/intern/IK_QJacobian.cpp @@ -21,11 +21,9 @@ * \ingroup iksolver */ - #include "IK_QJacobian.h" -IK_QJacobian::IK_QJacobian() - : m_sdls(true), m_min_damp(1.0) +IK_QJacobian::IK_QJacobian() : m_sdls(true), m_min_damp(1.0) { } @@ -35,392 +33,393 @@ IK_QJacobian::~IK_QJacobian() void IK_QJacobian::ArmMatrices(int dof, int task_size) { - m_dof = dof; - m_task_size = task_size; + m_dof = dof; + m_task_size = task_size; - m_jacobian.resize(task_size, dof); - m_jacobian.setZero(); + m_jacobian.resize(task_size, dof); + m_jacobian.setZero(); - m_alpha.resize(dof); - m_alpha.setZero(); + m_alpha.resize(dof); + m_alpha.setZero(); - m_nullspace.resize(dof, dof); + m_nullspace.resize(dof, dof); - m_d_theta.resize(dof); - m_d_theta_tmp.resize(dof); - m_d_norm_weight.resize(dof); + m_d_theta.resize(dof); + m_d_theta_tmp.resize(dof); + m_d_norm_weight.resize(dof); - m_norm.resize(dof); - m_norm.setZero(); + m_norm.resize(dof); + m_norm.setZero(); - m_beta.resize(task_size); + m_beta.resize(task_size); - m_weight.resize(dof); - m_weight_sqrt.resize(dof); - m_weight.setOnes(); - m_weight_sqrt.setOnes(); + m_weight.resize(dof); + m_weight_sqrt.resize(dof); + m_weight.setOnes(); + m_weight_sqrt.setOnes(); - if (task_size >= dof) { - m_transpose = false; + if (task_size >= dof) { + m_transpose = false; - m_jacobian_tmp.resize(task_size, dof); + m_jacobian_tmp.resize(task_size, dof); - m_svd_u.resize(task_size, dof); - m_svd_v.resize(dof, dof); - m_svd_w.resize(dof); + m_svd_u.resize(task_size, dof); + m_svd_v.resize(dof, dof); + m_svd_w.resize(dof); - m_svd_u_beta.resize(dof); - } - else { - // use the SVD of the transpose jacobian, it works just as well - // as the original, and often allows using smaller matrices. - m_transpose = true; + m_svd_u_beta.resize(dof); + } + else { + // use the SVD of the transpose jacobian, it works just as well + // as the original, and often allows using smaller matrices. + m_transpose = true; - m_jacobian_tmp.resize(dof, task_size); + m_jacobian_tmp.resize(dof, task_size); - m_svd_u.resize(task_size, task_size); - m_svd_v.resize(dof, task_size); - m_svd_w.resize(task_size); + m_svd_u.resize(task_size, task_size); + m_svd_v.resize(dof, task_size); + m_svd_w.resize(task_size); - m_svd_u_beta.resize(task_size); - } + m_svd_u_beta.resize(task_size); + } } -void IK_QJacobian::SetBetas(int id, int, const Vector3d& v) +void IK_QJacobian::SetBetas(int id, int, const Vector3d &v) { - m_beta[id + 0] = v.x(); - m_beta[id + 1] = v.y(); - m_beta[id + 2] = v.z(); + m_beta[id + 0] = v.x(); + m_beta[id + 1] = v.y(); + m_beta[id + 2] = v.z(); } -void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d& v, double norm_weight) +void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight) { - m_jacobian(id + 0, dof_id) = v.x() * m_weight_sqrt[dof_id]; - m_jacobian(id + 1, dof_id) = v.y() * m_weight_sqrt[dof_id]; - m_jacobian(id + 2, dof_id) = v.z() * m_weight_sqrt[dof_id]; + m_jacobian(id + 0, dof_id) = v.x() * m_weight_sqrt[dof_id]; + m_jacobian(id + 1, dof_id) = v.y() * m_weight_sqrt[dof_id]; + m_jacobian(id + 2, dof_id) = v.z() * m_weight_sqrt[dof_id]; - m_d_norm_weight[dof_id] = norm_weight; + m_d_norm_weight[dof_id] = norm_weight; } void IK_QJacobian::Invert() { - if (m_transpose) { - // SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal, - // so J = U*W*Vt and Jinv = V*Winv*Ut - Eigen::JacobiSVD<MatrixXd> svd(m_jacobian.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV); - m_svd_u = svd.matrixV(); - m_svd_w = svd.singularValues(); - m_svd_v = svd.matrixU(); - } - else { - // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal, - // so Jinv = V*Winv*Ut - Eigen::JacobiSVD<MatrixXd> svd(m_jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); - m_svd_u = svd.matrixU(); - m_svd_w = svd.singularValues(); - m_svd_v = svd.matrixV(); - } - - if (m_sdls) - InvertSDLS(); - else - InvertDLS(); + if (m_transpose) { + // SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal, + // so J = U*W*Vt and Jinv = V*Winv*Ut + Eigen::JacobiSVD<MatrixXd> svd(m_jacobian.transpose(), + Eigen::ComputeThinU | Eigen::ComputeThinV); + m_svd_u = svd.matrixV(); + m_svd_w = svd.singularValues(); + m_svd_v = svd.matrixU(); + } + else { + // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal, + // so Jinv = V*Winv*Ut + Eigen::JacobiSVD<MatrixXd> svd(m_jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + m_svd_u = svd.matrixU(); + m_svd_w = svd.singularValues(); + m_svd_v = svd.matrixV(); + } + + if (m_sdls) + InvertSDLS(); + else + InvertDLS(); } bool IK_QJacobian::ComputeNullProjection() { - double epsilon = 1e-10; - - // compute null space projection based on V - int i, j, rank = 0; - for (i = 0; i < m_svd_w.size(); i++) - if (m_svd_w[i] > epsilon) - rank++; - - if (rank < m_task_size) - return false; - - MatrixXd basis(m_svd_v.rows(), rank); - int b = 0; - - for (i = 0; i < m_svd_w.size(); i++) - if (m_svd_w[i] > epsilon) { - for (j = 0; j < m_svd_v.rows(); j++) - basis(j, b) = m_svd_v(j, i); - b++; - } - - m_nullspace = basis * basis.transpose(); - - for (i = 0; i < m_nullspace.rows(); i++) - for (j = 0; j < m_nullspace.cols(); j++) - if (i == j) - m_nullspace(i, j) = 1.0 - m_nullspace(i, j); - else - m_nullspace(i, j) = -m_nullspace(i, j); - - return true; + double epsilon = 1e-10; + + // compute null space projection based on V + int i, j, rank = 0; + for (i = 0; i < m_svd_w.size(); i++) + if (m_svd_w[i] > epsilon) + rank++; + + if (rank < m_task_size) + return false; + + MatrixXd basis(m_svd_v.rows(), rank); + int b = 0; + + for (i = 0; i < m_svd_w.size(); i++) + if (m_svd_w[i] > epsilon) { + for (j = 0; j < m_svd_v.rows(); j++) + basis(j, b) = m_svd_v(j, i); + b++; + } + + m_nullspace = basis * basis.transpose(); + + for (i = 0; i < m_nullspace.rows(); i++) + for (j = 0; j < m_nullspace.cols(); j++) + if (i == j) + m_nullspace(i, j) = 1.0 - m_nullspace(i, j); + else + m_nullspace(i, j) = -m_nullspace(i, j); + + return true; } -void IK_QJacobian::SubTask(IK_QJacobian& jacobian) +void IK_QJacobian::SubTask(IK_QJacobian &jacobian) { - if (!ComputeNullProjection()) - return; + if (!ComputeNullProjection()) + return; - // restrict lower priority jacobian - jacobian.Restrict(m_d_theta, m_nullspace); + // restrict lower priority jacobian + jacobian.Restrict(m_d_theta, m_nullspace); - // add angle update from lower priority - jacobian.Invert(); + // add angle update from lower priority + jacobian.Invert(); - // note: now damps secondary angles with minimum damping value from - // SDLS, to avoid shaking when the primary task is near singularities, - // doesn't work well at all - int i; - for (i = 0; i < m_d_theta.size(); i++) - m_d_theta[i] = m_d_theta[i] + /*m_min_damp * */ jacobian.AngleUpdate(i); + // note: now damps secondary angles with minimum damping value from + // SDLS, to avoid shaking when the primary task is near singularities, + // doesn't work well at all + int i; + for (i = 0; i < m_d_theta.size(); i++) + m_d_theta[i] = m_d_theta[i] + /*m_min_damp * */ jacobian.AngleUpdate(i); } -void IK_QJacobian::Restrict(VectorXd& d_theta, MatrixXd& nullspace) +void IK_QJacobian::Restrict(VectorXd &d_theta, MatrixXd &nullspace) { - // subtract part already moved by higher task from beta - m_beta = m_beta - m_jacobian * d_theta; + // subtract part already moved by higher task from beta + m_beta = m_beta - m_jacobian * d_theta; + + // note: should we be using the norm of the unrestricted jacobian for SDLS? - // note: should we be using the norm of the unrestricted jacobian for SDLS? - - // project jacobian on to null space of higher priority task - m_jacobian = m_jacobian * nullspace; + // project jacobian on to null space of higher priority task + m_jacobian = m_jacobian * nullspace; } void IK_QJacobian::InvertSDLS() { - // Compute the dampeds least squeares pseudo inverse of J. - // - // Since J is usually not invertible (most of the times it's not even - // square), the psuedo inverse is used. This gives us a least squares - // solution. - // - // This is fine when the J*Jt is of full rank. When J*Jt is near to - // singular the least squares inverse tries to minimize |J(dtheta) - dX)| - // and doesn't try to minimize dTheta. This results in eratic changes in - // angle. The damped least squares minimizes |dtheta| to try and reduce this - // erratic behaviour. - // - // The selectively damped least squares (SDLS) is used here instead of the - // DLS. The SDLS damps individual singular values, instead of using a single - // damping term. - - double max_angle_change = M_PI / 4.0; - double epsilon = 1e-10; - int i, j; - - m_d_theta.setZero(); - m_min_damp = 1.0; - - for (i = 0; i < m_dof; i++) { - m_norm[i] = 0.0; - for (j = 0; j < m_task_size; j += 3) { - double n = 0.0; - n += m_jacobian(j, i) * m_jacobian(j, i); - n += m_jacobian(j + 1, i) * m_jacobian(j + 1, i); - n += m_jacobian(j + 2, i) * m_jacobian(j + 2, i); - m_norm[i] += sqrt(n); - } - } - - for (i = 0; i < m_svd_w.size(); i++) { - if (m_svd_w[i] <= epsilon) - continue; - - double wInv = 1.0 / m_svd_w[i]; - double alpha = 0.0; - double N = 0.0; - - // compute alpha and N - for (j = 0; j < m_svd_u.rows(); j += 3) { - alpha += m_svd_u(j, i) * m_beta[j]; - alpha += m_svd_u(j + 1, i) * m_beta[j + 1]; - alpha += m_svd_u(j + 2, i) * m_beta[j + 2]; - - // note: for 1 end effector, N will always be 1, since U is - // orthogonal, .. so could be optimized - double tmp; - tmp = m_svd_u(j, i) * m_svd_u(j, i); - tmp += m_svd_u(j + 1, i) * m_svd_u(j + 1, i); - tmp += m_svd_u(j + 2, i) * m_svd_u(j + 2, i); - N += sqrt(tmp); - } - alpha *= wInv; - - // compute M, dTheta and max_dtheta - double M = 0.0; - double max_dtheta = 0.0, abs_dtheta; - - for (j = 0; j < m_d_theta.size(); j++) { - double v = m_svd_v(j, i); - M += fabs(v) * m_norm[j]; - - // compute tmporary dTheta's - m_d_theta_tmp[j] = v * alpha; - - // find largest absolute dTheta - // multiply with weight to prevent unnecessary damping - abs_dtheta = fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j]; - if (abs_dtheta > max_dtheta) - max_dtheta = abs_dtheta; - } - - M *= wInv; - - // compute damping term and damp the dTheta's - double gamma = max_angle_change; - if (N < M) - gamma *= N / M; - - double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0; - - 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 - - double 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; - } - - // weight + prevent from doing angle updates with angles > max_angle_change - double max_angle = 0.0, abs_angle; - - for (j = 0; j < m_dof; j++) { - m_d_theta[j] *= m_weight[j]; - - abs_angle = fabs(m_d_theta[j]); - - if (abs_angle > max_angle) - max_angle = abs_angle; - } - - if (max_angle > max_angle_change) { - double damp = (max_angle_change) / (max_angle_change + max_angle); - - for (j = 0; j < m_dof; j++) - m_d_theta[j] *= damp; - } + // Compute the dampeds least squeares pseudo inverse of J. + // + // Since J is usually not invertible (most of the times it's not even + // square), the psuedo inverse is used. This gives us a least squares + // solution. + // + // This is fine when the J*Jt is of full rank. When J*Jt is near to + // singular the least squares inverse tries to minimize |J(dtheta) - dX)| + // and doesn't try to minimize dTheta. This results in eratic changes in + // angle. The damped least squares minimizes |dtheta| to try and reduce this + // erratic behaviour. + // + // The selectively damped least squares (SDLS) is used here instead of the + // DLS. The SDLS damps individual singular values, instead of using a single + // damping term. + + double max_angle_change = M_PI / 4.0; + double epsilon = 1e-10; + int i, j; + + m_d_theta.setZero(); + m_min_damp = 1.0; + + for (i = 0; i < m_dof; i++) { + m_norm[i] = 0.0; + for (j = 0; j < m_task_size; j += 3) { + double n = 0.0; + n += m_jacobian(j, i) * m_jacobian(j, i); + n += m_jacobian(j + 1, i) * m_jacobian(j + 1, i); + n += m_jacobian(j + 2, i) * m_jacobian(j + 2, i); + m_norm[i] += sqrt(n); + } + } + + for (i = 0; i < m_svd_w.size(); i++) { + if (m_svd_w[i] <= epsilon) + continue; + + double wInv = 1.0 / m_svd_w[i]; + double alpha = 0.0; + double N = 0.0; + + // compute alpha and N + for (j = 0; j < m_svd_u.rows(); j += 3) { + alpha += m_svd_u(j, i) * m_beta[j]; + alpha += m_svd_u(j + 1, i) * m_beta[j + 1]; + alpha += m_svd_u(j + 2, i) * m_beta[j + 2]; + + // note: for 1 end effector, N will always be 1, since U is + // orthogonal, .. so could be optimized + double tmp; + tmp = m_svd_u(j, i) * m_svd_u(j, i); + tmp += m_svd_u(j + 1, i) * m_svd_u(j + 1, i); + tmp += m_svd_u(j + 2, i) * m_svd_u(j + 2, i); + N += sqrt(tmp); + } + alpha *= wInv; + + // compute M, dTheta and max_dtheta + double M = 0.0; + double max_dtheta = 0.0, abs_dtheta; + + for (j = 0; j < m_d_theta.size(); j++) { + double v = m_svd_v(j, i); + M += fabs(v) * m_norm[j]; + + // compute tmporary dTheta's + m_d_theta_tmp[j] = v * alpha; + + // find largest absolute dTheta + // multiply with weight to prevent unnecessary damping + abs_dtheta = fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j]; + if (abs_dtheta > max_dtheta) + max_dtheta = abs_dtheta; + } + + M *= wInv; + + // compute damping term and damp the dTheta's + double gamma = max_angle_change; + if (N < M) + gamma *= N / M; + + double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0; + + 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 + + double 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; + } + + // weight + prevent from doing angle updates with angles > max_angle_change + double max_angle = 0.0, abs_angle; + + for (j = 0; j < m_dof; j++) { + m_d_theta[j] *= m_weight[j]; + + abs_angle = fabs(m_d_theta[j]); + + if (abs_angle > max_angle) + max_angle = abs_angle; + } + + if (max_angle > max_angle_change) { + double damp = (max_angle_change) / (max_angle_change + max_angle); + + for (j = 0; j < m_dof; j++) + m_d_theta[j] *= damp; + } } void IK_QJacobian::InvertDLS() { - // Compute damped least squares inverse of pseudo inverse - // Compute damping term lambda + // Compute damped least squares inverse of pseudo inverse + // Compute damping term lambda + + // Note when lambda is zero this is equivalent to the + // least squares solution. This is fine when the m_jjt is + // of full rank. When m_jjt is near to singular the least squares + // inverse tries to minimize |J(dtheta) - dX)| and doesn't + // try to minimize dTheta. This results in eratic changes in angle. + // Damped least squares minimizes |dtheta| to try and reduce this + // erratic behaviour. - // Note when lambda is zero this is equivalent to the - // least squares solution. This is fine when the m_jjt is - // of full rank. When m_jjt is near to singular the least squares - // inverse tries to minimize |J(dtheta) - dX)| and doesn't - // try to minimize dTheta. This results in eratic changes in angle. - // Damped least squares minimizes |dtheta| to try and reduce this - // erratic behaviour. + // We don't want to use the damped solution everywhere so we + // only increase lamda from zero as we approach a singularity. - // We don't want to use the damped solution everywhere so we - // only increase lamda from zero as we approach a singularity. + // find the smallest non-zero W value, anything below epsilon is + // treated as zero - // find the smallest non-zero W value, anything below epsilon is - // treated as zero + double epsilon = 1e-10; + double max_angle_change = 0.1; + double x_length = sqrt(m_beta.dot(m_beta)); - double epsilon = 1e-10; - double max_angle_change = 0.1; - double x_length = sqrt(m_beta.dot(m_beta)); + int i, j; + double w_min = std::numeric_limits<double>::max(); - int i, j; - double w_min = std::numeric_limits<double>::max(); + for (i = 0; i < m_svd_w.size(); i++) { + if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min) + w_min = m_svd_w[i]; + } - for (i = 0; i < m_svd_w.size(); i++) { - if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min) - w_min = m_svd_w[i]; - } - - // compute lambda damping term + // compute lambda damping term - double d = x_length / max_angle_change; - double lambda; + double d = x_length / max_angle_change; + double lambda; - if (w_min <= d / 2) - lambda = d / 2; - else if (w_min < d) - lambda = sqrt(w_min * (d - w_min)); - else - lambda = 0.0; + if (w_min <= d / 2) + lambda = d / 2; + else if (w_min < d) + lambda = sqrt(w_min * (d - w_min)); + else + lambda = 0.0; - lambda *= lambda; + lambda *= lambda; - if (lambda > 10) - lambda = 10; + if (lambda > 10) + lambda = 10; - // immediately multiply with Beta, so we can do matrix*vector products - // rather than matrix*matrix products + // immediately multiply with Beta, so we can do matrix*vector products + // rather than matrix*matrix products - // compute Ut*Beta - m_svd_u_beta = m_svd_u.transpose() * m_beta; + // compute Ut*Beta + m_svd_u_beta = m_svd_u.transpose() * m_beta; - m_d_theta.setZero(); + m_d_theta.setZero(); - for (i = 0; i < m_svd_w.size(); i++) { - if (m_svd_w[i] > epsilon) { - double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda); + for (i = 0; i < m_svd_w.size(); i++) { + if (m_svd_w[i] > epsilon) { + double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda); - // compute V*Winv*Ut*Beta - m_svd_u_beta[i] *= wInv; + // compute V*Winv*Ut*Beta + m_svd_u_beta[i] *= wInv; - for (j = 0; j < m_d_theta.size(); j++) - m_d_theta[j] += m_svd_v(j, i) * m_svd_u_beta[i]; - } - } + for (j = 0; j < m_d_theta.size(); j++) + m_d_theta[j] += m_svd_v(j, i) * m_svd_u_beta[i]; + } + } - for (j = 0; j < m_d_theta.size(); j++) - m_d_theta[j] *= m_weight[j]; + for (j = 0; j < m_d_theta.size(); j++) + m_d_theta[j] *= m_weight[j]; } void IK_QJacobian::Lock(int dof_id, double delta) { - int i; + int i; - for (i = 0; i < m_task_size; i++) { - m_beta[i] -= m_jacobian(i, dof_id) * delta; - m_jacobian(i, dof_id) = 0.0; - } + for (i = 0; i < m_task_size; i++) { + m_beta[i] -= m_jacobian(i, dof_id) * delta; + m_jacobian(i, dof_id) = 0.0; + } - m_norm[dof_id] = 0.0; // unneeded - m_d_theta[dof_id] = 0.0; + m_norm[dof_id] = 0.0; // unneeded + m_d_theta[dof_id] = 0.0; } double IK_QJacobian::AngleUpdate(int dof_id) const { - return m_d_theta[dof_id]; + return m_d_theta[dof_id]; } double IK_QJacobian::AngleUpdateNorm() const { - int i; - double mx = 0.0, dtheta_abs; - - for (i = 0; i < m_d_theta.size(); i++) { - dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]); - if (dtheta_abs > mx) - mx = dtheta_abs; - } - - return mx; + int i; + double mx = 0.0, dtheta_abs; + + for (i = 0; i < m_d_theta.size(); i++) { + dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]); + if (dtheta_abs > mx) + mx = dtheta_abs; + } + + return mx; } void IK_QJacobian::SetDoFWeight(int dof, double weight) { - m_weight[dof] = weight; - m_weight_sqrt[dof] = sqrt(weight); + m_weight[dof] = weight; + m_weight_sqrt[dof] = sqrt(weight); } - diff --git a/intern/iksolver/intern/IK_QJacobian.h b/intern/iksolver/intern/IK_QJacobian.h index f7b26ad6d33..1b18107fb67 100644 --- a/intern/iksolver/intern/IK_QJacobian.h +++ b/intern/iksolver/intern/IK_QJacobian.h @@ -26,72 +26,69 @@ #include "IK_Math.h" -class IK_QJacobian -{ -public: - IK_QJacobian(); - ~IK_QJacobian(); +class IK_QJacobian { + public: + IK_QJacobian(); + ~IK_QJacobian(); - // Call once to initialize - void ArmMatrices(int dof, int task_size); - void SetDoFWeight(int dof, double weight); + // Call once to initialize + void ArmMatrices(int dof, int task_size); + void SetDoFWeight(int dof, double weight); - // Iteratively called - void SetBetas(int id, int size, const Vector3d& v); - void SetDerivatives(int id, int dof_id, const Vector3d& v, double norm_weight); + // Iteratively called + void SetBetas(int id, int size, const Vector3d &v); + void SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight); - void Invert(); + void Invert(); - double AngleUpdate(int dof_id) const; - double AngleUpdateNorm() const; + double AngleUpdate(int dof_id) const; + double AngleUpdateNorm() const; - // DoF locking for inner clamping loop - void Lock(int dof_id, double delta); + // DoF locking for inner clamping loop + void Lock(int dof_id, double delta); - // Secondary task - bool ComputeNullProjection(); + // Secondary task + bool ComputeNullProjection(); - void Restrict(VectorXd& d_theta, MatrixXd& nullspace); - void SubTask(IK_QJacobian& jacobian); + void Restrict(VectorXd &d_theta, MatrixXd &nullspace); + void SubTask(IK_QJacobian &jacobian); -private: - - void InvertSDLS(); - void InvertDLS(); + private: + void InvertSDLS(); + void InvertDLS(); - int m_dof, m_task_size; - bool m_transpose; + int m_dof, m_task_size; + bool m_transpose; - // the jacobian matrix and it's null space projector - MatrixXd m_jacobian, m_jacobian_tmp; - MatrixXd m_nullspace; + // the jacobian matrix and it's null space projector + MatrixXd m_jacobian, m_jacobian_tmp; + MatrixXd m_nullspace; - /// the vector of intermediate betas - VectorXd m_beta; + /// the vector of intermediate betas + VectorXd m_beta; - /// the vector of computed angle changes - VectorXd m_d_theta; - VectorXd m_d_norm_weight; + /// the vector of computed angle changes + VectorXd m_d_theta; + VectorXd m_d_norm_weight; - /// space required for SVD computation - VectorXd m_svd_w; - MatrixXd m_svd_v; - MatrixXd m_svd_u; + /// space required for SVD computation + VectorXd m_svd_w; + MatrixXd m_svd_v; + MatrixXd m_svd_u; - VectorXd m_svd_u_beta; + VectorXd m_svd_u_beta; - // space required for SDLS + // space required for SDLS - bool m_sdls; - VectorXd m_norm; - VectorXd m_d_theta_tmp; - double m_min_damp; + bool m_sdls; + VectorXd m_norm; + VectorXd m_d_theta_tmp; + double m_min_damp; - // null space task vector - VectorXd m_alpha; + // null space task vector + VectorXd m_alpha; - // dof weighting - VectorXd m_weight; - VectorXd m_weight_sqrt; + // dof weighting + VectorXd m_weight; + VectorXd m_weight_sqrt; }; - diff --git a/intern/iksolver/intern/IK_QJacobianSolver.cpp b/intern/iksolver/intern/IK_QJacobianSolver.cpp index 5288def7f55..90032096d37 100644 --- a/intern/iksolver/intern/IK_QJacobianSolver.cpp +++ b/intern/iksolver/intern/IK_QJacobianSolver.cpp @@ -21,7 +21,6 @@ * \ingroup iksolver */ - #include <stdio.h> #include "IK_QJacobianSolver.h" @@ -29,340 +28,338 @@ //#include "analyze.h" IK_QJacobianSolver::IK_QJacobianSolver() { - m_poleconstraint = false; - m_getpoleangle = false; - m_rootmatrix.setIdentity(); + m_poleconstraint = false; + m_getpoleangle = false; + m_rootmatrix.setIdentity(); } double IK_QJacobianSolver::ComputeScale() { - std::vector<IK_QSegment *>::iterator seg; - double length = 0.0f; - - for (seg = m_segments.begin(); seg != m_segments.end(); seg++) - length += (*seg)->MaxExtension(); - - if (length == 0.0) - return 1.0; - else - return 1.0 / length; + std::vector<IK_QSegment *>::iterator seg; + double length = 0.0f; + + for (seg = m_segments.begin(); seg != m_segments.end(); seg++) + length += (*seg)->MaxExtension(); + + if (length == 0.0) + return 1.0; + else + return 1.0 / length; } -void IK_QJacobianSolver::Scale(double scale, std::list<IK_QTask *>& tasks) +void IK_QJacobianSolver::Scale(double scale, std::list<IK_QTask *> &tasks) { - std::list<IK_QTask *>::iterator task; - std::vector<IK_QSegment *>::iterator seg; - - for (task = tasks.begin(); task != tasks.end(); task++) - (*task)->Scale(scale); - - for (seg = m_segments.begin(); seg != m_segments.end(); seg++) - (*seg)->Scale(scale); - - m_rootmatrix.translation() *= scale; - m_goal *= scale; - m_polegoal *= scale; + std::list<IK_QTask *>::iterator task; + std::vector<IK_QSegment *>::iterator seg; + + for (task = tasks.begin(); task != tasks.end(); task++) + (*task)->Scale(scale); + + for (seg = m_segments.begin(); seg != m_segments.end(); seg++) + (*seg)->Scale(scale); + + m_rootmatrix.translation() *= scale; + m_goal *= scale; + m_polegoal *= scale; } void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg) { - m_segments.push_back(seg); + m_segments.push_back(seg); - IK_QSegment *child; - for (child = seg->Child(); child; child = child->Sibling()) - AddSegmentList(child); + IK_QSegment *child; + for (child = seg->Child(); child; child = child->Sibling()) + AddSegmentList(child); } -bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *>& tasks) +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; - double 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 || FuzzyZero(primary_weight)) - return false; - - m_secondary_enabled = (secondary > 0); - - // rescale weights of tasks to sum up to 1 - double primary_rescale = 1.0 / primary_weight; - double secondary_rescale; - if (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; + 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; + double 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 || FuzzyZero(primary_weight)) + return false; + + m_secondary_enabled = (secondary > 0); + + // rescale weights of tasks to sum up to 1 + double primary_rescale = 1.0 / primary_weight; + double secondary_rescale; + if (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; } -void IK_QJacobianSolver::SetPoleVectorConstraint(IK_QSegment *tip, Vector3d& goal, Vector3d& polegoal, float poleangle, bool getangle) +void IK_QJacobianSolver::SetPoleVectorConstraint( + IK_QSegment *tip, Vector3d &goal, Vector3d &polegoal, float poleangle, bool getangle) { - m_poleconstraint = true; - m_poletip = tip; - m_goal = goal; - m_polegoal = polegoal; - m_poleangle = (getangle) ? 0.0f : poleangle; - m_getpoleangle = getangle; + m_poleconstraint = true; + m_poletip = tip; + m_goal = goal; + m_polegoal = polegoal; + m_poleangle = (getangle) ? 0.0f : poleangle; + m_getpoleangle = getangle; } -void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *>& tasks) +void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *> &tasks) { - // this function will be called before and after solving. calling it before - // solving gives predictable solutions by rotating towards the solution, - // and calling it afterwards ensures the solution is exact. - - if (!m_poleconstraint) - return; - - // disable pole vector constraint in case of multiple position tasks - std::list<IK_QTask *>::iterator task; - int positiontasks = 0; - - for (task = tasks.begin(); task != tasks.end(); task++) - if ((*task)->PositionTask()) - positiontasks++; - - if (positiontasks >= 2) { - m_poleconstraint = false; - return; - } - - // get positions and rotations - root->UpdateTransform(m_rootmatrix); - - const Vector3d rootpos = root->GlobalStart(); - const Vector3d endpos = m_poletip->GlobalEnd(); - const Matrix3d& rootbasis = root->GlobalTransform().linear(); - - // construct "lookat" matrices (like gluLookAt), based on a direction and - // an up vector, with the direction going from the root to the end effector - // and the up vector going from the root to the pole constraint position. - Vector3d dir = normalize(endpos - rootpos); - Vector3d rootx = rootbasis.col(0); - Vector3d rootz = rootbasis.col(2); - Vector3d up = rootx * cos(m_poleangle) + rootz *sin(m_poleangle); - - // in post, don't rotate towards the goal but only correct the pole up - Vector3d poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos); - Vector3d poleup = normalize(m_polegoal - rootpos); - - Matrix3d mat, polemat; - - mat.row(0) = normalize(dir.cross(up)); - mat.row(1) = mat.row(0).cross(dir); - mat.row(2) = -dir; - - polemat.row(0) = normalize(poledir.cross(poleup)); - polemat.row(1) = polemat.row(0).cross(poledir); - polemat.row(2) = -poledir; - - if (m_getpoleangle) { - // we compute the pole angle that to rotate towards the target - m_poleangle = angle(mat.row(1), polemat.row(1)); - - double dt = rootz.dot(mat.row(1) * cos(m_poleangle) + mat.row(0) * sin(m_poleangle)); - if (dt > 0.0) - m_poleangle = -m_poleangle; - - // solve again, with the pole angle we just computed - m_getpoleangle = false; - ConstrainPoleVector(root, tasks); - } - else { - // now we set as root matrix the difference between the current and - // desired rotation based on the pole vector constraint. we use - // transpose instead of inverse because we have orthogonal matrices - // anyway, and in case of a singular matrix we don't get NaN's. - Affine3d trans; - trans.linear() = polemat.transpose() * mat; - trans.translation() = Vector3d(0, 0, 0); - m_rootmatrix = trans * m_rootmatrix; - } + // this function will be called before and after solving. calling it before + // solving gives predictable solutions by rotating towards the solution, + // and calling it afterwards ensures the solution is exact. + + if (!m_poleconstraint) + return; + + // disable pole vector constraint in case of multiple position tasks + std::list<IK_QTask *>::iterator task; + int positiontasks = 0; + + for (task = tasks.begin(); task != tasks.end(); task++) + if ((*task)->PositionTask()) + positiontasks++; + + if (positiontasks >= 2) { + m_poleconstraint = false; + return; + } + + // get positions and rotations + root->UpdateTransform(m_rootmatrix); + + const Vector3d rootpos = root->GlobalStart(); + const Vector3d endpos = m_poletip->GlobalEnd(); + const Matrix3d &rootbasis = root->GlobalTransform().linear(); + + // construct "lookat" matrices (like gluLookAt), based on a direction and + // an up vector, with the direction going from the root to the end effector + // and the up vector going from the root to the pole constraint position. + Vector3d dir = normalize(endpos - rootpos); + Vector3d rootx = rootbasis.col(0); + Vector3d rootz = rootbasis.col(2); + Vector3d up = rootx * cos(m_poleangle) + rootz * sin(m_poleangle); + + // in post, don't rotate towards the goal but only correct the pole up + Vector3d poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos); + Vector3d poleup = normalize(m_polegoal - rootpos); + + Matrix3d mat, polemat; + + mat.row(0) = normalize(dir.cross(up)); + mat.row(1) = mat.row(0).cross(dir); + mat.row(2) = -dir; + + polemat.row(0) = normalize(poledir.cross(poleup)); + polemat.row(1) = polemat.row(0).cross(poledir); + polemat.row(2) = -poledir; + + if (m_getpoleangle) { + // we compute the pole angle that to rotate towards the target + m_poleangle = angle(mat.row(1), polemat.row(1)); + + double dt = rootz.dot(mat.row(1) * cos(m_poleangle) + mat.row(0) * sin(m_poleangle)); + if (dt > 0.0) + m_poleangle = -m_poleangle; + + // solve again, with the pole angle we just computed + m_getpoleangle = false; + ConstrainPoleVector(root, tasks); + } + else { + // now we set as root matrix the difference between the current and + // desired rotation based on the pole vector constraint. we use + // transpose instead of inverse because we have orthogonal matrices + // anyway, and in case of a singular matrix we don't get NaN's. + Affine3d trans; + trans.linear() = polemat.transpose() * mat; + trans.translation() = Vector3d(0, 0, 0); + m_rootmatrix = trans * m_rootmatrix; + } } -bool IK_QJacobianSolver::UpdateAngles(double& norm) +bool IK_QJacobianSolver::UpdateAngles(double &norm) { - // assing each segment a unique id for the jacobian - std::vector<IK_QSegment *>::iterator seg; - IK_QSegment *qseg, *minseg = NULL; - double minabsdelta = 1e10, absdelta; - Vector3d delta, mindelta; - bool locked = false, clamp[3]; - int i, mindof = 0; - - // here we check if any angle limits were violated. angles whose clamped - // position is the same as it was before, are locked immediate. of the - // other violation angles the most violating angle is rememberd - for (seg = m_segments.begin(); seg != m_segments.end(); seg++) { - qseg = *seg; - if (qseg->UpdateAngle(m_jacobian, delta, clamp)) { - for (i = 0; i < qseg->NumberOfDoF(); i++) { - if (clamp[i] && !qseg->Locked(i)) { - absdelta = fabs(delta[i]); - - if (absdelta < IK_EPSILON) { - qseg->Lock(i, m_jacobian, delta); - locked = true; - } - else if (absdelta < minabsdelta) { - minabsdelta = absdelta; - mindelta = delta; - minseg = qseg; - mindof = i; - } - } - } - } - } - - // lock most violating angle - if (minseg) { - minseg->Lock(mindof, m_jacobian, mindelta); - locked = true; - - if (minabsdelta > norm) - norm = minabsdelta; - } - - if (locked == false) - // no locking done, last inner iteration, apply the angles - for (seg = m_segments.begin(); seg != m_segments.end(); seg++) { - (*seg)->UnLock(); - (*seg)->UpdateAngleApply(); - } - - // signal if another inner iteration is needed - return locked; + // assing each segment a unique id for the jacobian + std::vector<IK_QSegment *>::iterator seg; + IK_QSegment *qseg, *minseg = NULL; + double minabsdelta = 1e10, absdelta; + Vector3d delta, mindelta; + bool locked = false, clamp[3]; + int i, mindof = 0; + + // here we check if any angle limits were violated. angles whose clamped + // position is the same as it was before, are locked immediate. of the + // other violation angles the most violating angle is rememberd + for (seg = m_segments.begin(); seg != m_segments.end(); seg++) { + qseg = *seg; + if (qseg->UpdateAngle(m_jacobian, delta, clamp)) { + for (i = 0; i < qseg->NumberOfDoF(); i++) { + if (clamp[i] && !qseg->Locked(i)) { + absdelta = fabs(delta[i]); + + if (absdelta < IK_EPSILON) { + qseg->Lock(i, m_jacobian, delta); + locked = true; + } + else if (absdelta < minabsdelta) { + minabsdelta = absdelta; + mindelta = delta; + minseg = qseg; + mindof = i; + } + } + } + } + } + + // lock most violating angle + if (minseg) { + minseg->Lock(mindof, m_jacobian, mindelta); + locked = true; + + if (minabsdelta > norm) + norm = minabsdelta; + } + + if (locked == false) + // no locking done, last inner iteration, apply the angles + for (seg = m_segments.begin(); seg != m_segments.end(); seg++) { + (*seg)->UnLock(); + (*seg)->UpdateAngleApply(); + } + + // signal if another inner iteration is needed + return locked; } -bool IK_QJacobianSolver::Solve( - IK_QSegment *root, - std::list<IK_QTask *> tasks, - const double, - const int max_iterations - ) +bool IK_QJacobianSolver::Solve(IK_QSegment *root, + std::list<IK_QTask *> tasks, + const double, + const int max_iterations) { - float scale = ComputeScale(); - bool solved = false; - //double dt = analyze_time(); + float scale = ComputeScale(); + bool solved = false; + //double dt = analyze_time(); - Scale(scale, tasks); + Scale(scale, tasks); - ConstrainPoleVector(root, tasks); + ConstrainPoleVector(root, tasks); - root->UpdateTransform(m_rootmatrix); + root->UpdateTransform(m_rootmatrix); - // iterate - for (int iterations = 0; iterations < max_iterations; iterations++) { - // update transform - root->UpdateTransform(m_rootmatrix); + // iterate + for (int iterations = 0; iterations < max_iterations; iterations++) { + // update transform + root->UpdateTransform(m_rootmatrix); - std::list<IK_QTask *>::iterator task; + std::list<IK_QTask *>::iterator task; - // compute jacobian - for (task = tasks.begin(); task != tasks.end(); task++) { - if ((*task)->Primary()) - (*task)->ComputeJacobian(m_jacobian); - else - (*task)->ComputeJacobian(m_jacobian_sub); - } + // compute jacobian + for (task = tasks.begin(); task != tasks.end(); task++) { + if ((*task)->Primary()) + (*task)->ComputeJacobian(m_jacobian); + else + (*task)->ComputeJacobian(m_jacobian_sub); + } - double norm = 0.0; + double norm = 0.0; - do { - // invert jacobian - try { - m_jacobian.Invert(); - if (m_secondary_enabled) - m_jacobian.SubTask(m_jacobian_sub); - } - catch (...) { - fprintf(stderr, "IK Exception\n"); - return false; - } + do { + // invert jacobian + try { + m_jacobian.Invert(); + if (m_secondary_enabled) + m_jacobian.SubTask(m_jacobian_sub); + } + catch (...) { + fprintf(stderr, "IK Exception\n"); + return false; + } - // update angles and check limits - } while (UpdateAngles(norm)); + // update angles and check limits + } while (UpdateAngles(norm)); - // unlock segments again after locking in clamping loop - std::vector<IK_QSegment *>::iterator seg; - for (seg = m_segments.begin(); seg != m_segments.end(); seg++) - (*seg)->UnLock(); + // unlock segments again after locking in clamping loop + std::vector<IK_QSegment *>::iterator seg; + for (seg = m_segments.begin(); seg != m_segments.end(); seg++) + (*seg)->UnLock(); - // compute angle update norm - double maxnorm = m_jacobian.AngleUpdateNorm(); - if (maxnorm > norm) - norm = maxnorm; + // compute angle update norm + double maxnorm = m_jacobian.AngleUpdateNorm(); + if (maxnorm > norm) + norm = maxnorm; - // check for convergence - if (norm < 1e-3 && iterations > 10) { - solved = true; - break; - } - } + // check for convergence + if (norm < 1e-3 && iterations > 10) { + solved = true; + break; + } + } - if (m_poleconstraint) - root->PrependBasis(m_rootmatrix.linear()); + if (m_poleconstraint) + root->PrependBasis(m_rootmatrix.linear()); - Scale(1.0f / scale, tasks); + Scale(1.0f / scale, tasks); - //analyze_add_run(max_iterations, analyze_time()-dt); + //analyze_add_run(max_iterations, analyze_time()-dt); - return solved; + return solved; } - diff --git a/intern/iksolver/intern/IK_QJacobianSolver.h b/intern/iksolver/intern/IK_QJacobianSolver.h index 5434edf28b8..1ba3a1bebe1 100644 --- a/intern/iksolver/intern/IK_QJacobianSolver.h +++ b/intern/iksolver/intern/IK_QJacobianSolver.h @@ -36,52 +36,52 @@ #include "IK_QSegment.h" #include "IK_QTask.h" -class IK_QJacobianSolver -{ -public: - IK_QJacobianSolver(); - ~IK_QJacobianSolver() {} - - // setup pole vector constraint - void SetPoleVectorConstraint(IK_QSegment *tip, Vector3d& goal, - Vector3d& polegoal, float poleangle, bool getangle); - float GetPoleAngle() { return m_poleangle; } - - // call setup once before solving, if it fails don't solve - bool Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks); - - // returns true if converged, false if max number of iterations was used - bool Solve( - IK_QSegment *root, - std::list<IK_QTask*> tasks, - const double tolerance, - const int max_iterations - ); - -private: - void AddSegmentList(IK_QSegment *seg); - bool UpdateAngles(double& norm); - void ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask*>& tasks); - - double ComputeScale(); - void Scale(double scale, std::list<IK_QTask*>& tasks); - -private: - - IK_QJacobian m_jacobian; - IK_QJacobian m_jacobian_sub; - - bool m_secondary_enabled; - - std::vector<IK_QSegment*> m_segments; - - Affine3d m_rootmatrix; - - bool m_poleconstraint; - bool m_getpoleangle; - Vector3d m_goal; - Vector3d m_polegoal; - float m_poleangle; - IK_QSegment *m_poletip; +class IK_QJacobianSolver { + public: + IK_QJacobianSolver(); + ~IK_QJacobianSolver() + { + } + + // setup pole vector constraint + void SetPoleVectorConstraint( + IK_QSegment *tip, Vector3d &goal, Vector3d &polegoal, float poleangle, bool getangle); + float GetPoleAngle() + { + return m_poleangle; + } + + // call setup once before solving, if it fails don't solve + bool Setup(IK_QSegment *root, std::list<IK_QTask *> &tasks); + + // returns true if converged, false if max number of iterations was used + bool Solve(IK_QSegment *root, + std::list<IK_QTask *> tasks, + const double tolerance, + const int max_iterations); + + private: + void AddSegmentList(IK_QSegment *seg); + bool UpdateAngles(double &norm); + void ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *> &tasks); + + double ComputeScale(); + void Scale(double scale, std::list<IK_QTask *> &tasks); + + private: + IK_QJacobian m_jacobian; + IK_QJacobian m_jacobian_sub; + + bool m_secondary_enabled; + + std::vector<IK_QSegment *> m_segments; + + Affine3d m_rootmatrix; + + bool m_poleconstraint; + bool m_getpoleangle; + Vector3d m_goal; + Vector3d m_polegoal; + float m_poleangle; + IK_QSegment *m_poletip; }; - diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp index 09bceee0600..89a1afaafbd 100644 --- a/intern/iksolver/intern/IK_QSegment.cpp +++ b/intern/iksolver/intern/IK_QSegment.cpp @@ -21,837 +21,848 @@ * \ingroup iksolver */ - #include "IK_QSegment.h" // IK_QSegment IK_QSegment::IK_QSegment(int num_DoF, bool translational) - : m_parent(NULL), m_child(NULL), m_sibling(NULL), m_composite(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; + m_locked[0] = m_locked[1] = m_locked[2] = false; + m_weight[0] = m_weight[1] = m_weight[2] = 1.0; - m_max_extension = 0.0; + m_max_extension = 0.0; - m_start = Vector3d(0, 0, 0); - m_rest_basis.setIdentity(); - m_basis.setIdentity(); - m_translation = Vector3d(0, 0, 0); + m_start = Vector3d(0, 0, 0); + m_rest_basis.setIdentity(); + m_basis.setIdentity(); + m_translation = Vector3d(0, 0, 0); - m_orig_basis = m_basis; - m_orig_translation = m_translation; + m_orig_basis = m_basis; + m_orig_translation = m_translation; } void IK_QSegment::Reset() { - m_locked[0] = m_locked[1] = m_locked[2] = false; + m_locked[0] = m_locked[1] = m_locked[2] = false; - m_basis = m_orig_basis; - m_translation = m_orig_translation; - SetBasis(m_basis); + 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(); + for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) + seg->Reset(); } -void IK_QSegment::SetTransform( - const Vector3d& start, - const Matrix3d& rest_basis, - const Matrix3d& basis, - const double length - ) +void IK_QSegment::SetTransform(const Vector3d &start, + const Matrix3d &rest_basis, + const Matrix3d &basis, + const double length) { - m_max_extension = start.norm() + length; + m_max_extension = start.norm() + length; - m_start = start; - m_rest_basis = rest_basis; + m_start = start; + m_rest_basis = rest_basis; - m_orig_basis = basis; - SetBasis(basis); + m_orig_basis = basis; + SetBasis(basis); - m_translation = Vector3d(0, length, 0); - m_orig_translation = m_translation; + m_translation = Vector3d(0, length, 0); + m_orig_translation = m_translation; } Matrix3d IK_QSegment::BasisChange() const { - return m_orig_basis.transpose() * m_basis; + return m_orig_basis.transpose() * m_basis; } Vector3d IK_QSegment::TranslationChange() const { - return m_translation - m_orig_translation; + return m_translation - m_orig_translation; } IK_QSegment::~IK_QSegment() { - if (m_parent) - m_parent->RemoveChild(this); + if (m_parent) + m_parent->RemoveChild(this); - for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) - seg->m_parent = NULL; + for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) + seg->m_parent = NULL; } void IK_QSegment::SetParent(IK_QSegment *parent) { - if (m_parent == parent) - return; - - if (m_parent) - m_parent->RemoveChild(this); - - if (parent) { - m_sibling = parent->m_child; - parent->m_child = this; - } + if (m_parent == parent) + return; + + if (m_parent) + m_parent->RemoveChild(this); - m_parent = parent; + if (parent) { + m_sibling = parent->m_child; + parent->m_child = this; + } + + m_parent = parent; } void IK_QSegment::SetComposite(IK_QSegment *seg) { - m_composite = seg; + m_composite = seg; } void IK_QSegment::RemoveChild(IK_QSegment *child) { - if (m_child == NULL) - return; - else if (m_child == child) - m_child = m_child->m_sibling; - else { - IK_QSegment *seg = m_child; + if (m_child == NULL) + return; + else if (m_child == child) + m_child = m_child->m_sibling; + else { + IK_QSegment *seg = m_child; - while (seg->m_sibling != child) - seg = seg->m_sibling; + while (seg->m_sibling != child) + seg = seg->m_sibling; - if (child == seg->m_sibling) - seg->m_sibling = child->m_sibling; - } + if (child == seg->m_sibling) + seg->m_sibling = child->m_sibling; + } } -void IK_QSegment::UpdateTransform(const Affine3d& global) +void IK_QSegment::UpdateTransform(const Affine3d &global) { - // compute the global transform at the end of the segment - m_global_start = global.translation() + global.linear() * m_start; + // compute the global transform at the end of the segment + m_global_start = global.translation() + global.linear() * m_start; - m_global_transform.translation() = m_global_start; - m_global_transform.linear() = global.linear() * m_rest_basis * m_basis; - m_global_transform.translate(m_translation); + m_global_transform.translation() = m_global_start; + m_global_transform.linear() = global.linear() * 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); + // update child transforms + for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) + seg->UpdateTransform(m_global_transform); } -void IK_QSegment::PrependBasis(const Matrix3d& mat) +void IK_QSegment::PrependBasis(const Matrix3d &mat) { - m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis; + m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis; } void IK_QSegment::Scale(double scale) { - m_start *= scale; - m_translation *= scale; - m_orig_translation *= scale; - m_global_start *= scale; - m_global_transform.translation() *= scale; - m_max_extension *= scale; + m_start *= scale; + m_translation *= scale; + m_orig_translation *= scale; + m_global_start *= scale; + m_global_transform.translation() *= scale; + m_max_extension *= scale; } // IK_QSphericalSegment IK_QSphericalSegment::IK_QSphericalSegment() - : IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false) + : IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false) { } Vector3d IK_QSphericalSegment::Axis(int dof) const { - return m_global_transform.linear().col(dof); + return m_global_transform.linear().col(dof); } void IK_QSphericalSegment::SetLimit(int axis, double lmin, double lmax) { - if (lmin > lmax) - return; - - if (axis == 1) { - lmin = Clamp(lmin, -M_PI, M_PI); - lmax = Clamp(lmax, -M_PI, M_PI); - - m_min_y = lmin; - m_max_y = lmax; - - m_limit_y = true; - } - else { - // clamp and convert to axis angle parameters - lmin = Clamp(lmin, -M_PI, M_PI); - lmax = Clamp(lmax, -M_PI, M_PI); - - lmin = sin(lmin * 0.5); - lmax = sin(lmax * 0.5); - - if (axis == 0) { - m_min[0] = -lmax; - m_max[0] = -lmin; - m_limit_x = true; - } - else if (axis == 2) { - m_min[1] = -lmax; - m_max[1] = -lmin; - m_limit_z = true; - } - } + if (lmin > lmax) + return; + + if (axis == 1) { + lmin = Clamp(lmin, -M_PI, M_PI); + lmax = Clamp(lmax, -M_PI, M_PI); + + m_min_y = lmin; + m_max_y = lmax; + + m_limit_y = true; + } + else { + // clamp and convert to axis angle parameters + lmin = Clamp(lmin, -M_PI, M_PI); + lmax = Clamp(lmax, -M_PI, M_PI); + + lmin = sin(lmin * 0.5); + lmax = sin(lmax * 0.5); + + if (axis == 0) { + m_min[0] = -lmax; + m_max[0] = -lmin; + m_limit_x = true; + } + else if (axis == 2) { + m_min[1] = -lmax; + m_max[1] = -lmin; + m_limit_z = true; + } + } } void IK_QSphericalSegment::SetWeight(int axis, double weight) { - m_weight[axis] = weight; -} - -bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp) -{ - if (m_locked[0] && m_locked[1] && m_locked[2]) - return false; - - Vector3d dq; - dq.x() = jacobian.AngleUpdate(m_DoF_id); - dq.y() = jacobian.AngleUpdate(m_DoF_id + 1); - dq.z() = jacobian.AngleUpdate(m_DoF_id + 2); - - // Directly update the rotation matrix, with Rodrigues' rotation formula, - // to avoid singularities and allow smooth integration. - - double theta = dq.norm(); - - if (!FuzzyZero(theta)) { - Vector3d w = dq * (1.0 / theta); - - double sine = sin(theta); - double cosine = cos(theta); - double cosineInv = 1 - cosine; - - double xsine = w.x() * sine; - double ysine = w.y() * sine; - double zsine = w.z() * sine; - - double xxcosine = w.x() * w.x() * cosineInv; - double xycosine = w.x() * w.y() * cosineInv; - double xzcosine = w.x() * w.z() * cosineInv; - double yycosine = w.y() * w.y() * cosineInv; - double yzcosine = w.y() * w.z() * cosineInv; - double zzcosine = w.z() * w.z() * cosineInv; - - Matrix3d M = CreateMatrix( - cosine + xxcosine, -zsine + xycosine, ysine + xzcosine, - zsine + xycosine, cosine + yycosine, -xsine + yzcosine, - -ysine + xzcosine, xsine + yzcosine, cosine + zzcosine); - - m_new_basis = m_basis * M; - } - else - m_new_basis = m_basis; - - - if (m_limit_y == false && m_limit_x == false && m_limit_z == false) - return false; - - Vector3d a = SphericalRangeParameters(m_new_basis); - - if (m_locked[0]) - a.x() = m_locked_ax; - if (m_locked[1]) - a.y() = m_locked_ay; - if (m_locked[2]) - a.z() = m_locked_az; - - double ax = a.x(), ay = a.y(), az = a.z(); - - clamp[0] = clamp[1] = clamp[2] = false; - - if (m_limit_y) { - if (a.y() > m_max_y) { - ay = m_max_y; - clamp[1] = true; - } - else if (a.y() < m_min_y) { - ay = m_min_y; - clamp[1] = true; - } - } - - if (m_limit_x && m_limit_z) { - if (EllipseClamp(ax, az, m_min, m_max)) - clamp[0] = clamp[2] = true; - } - else if (m_limit_x) { - if (ax < m_min[0]) { - ax = m_min[0]; - clamp[0] = true; - } - else if (ax > m_max[0]) { - ax = m_max[0]; - clamp[0] = true; - } - } - else if (m_limit_z) { - if (az < m_min[1]) { - az = m_min[1]; - clamp[2] = true; - } - else if (az > m_max[1]) { - az = m_max[1]; - clamp[2] = true; - } - } - - if (clamp[0] == false && clamp[1] == false && clamp[2] == false) { - if (m_locked[0] || m_locked[1] || m_locked[2]) - m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay); - return false; - } - - m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay); - - delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis); - - if (!(m_locked[0] || m_locked[2]) && (clamp[0] || clamp[2])) { - m_locked_ax = ax; - m_locked_az = az; - } - - if (!m_locked[1] && clamp[1]) - m_locked_ay = ay; - - return true; -} - -void IK_QSphericalSegment::Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta) -{ - if (dof == 1) { - m_locked[1] = true; - jacobian.Lock(m_DoF_id + 1, delta[1]); - } - else { - m_locked[0] = m_locked[2] = true; - jacobian.Lock(m_DoF_id, delta[0]); - jacobian.Lock(m_DoF_id + 2, delta[2]); - } + m_weight[axis] = weight; +} + +bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) +{ + if (m_locked[0] && m_locked[1] && m_locked[2]) + return false; + + Vector3d dq; + dq.x() = jacobian.AngleUpdate(m_DoF_id); + dq.y() = jacobian.AngleUpdate(m_DoF_id + 1); + dq.z() = jacobian.AngleUpdate(m_DoF_id + 2); + + // Directly update the rotation matrix, with Rodrigues' rotation formula, + // to avoid singularities and allow smooth integration. + + double theta = dq.norm(); + + if (!FuzzyZero(theta)) { + Vector3d w = dq * (1.0 / theta); + + double sine = sin(theta); + double cosine = cos(theta); + double cosineInv = 1 - cosine; + + double xsine = w.x() * sine; + double ysine = w.y() * sine; + double zsine = w.z() * sine; + + double xxcosine = w.x() * w.x() * cosineInv; + double xycosine = w.x() * w.y() * cosineInv; + double xzcosine = w.x() * w.z() * cosineInv; + double yycosine = w.y() * w.y() * cosineInv; + double yzcosine = w.y() * w.z() * cosineInv; + double zzcosine = w.z() * w.z() * cosineInv; + + Matrix3d M = CreateMatrix(cosine + xxcosine, + -zsine + xycosine, + ysine + xzcosine, + zsine + xycosine, + cosine + yycosine, + -xsine + yzcosine, + -ysine + xzcosine, + xsine + yzcosine, + cosine + zzcosine); + + m_new_basis = m_basis * M; + } + else + m_new_basis = m_basis; + + if (m_limit_y == false && m_limit_x == false && m_limit_z == false) + return false; + + Vector3d a = SphericalRangeParameters(m_new_basis); + + if (m_locked[0]) + a.x() = m_locked_ax; + if (m_locked[1]) + a.y() = m_locked_ay; + if (m_locked[2]) + a.z() = m_locked_az; + + double ax = a.x(), ay = a.y(), az = a.z(); + + clamp[0] = clamp[1] = clamp[2] = false; + + if (m_limit_y) { + if (a.y() > m_max_y) { + ay = m_max_y; + clamp[1] = true; + } + else if (a.y() < m_min_y) { + ay = m_min_y; + clamp[1] = true; + } + } + + if (m_limit_x && m_limit_z) { + if (EllipseClamp(ax, az, m_min, m_max)) + clamp[0] = clamp[2] = true; + } + else if (m_limit_x) { + if (ax < m_min[0]) { + ax = m_min[0]; + clamp[0] = true; + } + else if (ax > m_max[0]) { + ax = m_max[0]; + clamp[0] = true; + } + } + else if (m_limit_z) { + if (az < m_min[1]) { + az = m_min[1]; + clamp[2] = true; + } + else if (az > m_max[1]) { + az = m_max[1]; + clamp[2] = true; + } + } + + if (clamp[0] == false && clamp[1] == false && clamp[2] == false) { + if (m_locked[0] || m_locked[1] || m_locked[2]) + m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay); + return false; + } + + m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay); + + delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis); + + if (!(m_locked[0] || m_locked[2]) && (clamp[0] || clamp[2])) { + m_locked_ax = ax; + m_locked_az = az; + } + + if (!m_locked[1] && clamp[1]) + m_locked_ay = ay; + + return true; +} + +void IK_QSphericalSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) +{ + if (dof == 1) { + m_locked[1] = true; + jacobian.Lock(m_DoF_id + 1, delta[1]); + } + else { + m_locked[0] = m_locked[2] = true; + jacobian.Lock(m_DoF_id, delta[0]); + jacobian.Lock(m_DoF_id + 2, delta[2]); + } } void IK_QSphericalSegment::UpdateAngleApply() { - m_basis = m_new_basis; + m_basis = m_new_basis; } // IK_QNullSegment -IK_QNullSegment::IK_QNullSegment() - : IK_QSegment(0, false) +IK_QNullSegment::IK_QNullSegment() : IK_QSegment(0, false) { } // IK_QRevoluteSegment IK_QRevoluteSegment::IK_QRevoluteSegment(int axis) - : IK_QSegment(1, false), m_axis(axis), m_angle(0.0), m_limit(false) + : IK_QSegment(1, false), m_axis(axis), m_angle(0.0), m_limit(false) { } -void IK_QRevoluteSegment::SetBasis(const Matrix3d& basis) +void IK_QRevoluteSegment::SetBasis(const Matrix3d &basis) { - if (m_axis == 1) { - m_angle = ComputeTwist(basis); - m_basis = ComputeTwistMatrix(m_angle); - } - else { - m_angle = EulerAngleFromMatrix(basis, m_axis); - m_basis = RotationMatrix(m_angle, m_axis); - } + if (m_axis == 1) { + m_angle = ComputeTwist(basis); + m_basis = ComputeTwistMatrix(m_angle); + } + else { + m_angle = EulerAngleFromMatrix(basis, m_axis); + m_basis = RotationMatrix(m_angle, m_axis); + } } Vector3d IK_QRevoluteSegment::Axis(int) const { - return m_global_transform.linear().col(m_axis); + return m_global_transform.linear().col(m_axis); } -bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp) +bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) { - if (m_locked[0]) - return false; + if (m_locked[0]) + return false; + + m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id); - m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id); + clamp[0] = false; - clamp[0] = false; + if (m_limit == false) + return false; - if (m_limit == false) - return false; + if (m_new_angle > m_max) + delta[0] = m_max - m_angle; + else if (m_new_angle < m_min) + delta[0] = m_min - m_angle; + else + return false; - if (m_new_angle > m_max) - delta[0] = m_max - m_angle; - else if (m_new_angle < m_min) - delta[0] = m_min - m_angle; - else - return false; - - clamp[0] = true; - m_new_angle = m_angle + delta[0]; + clamp[0] = true; + m_new_angle = m_angle + delta[0]; - return true; + return true; } -void IK_QRevoluteSegment::Lock(int, IK_QJacobian& jacobian, Vector3d& delta) +void IK_QRevoluteSegment::Lock(int, IK_QJacobian &jacobian, Vector3d &delta) { - m_locked[0] = true; - jacobian.Lock(m_DoF_id, delta[0]); + m_locked[0] = true; + jacobian.Lock(m_DoF_id, delta[0]); } void IK_QRevoluteSegment::UpdateAngleApply() { - m_angle = m_new_angle; - m_basis = RotationMatrix(m_angle, m_axis); + m_angle = m_new_angle; + m_basis = RotationMatrix(m_angle, m_axis); } void IK_QRevoluteSegment::SetLimit(int axis, double lmin, double lmax) { - if (lmin > lmax || m_axis != axis) - return; - - // clamp and convert to axis angle parameters - lmin = Clamp(lmin, -M_PI, M_PI); - lmax = Clamp(lmax, -M_PI, M_PI); + if (lmin > lmax || m_axis != axis) + return; + + // clamp and convert to axis angle parameters + lmin = Clamp(lmin, -M_PI, M_PI); + lmax = Clamp(lmax, -M_PI, M_PI); - m_min = lmin; - m_max = lmax; + m_min = lmin; + m_max = lmax; - m_limit = true; + m_limit = true; } void IK_QRevoluteSegment::SetWeight(int axis, double weight) { - if (axis == m_axis) - m_weight[0] = weight; + if (axis == m_axis) + m_weight[0] = weight; } // IK_QSwingSegment -IK_QSwingSegment::IK_QSwingSegment() - : IK_QSegment(2, false), m_limit_x(false), m_limit_z(false) +IK_QSwingSegment::IK_QSwingSegment() : IK_QSegment(2, false), m_limit_x(false), m_limit_z(false) { } -void IK_QSwingSegment::SetBasis(const Matrix3d& basis) +void IK_QSwingSegment::SetBasis(const Matrix3d &basis) { - m_basis = basis; - RemoveTwist(m_basis); + m_basis = basis; + RemoveTwist(m_basis); } Vector3d IK_QSwingSegment::Axis(int dof) const { - return m_global_transform.linear().col((dof == 0) ? 0 : 2); + return m_global_transform.linear().col((dof == 0) ? 0 : 2); } -bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp) +bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) { - if (m_locked[0] && m_locked[1]) - return false; + if (m_locked[0] && m_locked[1]) + return false; - Vector3d dq; - dq.x() = jacobian.AngleUpdate(m_DoF_id); - dq.y() = 0.0; - dq.z() = jacobian.AngleUpdate(m_DoF_id + 1); + Vector3d dq; + dq.x() = jacobian.AngleUpdate(m_DoF_id); + dq.y() = 0.0; + dq.z() = jacobian.AngleUpdate(m_DoF_id + 1); - // Directly update the rotation matrix, with Rodrigues' rotation formula, - // to avoid singularities and allow smooth integration. + // Directly update the rotation matrix, with Rodrigues' rotation formula, + // to avoid singularities and allow smooth integration. - double theta = dq.norm(); + double theta = dq.norm(); - if (!FuzzyZero(theta)) { - Vector3d w = dq * (1.0 / theta); + if (!FuzzyZero(theta)) { + Vector3d w = dq * (1.0 / theta); - double sine = sin(theta); - double cosine = cos(theta); - double cosineInv = 1 - cosine; + double sine = sin(theta); + double cosine = cos(theta); + double cosineInv = 1 - cosine; - double xsine = w.x() * sine; - double zsine = w.z() * sine; + double xsine = w.x() * sine; + double zsine = w.z() * sine; - double xxcosine = w.x() * w.x() * cosineInv; - double xzcosine = w.x() * w.z() * cosineInv; - double zzcosine = w.z() * w.z() * cosineInv; + double xxcosine = w.x() * w.x() * cosineInv; + double xzcosine = w.x() * w.z() * cosineInv; + double zzcosine = w.z() * w.z() * cosineInv; - Matrix3d M = CreateMatrix( - cosine + xxcosine, -zsine, xzcosine, - zsine, cosine, -xsine, - xzcosine, xsine, cosine + zzcosine); + Matrix3d M = CreateMatrix(cosine + xxcosine, + -zsine, + xzcosine, + zsine, + cosine, + -xsine, + xzcosine, + xsine, + cosine + zzcosine); - m_new_basis = m_basis * M; + m_new_basis = m_basis * M; - RemoveTwist(m_new_basis); - } - else - m_new_basis = m_basis; + RemoveTwist(m_new_basis); + } + else + m_new_basis = m_basis; - if (m_limit_x == false && m_limit_z == false) - return false; + if (m_limit_x == false && m_limit_z == false) + return false; - Vector3d a = SphericalRangeParameters(m_new_basis); - double ax = 0, az = 0; + Vector3d a = SphericalRangeParameters(m_new_basis); + double ax = 0, az = 0; - clamp[0] = clamp[1] = false; - - if (m_limit_x && m_limit_z) { - ax = a.x(); - az = a.z(); + clamp[0] = clamp[1] = false; - if (EllipseClamp(ax, az, m_min, m_max)) - clamp[0] = clamp[1] = true; - } - else if (m_limit_x) { - if (ax < m_min[0]) { - ax = m_min[0]; - clamp[0] = true; - } - else if (ax > m_max[0]) { - ax = m_max[0]; - clamp[0] = true; - } - } - else if (m_limit_z) { - if (az < m_min[1]) { - az = m_min[1]; - clamp[1] = true; - } - else if (az > m_max[1]) { - az = m_max[1]; - clamp[1] = true; - } - } + if (m_limit_x && m_limit_z) { + ax = a.x(); + az = a.z(); - if (clamp[0] == false && clamp[1] == false) - return false; + if (EllipseClamp(ax, az, m_min, m_max)) + clamp[0] = clamp[1] = true; + } + else if (m_limit_x) { + if (ax < m_min[0]) { + ax = m_min[0]; + clamp[0] = true; + } + else if (ax > m_max[0]) { + ax = m_max[0]; + clamp[0] = true; + } + } + else if (m_limit_z) { + if (az < m_min[1]) { + az = m_min[1]; + clamp[1] = true; + } + else if (az > m_max[1]) { + az = m_max[1]; + clamp[1] = true; + } + } - m_new_basis = ComputeSwingMatrix(ax, az); + if (clamp[0] == false && clamp[1] == false) + return false; - delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis); - delta[1] = delta[2]; delta[2] = 0.0; + m_new_basis = ComputeSwingMatrix(ax, az); - return true; + delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis); + delta[1] = delta[2]; + delta[2] = 0.0; + + return true; } -void IK_QSwingSegment::Lock(int, IK_QJacobian& jacobian, Vector3d& delta) +void IK_QSwingSegment::Lock(int, IK_QJacobian &jacobian, Vector3d &delta) { - m_locked[0] = m_locked[1] = true; - jacobian.Lock(m_DoF_id, delta[0]); - jacobian.Lock(m_DoF_id + 1, delta[1]); + m_locked[0] = m_locked[1] = true; + jacobian.Lock(m_DoF_id, delta[0]); + jacobian.Lock(m_DoF_id + 1, delta[1]); } void IK_QSwingSegment::UpdateAngleApply() { - m_basis = m_new_basis; + m_basis = m_new_basis; } void IK_QSwingSegment::SetLimit(int axis, double lmin, double lmax) { - if (lmin > lmax) - return; - - // clamp and convert to axis angle parameters - lmin = Clamp(lmin, -M_PI, M_PI); - lmax = Clamp(lmax, -M_PI, M_PI); + if (lmin > lmax) + return; + + // clamp and convert to axis angle parameters + lmin = Clamp(lmin, -M_PI, M_PI); + lmax = Clamp(lmax, -M_PI, M_PI); - lmin = sin(lmin * 0.5); - lmax = sin(lmax * 0.5); + lmin = sin(lmin * 0.5); + lmax = sin(lmax * 0.5); - // put center of ellispe in the middle between min and max - double offset = 0.5 * (lmin + lmax); - //lmax = lmax - offset; + // put center of ellispe in the middle between min and max + double offset = 0.5 * (lmin + lmax); + //lmax = lmax - offset; - if (axis == 0) { - m_min[0] = -lmax; - m_max[0] = -lmin; + 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_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; - } + m_limit_z = true; + m_offset_z = offset; + m_max_z = lmax; + } } void IK_QSwingSegment::SetWeight(int axis, double weight) { - if (axis == 0) - m_weight[0] = weight; - else if (axis == 2) - m_weight[1] = weight; + if (axis == 0) + m_weight[0] = weight; + else if (axis == 2) + m_weight[1] = weight; } // IK_QElbowSegment IK_QElbowSegment::IK_QElbowSegment(int axis) - : IK_QSegment(2, false), m_axis(axis), m_twist(0.0), m_angle(0.0), - m_cos_twist(1.0), m_sin_twist(0.0), m_limit(false), m_limit_twist(false) + : IK_QSegment(2, false), + m_axis(axis), + m_twist(0.0), + m_angle(0.0), + m_cos_twist(1.0), + m_sin_twist(0.0), + m_limit(false), + m_limit_twist(false) { } -void IK_QElbowSegment::SetBasis(const Matrix3d& basis) +void IK_QElbowSegment::SetBasis(const Matrix3d &basis) { - m_basis = basis; + m_basis = basis; - m_twist = ComputeTwist(m_basis); - RemoveTwist(m_basis); - m_angle = EulerAngleFromMatrix(basis, m_axis); + m_twist = ComputeTwist(m_basis); + RemoveTwist(m_basis); + m_angle = EulerAngleFromMatrix(basis, m_axis); - m_basis = RotationMatrix(m_angle, m_axis) * ComputeTwistMatrix(m_twist); + m_basis = RotationMatrix(m_angle, m_axis) * ComputeTwistMatrix(m_twist); } Vector3d IK_QElbowSegment::Axis(int dof) const { - if (dof == 0) { - Vector3d v; - if (m_axis == 0) - v = Vector3d(m_cos_twist, 0, m_sin_twist); - else - v = Vector3d(-m_sin_twist, 0, m_cos_twist); - - return m_global_transform.linear() * v; - } - else - return m_global_transform.linear().col(1); -} - -bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp) -{ - if (m_locked[0] && m_locked[1]) - return false; - - clamp[0] = clamp[1] = false; - - if (!m_locked[0]) { - m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id); - - if (m_limit) { - if (m_new_angle > m_max) { - delta[0] = m_max - m_angle; - m_new_angle = m_max; - clamp[0] = true; - } - else if (m_new_angle < m_min) { - delta[0] = m_min - m_angle; - m_new_angle = m_min; - clamp[0] = true; - } - } - } - - if (!m_locked[1]) { - m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id + 1); - - if (m_limit_twist) { - if (m_new_twist > m_max_twist) { - delta[1] = m_max_twist - m_twist; - m_new_twist = m_max_twist; - clamp[1] = true; - } - else if (m_new_twist < m_min_twist) { - delta[1] = m_min_twist - m_twist; - m_new_twist = m_min_twist; - clamp[1] = true; - } - } - } - - return (clamp[0] || clamp[1]); -} - -void IK_QElbowSegment::Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta) -{ - if (dof == 0) { - m_locked[0] = true; - jacobian.Lock(m_DoF_id, delta[0]); - } - else { - m_locked[1] = true; - jacobian.Lock(m_DoF_id + 1, delta[1]); - } + if (dof == 0) { + Vector3d v; + if (m_axis == 0) + v = Vector3d(m_cos_twist, 0, m_sin_twist); + else + v = Vector3d(-m_sin_twist, 0, m_cos_twist); + + return m_global_transform.linear() * v; + } + else + return m_global_transform.linear().col(1); +} + +bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) +{ + if (m_locked[0] && m_locked[1]) + return false; + + clamp[0] = clamp[1] = false; + + if (!m_locked[0]) { + m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id); + + if (m_limit) { + if (m_new_angle > m_max) { + delta[0] = m_max - m_angle; + m_new_angle = m_max; + clamp[0] = true; + } + else if (m_new_angle < m_min) { + delta[0] = m_min - m_angle; + m_new_angle = m_min; + clamp[0] = true; + } + } + } + + if (!m_locked[1]) { + m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id + 1); + + if (m_limit_twist) { + if (m_new_twist > m_max_twist) { + delta[1] = m_max_twist - m_twist; + m_new_twist = m_max_twist; + clamp[1] = true; + } + else if (m_new_twist < m_min_twist) { + delta[1] = m_min_twist - m_twist; + m_new_twist = m_min_twist; + clamp[1] = true; + } + } + } + + return (clamp[0] || clamp[1]); +} + +void IK_QElbowSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) +{ + if (dof == 0) { + m_locked[0] = true; + jacobian.Lock(m_DoF_id, delta[0]); + } + else { + m_locked[1] = true; + jacobian.Lock(m_DoF_id + 1, delta[1]); + } } void IK_QElbowSegment::UpdateAngleApply() { - m_angle = m_new_angle; - m_twist = m_new_twist; + m_angle = m_new_angle; + m_twist = m_new_twist; - m_sin_twist = sin(m_twist); - m_cos_twist = cos(m_twist); + m_sin_twist = sin(m_twist); + m_cos_twist = cos(m_twist); - Matrix3d A = RotationMatrix(m_angle, m_axis); - Matrix3d T = RotationMatrix(m_sin_twist, m_cos_twist, 1); + Matrix3d A = RotationMatrix(m_angle, m_axis); + Matrix3d T = RotationMatrix(m_sin_twist, m_cos_twist, 1); - m_basis = A * T; + m_basis = A * T; } void IK_QElbowSegment::SetLimit(int axis, double lmin, double lmax) { - if (lmin > lmax) - return; + if (lmin > lmax) + return; - // clamp and convert to axis angle parameters - lmin = Clamp(lmin, -M_PI, M_PI); - lmax = Clamp(lmax, -M_PI, M_PI); + // clamp and convert to axis angle parameters + lmin = Clamp(lmin, -M_PI, M_PI); + lmax = Clamp(lmax, -M_PI, M_PI); - if (axis == 1) { - m_min_twist = lmin; - m_max_twist = lmax; - m_limit_twist = true; - } - else if (axis == m_axis) { - m_min = lmin; - m_max = lmax; - m_limit = true; - } + if (axis == 1) { + m_min_twist = lmin; + m_max_twist = lmax; + m_limit_twist = true; + } + else if (axis == m_axis) { + m_min = lmin; + m_max = lmax; + m_limit = true; + } } void IK_QElbowSegment::SetWeight(int axis, double weight) { - if (axis == m_axis) - m_weight[0] = weight; - else if (axis == 1) - m_weight[1] = weight; + if (axis == m_axis) + m_weight[0] = weight; + else if (axis == 1) + m_weight[1] = weight; } // IK_QTranslateSegment -IK_QTranslateSegment::IK_QTranslateSegment(int axis1) - : IK_QSegment(1, true) +IK_QTranslateSegment::IK_QTranslateSegment(int axis1) : IK_QSegment(1, true) { - m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false; - m_axis_enabled[axis1] = true; + m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false; + m_axis_enabled[axis1] = true; - m_axis[0] = axis1; + m_axis[0] = axis1; - m_limit[0] = m_limit[1] = m_limit[2] = false; + m_limit[0] = m_limit[1] = m_limit[2] = false; } -IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2) - : IK_QSegment(2, true) +IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2) : IK_QSegment(2, true) { - m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false; - m_axis_enabled[axis1] = true; - m_axis_enabled[axis2] = true; + m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false; + m_axis_enabled[axis1] = true; + m_axis_enabled[axis2] = true; - m_axis[0] = axis1; - m_axis[1] = axis2; + m_axis[0] = axis1; + m_axis[1] = axis2; - m_limit[0] = m_limit[1] = m_limit[2] = false; + m_limit[0] = m_limit[1] = m_limit[2] = false; } -IK_QTranslateSegment::IK_QTranslateSegment() - : IK_QSegment(3, true) +IK_QTranslateSegment::IK_QTranslateSegment() : IK_QSegment(3, true) { - m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = true; + m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = true; - m_axis[0] = 0; - m_axis[1] = 1; - m_axis[2] = 2; + m_axis[0] = 0; + m_axis[1] = 1; + m_axis[2] = 2; - m_limit[0] = m_limit[1] = m_limit[2] = false; + m_limit[0] = m_limit[1] = m_limit[2] = false; } Vector3d IK_QTranslateSegment::Axis(int dof) const { - return m_global_transform.linear().col(m_axis[dof]); + return m_global_transform.linear().col(m_axis[dof]); } -bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp) +bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) { - int dof_id = m_DoF_id, dof = 0, i, clamped = false; + int dof_id = m_DoF_id, dof = 0, i, clamped = false; - Vector3d dx(0.0, 0.0, 0.0); + Vector3d dx(0.0, 0.0, 0.0); - for (i = 0; i < 3; i++) { - if (!m_axis_enabled[i]) { - m_new_translation[i] = m_translation[i]; - continue; - } + for (i = 0; i < 3; i++) { + if (!m_axis_enabled[i]) { + m_new_translation[i] = m_translation[i]; + continue; + } - clamp[dof] = false; + clamp[dof] = false; - if (!m_locked[dof]) { - m_new_translation[i] = m_translation[i] + jacobian.AngleUpdate(dof_id); + 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; - } - } - } + 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++; - } + dof_id++; + dof++; + } - return clamped; + return clamped; } void IK_QTranslateSegment::UpdateAngleApply() { - m_translation = m_new_translation; + m_translation = m_new_translation; } -void IK_QTranslateSegment::Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta) +void IK_QTranslateSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) { - m_locked[dof] = true; - jacobian.Lock(m_DoF_id + dof, delta[dof]); + m_locked[dof] = true; + jacobian.Lock(m_DoF_id + dof, delta[dof]); } void IK_QTranslateSegment::SetWeight(int axis, double weight) { - int i; + int i; - for (i = 0; i < m_num_DoF; i++) - if (m_axis[i] == axis) - m_weight[i] = weight; + for (i = 0; i < m_num_DoF; i++) + if (m_axis[i] == axis) + m_weight[i] = weight; } void IK_QTranslateSegment::SetLimit(int axis, double lmin, double lmax) { - if (lmax < lmin) - return; + if (lmax < lmin) + return; - m_min[axis] = lmin; - m_max[axis] = lmax; - m_limit[axis] = true; + m_min[axis] = lmin; + m_max[axis] = lmax; + m_limit[axis] = true; } void IK_QTranslateSegment::Scale(double scale) { - int i; + int i; - IK_QSegment::Scale(scale); + IK_QSegment::Scale(scale); - for (i = 0; i < 3; i++) { - m_min[0] *= scale; - m_max[1] *= scale; - } + for (i = 0; i < 3; i++) { + m_min[0] *= scale; + m_max[1] *= scale; + } - m_new_translation *= scale; + m_new_translation *= scale; } - diff --git a/intern/iksolver/intern/IK_QSegment.h b/intern/iksolver/intern/IK_QSegment.h index c4576c08145..47125db3865 100644 --- a/intern/iksolver/intern/IK_QSegment.h +++ b/intern/iksolver/intern/IK_QSegment.h @@ -49,289 +49,330 @@ * use exactly the same transformations when displaying the segments */ -class IK_QSegment -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - virtual ~IK_QSegment(); - - // start: a user defined translation - // rest_basis: a user defined rotation - // basis: a user defined rotation - // length: length of this segment - - void SetTransform( - const Vector3d& start, - const Matrix3d& rest_basis, - const Matrix3d& basis, - const double length - ); - - // tree structure access - void SetParent(IK_QSegment *parent); - - IK_QSegment *Child() const - { return m_child; } - - IK_QSegment *Sibling() const - { return m_sibling; } - - 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; } - - // unique id for this segment, for identification in the jacobian - int DoFId() const - { return m_DoF_id; } - - void SetDoFId(int dof_id) - { m_DoF_id = dof_id; } - - // the max distance of the end of this bone from the local origin. - const double MaxExtension() const - { return m_max_extension; } - - // the change in rotation and translation w.r.t. the rest pose - Matrix3d BasisChange() const; - Vector3d TranslationChange() const; - - // the start and end of the segment - const Vector3d GlobalStart() const - { return m_global_start; } - - const Vector3d GlobalEnd() const - { return m_global_transform.translation(); } - - // the global transformation at the end of the segment - const Affine3d &GlobalTransform() const - { return m_global_transform; } - - // is a translational segment? - bool Translational() const - { return m_translational; } - - // locking (during inner clamping loop) - bool Locked(int dof) const - { return m_locked[dof]; } - - void UnLock() - { m_locked[0] = m_locked[1] = m_locked[2] = false; } - - // per dof joint weighting - double Weight(int dof) const - { return m_weight[dof]; } - - void ScaleWeight(int dof, double scale) - { m_weight[dof] *= scale; } - - // recursively update the global coordinates of this segment, 'global' - // is the global transformation from the parent segment - void UpdateTransform(const Affine3d &global); - - // get axis from rotation matrix for derivative computation - virtual Vector3d Axis(int dof) const=0; - - // update the angles using the dTheta's computed using the jacobian matrix - virtual bool UpdateAngle(const IK_QJacobian&, Vector3d&, bool*)=0; - virtual void Lock(int, IK_QJacobian&, Vector3d&) {} - virtual void UpdateAngleApply()=0; - - // set joint limits - virtual void SetLimit(int, double, double) {} - - // set joint weights (per axis) - virtual void SetWeight(int, double) {} - - virtual void SetBasis(const Matrix3d& basis) { m_basis = basis; } - - // functions needed for pole vector constraint - void PrependBasis(const Matrix3d& mat); - void Reset(); - - // scale - virtual void Scale(double scale); - -protected: - - // num_DoF: number of degrees of freedom - IK_QSegment(int num_DoF, bool translational); - - // remove child as a child of this segment - void RemoveChild(IK_QSegment *child); - - // tree structure variables - IK_QSegment *m_parent; - IK_QSegment *m_child; - IK_QSegment *m_sibling; - IK_QSegment *m_composite; - - // full transform = - // start * rest_basis * basis * translation - Vector3d m_start; - Matrix3d m_rest_basis; - Matrix3d m_basis; - Vector3d m_translation; - - // original basis - Matrix3d m_orig_basis; - Vector3d m_orig_translation; - - // maximum extension of this segment - double m_max_extension; - - // accumulated transformations starting from root - Vector3d m_global_start; - Affine3d m_global_transform; - - // number degrees of freedom, (first) id of this segments DOF's - int m_num_DoF, m_DoF_id; - - bool m_locked[3]; - bool m_translational; - double m_weight[3]; +class IK_QSegment { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + virtual ~IK_QSegment(); + + // start: a user defined translation + // rest_basis: a user defined rotation + // basis: a user defined rotation + // length: length of this segment + + void SetTransform(const Vector3d &start, + const Matrix3d &rest_basis, + const Matrix3d &basis, + const double length); + + // tree structure access + void SetParent(IK_QSegment *parent); + + IK_QSegment *Child() const + { + return m_child; + } + + IK_QSegment *Sibling() const + { + return m_sibling; + } + + 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; + } + + // unique id for this segment, for identification in the jacobian + int DoFId() const + { + return m_DoF_id; + } + + void SetDoFId(int dof_id) + { + m_DoF_id = dof_id; + } + + // the max distance of the end of this bone from the local origin. + const double MaxExtension() const + { + return m_max_extension; + } + + // the change in rotation and translation w.r.t. the rest pose + Matrix3d BasisChange() const; + Vector3d TranslationChange() const; + + // the start and end of the segment + const Vector3d GlobalStart() const + { + return m_global_start; + } + + const Vector3d GlobalEnd() const + { + return m_global_transform.translation(); + } + + // the global transformation at the end of the segment + const Affine3d &GlobalTransform() const + { + return m_global_transform; + } + + // is a translational segment? + bool Translational() const + { + return m_translational; + } + + // locking (during inner clamping loop) + bool Locked(int dof) const + { + return m_locked[dof]; + } + + void UnLock() + { + m_locked[0] = m_locked[1] = m_locked[2] = false; + } + + // per dof joint weighting + double Weight(int dof) const + { + return m_weight[dof]; + } + + void ScaleWeight(int dof, double scale) + { + m_weight[dof] *= scale; + } + + // recursively update the global coordinates of this segment, 'global' + // is the global transformation from the parent segment + void UpdateTransform(const Affine3d &global); + + // get axis from rotation matrix for derivative computation + virtual Vector3d Axis(int dof) const = 0; + + // update the angles using the dTheta's computed using the jacobian matrix + virtual bool UpdateAngle(const IK_QJacobian &, Vector3d &, bool *) = 0; + virtual void Lock(int, IK_QJacobian &, Vector3d &) + { + } + virtual void UpdateAngleApply() = 0; + + // set joint limits + virtual void SetLimit(int, double, double) + { + } + + // set joint weights (per axis) + virtual void SetWeight(int, double) + { + } + + virtual void SetBasis(const Matrix3d &basis) + { + m_basis = basis; + } + + // functions needed for pole vector constraint + void PrependBasis(const Matrix3d &mat); + void Reset(); + + // scale + virtual void Scale(double scale); + + protected: + // num_DoF: number of degrees of freedom + IK_QSegment(int num_DoF, bool translational); + + // remove child as a child of this segment + void RemoveChild(IK_QSegment *child); + + // tree structure variables + IK_QSegment *m_parent; + IK_QSegment *m_child; + IK_QSegment *m_sibling; + IK_QSegment *m_composite; + + // full transform = + // start * rest_basis * basis * translation + Vector3d m_start; + Matrix3d m_rest_basis; + Matrix3d m_basis; + Vector3d m_translation; + + // original basis + Matrix3d m_orig_basis; + Vector3d m_orig_translation; + + // maximum extension of this segment + double m_max_extension; + + // accumulated transformations starting from root + Vector3d m_global_start; + Affine3d m_global_transform; + + // number degrees of freedom, (first) id of this segments DOF's + int m_num_DoF, m_DoF_id; + + bool m_locked[3]; + bool m_translational; + double m_weight[3]; }; -class IK_QSphericalSegment : public IK_QSegment -{ -public: - IK_QSphericalSegment(); +class IK_QSphericalSegment : public IK_QSegment { + public: + IK_QSphericalSegment(); - Vector3d Axis(int dof) const; + Vector3d Axis(int dof) const; - bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp); - void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta); - void UpdateAngleApply(); + bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp); + void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta); + void UpdateAngleApply(); - bool ComputeClampRotation(Vector3d& clamp); + bool ComputeClampRotation(Vector3d &clamp); - void SetLimit(int axis, double lmin, double lmax); - void SetWeight(int axis, double weight); + void SetLimit(int axis, double lmin, double lmax); + void SetWeight(int axis, double weight); -private: - Matrix3d m_new_basis; - bool m_limit_x, m_limit_y, m_limit_z; - double m_min[2], m_max[2]; - double m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z; - double m_locked_ax, m_locked_ay, m_locked_az; + private: + Matrix3d m_new_basis; + bool m_limit_x, m_limit_y, m_limit_z; + double m_min[2], m_max[2]; + double m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z; + double m_locked_ax, m_locked_ay, m_locked_az; }; -class IK_QNullSegment : public IK_QSegment -{ -public: - IK_QNullSegment(); - - bool UpdateAngle(const IK_QJacobian&, Vector3d&, bool*) { return false; } - void UpdateAngleApply() {} - - Vector3d Axis(int) const { return Vector3d(0, 0, 0); } - void SetBasis(const Matrix3d&) { m_basis.setIdentity(); } +class IK_QNullSegment : public IK_QSegment { + public: + IK_QNullSegment(); + + bool UpdateAngle(const IK_QJacobian &, Vector3d &, bool *) + { + return false; + } + void UpdateAngleApply() + { + } + + Vector3d Axis(int) const + { + return Vector3d(0, 0, 0); + } + void SetBasis(const Matrix3d &) + { + m_basis.setIdentity(); + } }; -class IK_QRevoluteSegment : public IK_QSegment -{ -public: - // axis: the axis of the DoF, in range 0..2 - IK_QRevoluteSegment(int axis); +class IK_QRevoluteSegment : public IK_QSegment { + public: + // axis: the axis of the DoF, in range 0..2 + IK_QRevoluteSegment(int axis); - Vector3d Axis(int dof) const; + Vector3d Axis(int dof) const; - bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp); - void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta); - void UpdateAngleApply(); + bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp); + void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta); + void UpdateAngleApply(); - void SetLimit(int axis, double lmin, double lmax); - void SetWeight(int axis, double weight); - void SetBasis(const Matrix3d& basis); + void SetLimit(int axis, double lmin, double lmax); + void SetWeight(int axis, double weight); + void SetBasis(const Matrix3d &basis); -private: - int m_axis; - double m_angle, m_new_angle; - bool m_limit; - double m_min, m_max; + private: + int m_axis; + double m_angle, m_new_angle; + bool m_limit; + double m_min, m_max; }; -class IK_QSwingSegment : public IK_QSegment -{ -public: - // XZ DOF, uses one direct rotation - IK_QSwingSegment(); +class IK_QSwingSegment : public IK_QSegment { + public: + // XZ DOF, uses one direct rotation + IK_QSwingSegment(); - Vector3d Axis(int dof) const; + Vector3d Axis(int dof) const; - bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp); - void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta); - void UpdateAngleApply(); + bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp); + void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta); + void UpdateAngleApply(); - void SetLimit(int axis, double lmin, double lmax); - void SetWeight(int axis, double weight); - void SetBasis(const Matrix3d& basis); + void SetLimit(int axis, double lmin, double lmax); + void SetWeight(int axis, double weight); + void SetBasis(const Matrix3d &basis); -private: - Matrix3d m_new_basis; - bool m_limit_x, m_limit_z; - double m_min[2], m_max[2]; - double m_max_x, m_max_z, m_offset_x, m_offset_z; + private: + Matrix3d m_new_basis; + bool m_limit_x, m_limit_z; + double m_min[2], m_max[2]; + double m_max_x, m_max_z, m_offset_x, m_offset_z; }; -class IK_QElbowSegment : public IK_QSegment -{ -public: - // XY or ZY DOF, uses two sequential rotations: first rotate around - // X or Z, then rotate around Y (twist) - IK_QElbowSegment(int axis); +class IK_QElbowSegment : public IK_QSegment { + public: + // XY or ZY DOF, uses two sequential rotations: first rotate around + // X or Z, then rotate around Y (twist) + IK_QElbowSegment(int axis); - Vector3d Axis(int dof) const; + Vector3d Axis(int dof) const; - bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp); - void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta); - void UpdateAngleApply(); + bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp); + void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta); + void UpdateAngleApply(); - void SetLimit(int axis, double lmin, double lmax); - void SetWeight(int axis, double weight); - void SetBasis(const Matrix3d& basis); + void SetLimit(int axis, double lmin, double lmax); + void SetWeight(int axis, double weight); + void SetBasis(const Matrix3d &basis); -private: - int m_axis; + private: + int m_axis; - double m_twist, m_angle, m_new_twist, m_new_angle; - double m_cos_twist, m_sin_twist; + double m_twist, m_angle, m_new_twist, m_new_angle; + double m_cos_twist, m_sin_twist; - bool m_limit, m_limit_twist; - double m_min, m_max, m_min_twist, m_max_twist; + bool m_limit, m_limit_twist; + double m_min, m_max, m_min_twist, m_max_twist; }; -class IK_QTranslateSegment : public IK_QSegment -{ -public: - // 1DOF, 2DOF or 3DOF translational segments - IK_QTranslateSegment(int axis1); - IK_QTranslateSegment(int axis1, int axis2); - IK_QTranslateSegment(); +class IK_QTranslateSegment : public IK_QSegment { + public: + // 1DOF, 2DOF or 3DOF translational segments + IK_QTranslateSegment(int axis1); + IK_QTranslateSegment(int axis1, int axis2); + IK_QTranslateSegment(); - Vector3d Axis(int dof) const; + Vector3d Axis(int dof) const; - bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp); - void Lock(int, IK_QJacobian&, Vector3d&); - void UpdateAngleApply(); + bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp); + void Lock(int, IK_QJacobian &, Vector3d &); + void UpdateAngleApply(); - void SetWeight(int axis, double weight); - void SetLimit(int axis, double lmin, double lmax); + void SetWeight(int axis, double weight); + void SetLimit(int axis, double lmin, double lmax); - void Scale(double scale); + void Scale(double scale); -private: - int m_axis[3]; - bool m_axis_enabled[3], m_limit[3]; - Vector3d m_new_translation; - double m_min[3], m_max[3]; + private: + int m_axis[3]; + bool m_axis_enabled[3], m_limit[3]; + Vector3d m_new_translation; + double m_min[3], m_max[3]; }; - diff --git a/intern/iksolver/intern/IK_QTask.cpp b/intern/iksolver/intern/IK_QTask.cpp index c2ae46d7205..f3fe24bef18 100644 --- a/intern/iksolver/intern/IK_QTask.cpp +++ b/intern/iksolver/intern/IK_QTask.cpp @@ -21,210 +21,196 @@ * \ingroup iksolver */ - #include "IK_QTask.h" // IK_QTask -IK_QTask::IK_QTask( - int size, - bool primary, - bool active, - const IK_QSegment *segment - ) : - m_size(size), m_primary(primary), m_active(active), m_segment(segment), - m_weight(1.0) +IK_QTask::IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment) + : m_size(size), m_primary(primary), m_active(active), m_segment(segment), m_weight(1.0) { } // IK_QPositionTask -IK_QPositionTask::IK_QPositionTask( - bool primary, - const IK_QSegment *segment, - const Vector3d& goal - ) : - IK_QTask(3, primary, true, segment), m_goal(goal) +IK_QPositionTask::IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal) + : IK_QTask(3, primary, true, segment), m_goal(goal) { - // computing clamping length - int num; - const IK_QSegment *seg; + // computing clamping length + int num; + const IK_QSegment *seg; - m_clamp_length = 0.0; - num = 0; + m_clamp_length = 0.0; + num = 0; - for (seg = m_segment; seg; seg = seg->Parent()) { - m_clamp_length += seg->MaxExtension(); - num++; - } + for (seg = m_segment; seg; seg = seg->Parent()) { + m_clamp_length += seg->MaxExtension(); + num++; + } - m_clamp_length /= 2 * num; + m_clamp_length /= 2 * num; } -void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian) +void IK_QPositionTask::ComputeJacobian(IK_QJacobian &jacobian) { - // compute beta - const Vector3d& pos = m_segment->GlobalEnd(); - - Vector3d d_pos = m_goal - pos; - double length = d_pos.norm(); - - if (length > m_clamp_length) - d_pos = (m_clamp_length / length) * d_pos; - - jacobian.SetBetas(m_id, m_size, m_weight * d_pos); - - // compute derivatives - int i; - const IK_QSegment *seg; - - for (seg = m_segment; seg; seg = seg->Parent()) { - Vector3d p = seg->GlobalStart() - pos; - - for (i = 0; i < seg->NumberOfDoF(); i++) { - Vector3d axis = seg->Axis(i) * m_weight; - - if (seg->Translational()) - jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e2); - else { - Vector3d pa = p.cross(axis); - jacobian.SetDerivatives(m_id, seg->DoFId() + i, pa, 1e0); - } - } - } + // compute beta + const Vector3d &pos = m_segment->GlobalEnd(); + + Vector3d d_pos = m_goal - pos; + double length = d_pos.norm(); + + if (length > m_clamp_length) + d_pos = (m_clamp_length / length) * d_pos; + + jacobian.SetBetas(m_id, m_size, m_weight * d_pos); + + // compute derivatives + int i; + const IK_QSegment *seg; + + for (seg = m_segment; seg; seg = seg->Parent()) { + Vector3d p = seg->GlobalStart() - pos; + + for (i = 0; i < seg->NumberOfDoF(); i++) { + Vector3d axis = seg->Axis(i) * m_weight; + + if (seg->Translational()) + jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e2); + else { + Vector3d pa = p.cross(axis); + jacobian.SetDerivatives(m_id, seg->DoFId() + i, pa, 1e0); + } + } + } } double IK_QPositionTask::Distance() const { - const Vector3d& pos = m_segment->GlobalEnd(); - Vector3d d_pos = m_goal - pos; - return d_pos.norm(); + const Vector3d &pos = m_segment->GlobalEnd(); + Vector3d d_pos = m_goal - pos; + return d_pos.norm(); } // IK_QOrientationTask -IK_QOrientationTask::IK_QOrientationTask( - bool primary, - const IK_QSegment *segment, - const Matrix3d& goal - ) : - IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0) +IK_QOrientationTask::IK_QOrientationTask(bool primary, + const IK_QSegment *segment, + const Matrix3d &goal) + : IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0) { } -void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian) +void IK_QOrientationTask::ComputeJacobian(IK_QJacobian &jacobian) { - // compute betas - const Matrix3d& rot = m_segment->GlobalTransform().linear(); + // compute betas + const Matrix3d &rot = m_segment->GlobalTransform().linear(); - Matrix3d d_rotm = (m_goal * rot.transpose()).transpose(); + Matrix3d d_rotm = (m_goal * rot.transpose()).transpose(); - Vector3d d_rot; - d_rot = -0.5 * Vector3d(d_rotm(2, 1) - d_rotm(1, 2), - d_rotm(0, 2) - d_rotm(2, 0), - d_rotm(1, 0) - d_rotm(0, 1)); + Vector3d d_rot; + d_rot = -0.5 * Vector3d(d_rotm(2, 1) - d_rotm(1, 2), + d_rotm(0, 2) - d_rotm(2, 0), + d_rotm(1, 0) - d_rotm(0, 1)); - m_distance = d_rot.norm(); + m_distance = d_rot.norm(); - jacobian.SetBetas(m_id, m_size, m_weight * d_rot); + jacobian.SetBetas(m_id, m_size, m_weight * d_rot); - // compute derivatives - int i; - const IK_QSegment *seg; + // compute derivatives + int i; + const IK_QSegment *seg; - for (seg = m_segment; seg; seg = seg->Parent()) - for (i = 0; i < seg->NumberOfDoF(); i++) { + for (seg = m_segment; seg; seg = seg->Parent()) + for (i = 0; i < seg->NumberOfDoF(); i++) { - if (seg->Translational()) - jacobian.SetDerivatives(m_id, seg->DoFId() + i, Vector3d(0, 0, 0), 1e2); - else { - Vector3d axis = seg->Axis(i) * m_weight; - jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e0); - } - } + if (seg->Translational()) + jacobian.SetDerivatives(m_id, seg->DoFId() + i, Vector3d(0, 0, 0), 1e2); + else { + Vector3d axis = seg->Axis(i) * m_weight; + jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e0); + } + } } // IK_QCenterOfMassTask // Note: implementation not finished! -IK_QCenterOfMassTask::IK_QCenterOfMassTask( - bool primary, - const IK_QSegment *segment, - const Vector3d& goal_center - ) : - IK_QTask(3, primary, true, segment), m_goal_center(goal_center) +IK_QCenterOfMassTask::IK_QCenterOfMassTask(bool primary, + const IK_QSegment *segment, + const Vector3d &goal_center) + : IK_QTask(3, primary, true, segment), m_goal_center(goal_center) { - m_total_mass_inv = ComputeTotalMass(m_segment); - if (!FuzzyZero(m_total_mass_inv)) - m_total_mass_inv = 1.0 / m_total_mass_inv; + m_total_mass_inv = ComputeTotalMass(m_segment); + if (!FuzzyZero(m_total_mass_inv)) + m_total_mass_inv = 1.0 / m_total_mass_inv; } double IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment) { - double mass = /*seg->Mass()*/ 1.0; + double mass = /*seg->Mass()*/ 1.0; + + const IK_QSegment *seg; + for (seg = segment->Child(); seg; seg = seg->Sibling()) + mass += ComputeTotalMass(seg); - const IK_QSegment *seg; - for (seg = segment->Child(); seg; seg = seg->Sibling()) - mass += ComputeTotalMass(seg); - - return mass; + return mass; } Vector3d IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment) { - Vector3d center = /*seg->Mass()**/ segment->GlobalStart(); + Vector3d center = /*seg->Mass()**/ segment->GlobalStart(); - const IK_QSegment *seg; - for (seg = segment->Child(); seg; seg = seg->Sibling()) - center += ComputeCenter(seg); - - return center; + const IK_QSegment *seg; + for (seg = segment->Child(); seg; seg = seg->Sibling()) + center += ComputeCenter(seg); + + return center; } -void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, Vector3d& center, const IK_QSegment *segment) +void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian &jacobian, + Vector3d ¢er, + const IK_QSegment *segment) { - int i; - Vector3d p = center - segment->GlobalStart(); - - for (i = 0; i < segment->NumberOfDoF(); i++) { - Vector3d axis = segment->Axis(i) * m_weight; - axis *= /*segment->Mass()**/ m_total_mass_inv; - - if (segment->Translational()) - jacobian.SetDerivatives(m_id, segment->DoFId() + i, axis, 1e2); - else { - Vector3d pa = axis.cross(p); - jacobian.SetDerivatives(m_id, segment->DoFId() + i, pa, 1e0); - } - } - - const IK_QSegment *seg; - for (seg = segment->Child(); seg; seg = seg->Sibling()) - JacobianSegment(jacobian, center, seg); + int i; + Vector3d p = center - segment->GlobalStart(); + + for (i = 0; i < segment->NumberOfDoF(); i++) { + Vector3d axis = segment->Axis(i) * m_weight; + axis *= /*segment->Mass()**/ m_total_mass_inv; + + if (segment->Translational()) + jacobian.SetDerivatives(m_id, segment->DoFId() + i, axis, 1e2); + else { + Vector3d pa = axis.cross(p); + jacobian.SetDerivatives(m_id, segment->DoFId() + i, pa, 1e0); + } + } + + const IK_QSegment *seg; + for (seg = segment->Child(); seg; seg = seg->Sibling()) + JacobianSegment(jacobian, center, seg); } -void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian& jacobian) +void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian &jacobian) { - Vector3d center = ComputeCenter(m_segment) * m_total_mass_inv; + Vector3d center = ComputeCenter(m_segment) * m_total_mass_inv; - // compute beta - Vector3d d_pos = m_goal_center - center; + // compute beta + Vector3d d_pos = m_goal_center - center; - m_distance = d_pos.norm(); + m_distance = d_pos.norm(); #if 0 - if (m_distance > m_clamp_length) - d_pos = (m_clamp_length / m_distance) * d_pos; + if (m_distance > m_clamp_length) + d_pos = (m_clamp_length / m_distance) * d_pos; #endif - - jacobian.SetBetas(m_id, m_size, m_weight * d_pos); - // compute derivatives - JacobianSegment(jacobian, center, m_segment); + jacobian.SetBetas(m_id, m_size, m_weight * d_pos); + + // compute derivatives + JacobianSegment(jacobian, center, m_segment); } double IK_QCenterOfMassTask::Distance() const { - return m_distance; + return m_distance; } - diff --git a/intern/iksolver/intern/IK_QTask.h b/intern/iksolver/intern/IK_QTask.h index 58b832177db..595d1454872 100644 --- a/intern/iksolver/intern/IK_QTask.h +++ b/intern/iksolver/intern/IK_QTask.h @@ -28,116 +28,128 @@ #include "IK_QJacobian.h" #include "IK_QSegment.h" -class IK_QTask -{ -public: - IK_QTask( - int size, - bool primary, - bool active, - const IK_QSegment *segment - ); - virtual ~IK_QTask() {} - - int Id() const - { return m_size; } - - void SetId(int id) - { m_id = id; } - - int Size() const - { return m_size; } - - bool Primary() const - { return m_primary; } - - bool Active() const - { return m_active; } - - double Weight() const - { return m_weight*m_weight; } - - void SetWeight(double weight) - { m_weight = sqrt(weight); } +class IK_QTask { + public: + IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment); + virtual ~IK_QTask() + { + } + + int Id() const + { + return m_size; + } + + void SetId(int id) + { + m_id = id; + } + + int Size() const + { + return m_size; + } + + bool Primary() const + { + return m_primary; + } + + bool Active() const + { + return m_active; + } + + double Weight() const + { + return m_weight * m_weight; + } + + void SetWeight(double weight) + { + m_weight = sqrt(weight); + } + + virtual void ComputeJacobian(IK_QJacobian &jacobian) = 0; + + virtual double Distance() const = 0; + + virtual bool PositionTask() const + { + return false; + } + + virtual void Scale(double) + { + } + + protected: + int m_id; + int m_size; + bool m_primary; + bool m_active; + const IK_QSegment *m_segment; + double m_weight; +}; - virtual void ComputeJacobian(IK_QJacobian& jacobian)=0; +class IK_QPositionTask : public IK_QTask { + public: + IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal); - virtual double Distance() const=0; + void ComputeJacobian(IK_QJacobian &jacobian); - virtual bool PositionTask() const { return false; } + double Distance() const; - virtual void Scale(double) {} + bool PositionTask() const + { + return true; + } + void Scale(double scale) + { + m_goal *= scale; + m_clamp_length *= scale; + } -protected: - int m_id; - int m_size; - bool m_primary; - bool m_active; - const IK_QSegment *m_segment; - double m_weight; + private: + Vector3d m_goal; + double m_clamp_length; }; -class IK_QPositionTask : public IK_QTask -{ -public: - IK_QPositionTask( - bool primary, - const IK_QSegment *segment, - const Vector3d& goal - ); - - void ComputeJacobian(IK_QJacobian& jacobian); +class IK_QOrientationTask : public IK_QTask { + public: + IK_QOrientationTask(bool primary, const IK_QSegment *segment, const Matrix3d &goal); - double Distance() const; + double Distance() const + { + return m_distance; + } + void ComputeJacobian(IK_QJacobian &jacobian); - bool PositionTask() const { return true; } - void Scale(double scale) { m_goal *= scale; m_clamp_length *= scale; } - -private: - Vector3d m_goal; - double m_clamp_length; + private: + Matrix3d m_goal; + double m_distance; }; -class IK_QOrientationTask : public IK_QTask -{ -public: - IK_QOrientationTask( - bool primary, - const IK_QSegment *segment, - const Matrix3d& goal - ); - - double Distance() const { return m_distance; } - void ComputeJacobian(IK_QJacobian& jacobian); - -private: - Matrix3d m_goal; - double m_distance; -}; +class IK_QCenterOfMassTask : public IK_QTask { + public: + IK_QCenterOfMassTask(bool primary, const IK_QSegment *segment, const Vector3d ¢er); + void ComputeJacobian(IK_QJacobian &jacobian); -class IK_QCenterOfMassTask : public IK_QTask -{ -public: - IK_QCenterOfMassTask( - bool primary, - const IK_QSegment *segment, - const Vector3d& center - ); + double Distance() const; - void ComputeJacobian(IK_QJacobian& jacobian); + void Scale(double scale) + { + m_goal_center *= scale; + m_distance *= scale; + } - double Distance() const; + private: + double ComputeTotalMass(const IK_QSegment *segment); + Vector3d ComputeCenter(const IK_QSegment *segment); + void JacobianSegment(IK_QJacobian &jacobian, Vector3d ¢er, const IK_QSegment *segment); - void Scale(double scale) { m_goal_center *= scale; m_distance *= scale; } - -private: - double ComputeTotalMass(const IK_QSegment *segment); - Vector3d ComputeCenter(const IK_QSegment *segment); - void JacobianSegment(IK_QJacobian& jacobian, Vector3d& center, const IK_QSegment *segment); - - Vector3d m_goal_center; - double m_total_mass_inv; - double m_distance; + Vector3d m_goal_center; + double m_total_mass_inv; + double m_distance; }; - diff --git a/intern/iksolver/intern/IK_Solver.cpp b/intern/iksolver/intern/IK_Solver.cpp index beec5da983f..5205fd6e16c 100644 --- a/intern/iksolver/intern/IK_Solver.cpp +++ b/intern/iksolver/intern/IK_Solver.cpp @@ -21,7 +21,6 @@ * \ingroup iksolver */ - #include "../extern/IK_solver.h" #include "IK_QJacobianSolver.h" @@ -32,355 +31,387 @@ using namespace std; class IK_QSolver { -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - IK_QSolver() : root(NULL) { - } - - IK_QJacobianSolver solver; - IK_QSegment *root; - std::list<IK_QTask *> tasks; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + IK_QSolver() : root(NULL) + { + } + + IK_QJacobianSolver solver; + IK_QSegment *root; + std::list<IK_QTask *> tasks; }; // FIXME: locks still result in small "residual" changes to the locked axes... static IK_QSegment *CreateSegment(int flag, bool translate) { - int ndof = 0; - ndof += (flag & IK_XDOF) ? 1 : 0; - ndof += (flag & IK_YDOF) ? 1 : 0; - ndof += (flag & IK_ZDOF) ? 1 : 0; - - IK_QSegment *seg; - - if (ndof == 0) - return NULL; - else if (ndof == 1) { - int axis; - - if (flag & IK_XDOF) axis = 0; - else if (flag & IK_YDOF) axis = 1; - else axis = 2; - - if (translate) - seg = new IK_QTranslateSegment(axis); - else - seg = new IK_QRevoluteSegment(axis); - } - else if (ndof == 2) { - int axis1, axis2; - - if (flag & IK_XDOF) { - axis1 = 0; - axis2 = (flag & IK_YDOF) ? 1 : 2; - } - else { - axis1 = 1; - axis2 = 2; - } - - if (translate) - seg = new IK_QTranslateSegment(axis1, axis2); - else { - if (axis1 + axis2 == 2) - seg = new IK_QSwingSegment(); - else - seg = new IK_QElbowSegment((axis1 == 0) ? 0 : 2); - } - } - else { - if (translate) - seg = new IK_QTranslateSegment(); - else - seg = new IK_QSphericalSegment(); - } - - return seg; + int ndof = 0; + ndof += (flag & IK_XDOF) ? 1 : 0; + ndof += (flag & IK_YDOF) ? 1 : 0; + ndof += (flag & IK_ZDOF) ? 1 : 0; + + IK_QSegment *seg; + + if (ndof == 0) + return NULL; + else if (ndof == 1) { + int axis; + + if (flag & IK_XDOF) + axis = 0; + else if (flag & IK_YDOF) + axis = 1; + else + axis = 2; + + if (translate) + seg = new IK_QTranslateSegment(axis); + else + seg = new IK_QRevoluteSegment(axis); + } + else if (ndof == 2) { + int axis1, axis2; + + if (flag & IK_XDOF) { + axis1 = 0; + axis2 = (flag & IK_YDOF) ? 1 : 2; + } + else { + axis1 = 1; + axis2 = 2; + } + + if (translate) + seg = new IK_QTranslateSegment(axis1, axis2); + else { + if (axis1 + axis2 == 2) + seg = new IK_QSwingSegment(); + else + seg = new IK_QElbowSegment((axis1 == 0) ? 0 : 2); + } + } + else { + if (translate) + seg = new IK_QTranslateSegment(); + else + seg = new IK_QSphericalSegment(); + } + + 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; + 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; + IK_QSegment *qseg = (IK_QSegment *)seg; - if (qseg->Composite()) - delete qseg->Composite(); - delete qseg; + 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; + IK_QSegment *qseg = (IK_QSegment *)seg; + IK_QSegment *qparent = (IK_QSegment *)parent; - if (qparent && qparent->Composite()) - qseg->SetParent(qparent->Composite()); - else - qseg->SetParent(qparent); + 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) +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); + 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_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); + 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); } 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); + 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_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); + 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_GetTranslationChange(IK_Segment *seg, float *translation_change) { - IK_QSegment *qseg = (IK_QSegment *)seg; + IK_QSegment *qseg = (IK_QSegment *)seg; + + if (!qseg->Translational() && qseg->Composite()) + qseg = qseg->Composite(); - if (!qseg->Translational() && qseg->Composite()) - qseg = qseg->Composite(); - - const Vector3d& change = qseg->TranslationChange(); + const Vector3d &change = qseg->TranslationChange(); - translation_change[0] = (float)change[0]; - translation_change[1] = (float)change[1]; - translation_change[2] = (float)change[2]; + translation_change[0] = (float)change[0]; + translation_change[1] = (float)change[1]; + translation_change[2] = (float)change[2]; } IK_Solver *IK_CreateSolver(IK_Segment *root) { - if (root == NULL) - return NULL; - - IK_QSolver *solver = new IK_QSolver(); - solver->root = (IK_QSegment *)root; + if (root == NULL) + return NULL; + + IK_QSolver *solver = new IK_QSolver(); + solver->root = (IK_QSegment *)root; - return (IK_Solver *)solver; + return (IK_Solver *)solver; } void IK_FreeSolver(IK_Solver *solver) { - if (solver == NULL) - return; + if (solver == NULL) + return; - IK_QSolver *qsolver = (IK_QSolver *)solver; - std::list<IK_QTask *>& tasks = qsolver->tasks; - std::list<IK_QTask *>::iterator task; + IK_QSolver *qsolver = (IK_QSolver *)solver; + std::list<IK_QTask *> &tasks = qsolver->tasks; + std::list<IK_QTask *>::iterator task; - for (task = tasks.begin(); task != tasks.end(); task++) - delete (*task); - - delete qsolver; + for (task = tasks.begin(); task != tasks.end(); task++) + delete (*task); + + delete qsolver; } void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight) { - if (solver == NULL || tip == NULL) - return; + if (solver == NULL || tip == NULL) + return; - IK_QSolver *qsolver = (IK_QSolver *)solver; - IK_QSegment *qtip = (IK_QSegment *)tip; + 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(); + // 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]); + 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); + 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; - - // 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); + 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_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle) +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; + if (solver == NULL || tip == NULL) + return; - IK_QSolver *qsolver = (IK_QSolver *)solver; - IK_QSegment *qtip = (IK_QSegment *)tip; + 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(); + // 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]); + Vector3d qgoal(goal[0], goal[1], goal[2]); + Vector3d qpolegoal(polegoal[0], polegoal[1], polegoal[2]); - qsolver->solver.SetPoleVectorConstraint( - qtip, qgoal, qpolegoal, poleangle, getangle); + qsolver->solver.SetPoleVectorConstraint(qtip, qgoal, qpolegoal, poleangle, getangle); } float IK_SolverGetPoleAngle(IK_Solver *solver) { - if (solver == NULL) - return 0.0f; + if (solver == NULL) + return 0.0f; - IK_QSolver *qsolver = (IK_QSolver *)solver; + IK_QSolver *qsolver = (IK_QSolver *)solver; - return qsolver->solver.GetPoleAngle(); + return qsolver->solver.GetPoleAngle(); } #if 0 static void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3], float weight) { - if (solver == NULL || root == NULL) - return; + if (solver == NULL || root == NULL) + return; - IK_QSolver *qsolver = (IK_QSolver *)solver; - IK_QSegment *qroot = (IK_QSegment *)root; + IK_QSolver *qsolver = (IK_QSolver *)solver; + IK_QSegment *qroot = (IK_QSegment *)root; - // convert from blender column major - Vector3d center(goal); + // 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); + IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center); + com->SetWeight(weight); + qsolver->tasks.push_back(com); } #endif int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations) { - if (solver == NULL) - return 0; + if (solver == NULL) + return 0; - IK_QSolver *qsolver = (IK_QSolver *)solver; + IK_QSolver *qsolver = (IK_QSolver *)solver; - IK_QSegment *root = qsolver->root; - IK_QJacobianSolver& jacobian = qsolver->solver; - std::list<IK_QTask *>& tasks = qsolver->tasks; - double tol = tolerance; + IK_QSegment *root = qsolver->root; + IK_QJacobianSolver &jacobian = qsolver->solver; + std::list<IK_QTask *> &tasks = qsolver->tasks; + double tol = tolerance; - if (!jacobian.Setup(root, tasks)) - return 0; + if (!jacobian.Setup(root, tasks)) + return 0; - bool result = jacobian.Solve(root, tasks, tol, max_iterations); + bool result = jacobian.Solve(root, tasks, tol, max_iterations); - return ((result) ? 1 : 0); + return ((result) ? 1 : 0); } - |