From 8d36b517f928e66c52f795ac93364f778926a08e Mon Sep 17 00:00:00 2001 From: Ton Roosendaal Date: Sat, 27 Aug 2005 13:10:41 +0000 Subject: Forgot to add new files... --- intern/iksolver/intern/IK_QJacobian.cpp | 516 ++++++++++++++++++++++++++++++++ intern/iksolver/intern/IK_QJacobian.h | 123 ++++++++ intern/iksolver/intern/IK_QTask.cpp | 242 +++++++++++++++ intern/iksolver/intern/IK_QTask.h | 146 +++++++++ 4 files changed, 1027 insertions(+) create mode 100644 intern/iksolver/intern/IK_QJacobian.cpp create mode 100644 intern/iksolver/intern/IK_QJacobian.h create mode 100644 intern/iksolver/intern/IK_QTask.cpp create mode 100644 intern/iksolver/intern/IK_QTask.h diff --git a/intern/iksolver/intern/IK_QJacobian.cpp b/intern/iksolver/intern/IK_QJacobian.cpp new file mode 100644 index 00000000000..c9690e68d4c --- /dev/null +++ b/intern/iksolver/intern/IK_QJacobian.cpp @@ -0,0 +1,516 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Original Author: Laurence + * Contributor(s): Brecht + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#include "IK_QJacobian.h" +#include "TNT/svd.h" + +#include +using namespace std; + +IK_QJacobian::IK_QJacobian() +: m_sdls(true), m_min_damp(1.0) +{ +} + +IK_QJacobian::~IK_QJacobian() +{ +} + +void IK_QJacobian::ArmMatrices(int dof, int task_size, int tasks) +{ + m_dof = dof; + m_task_size = task_size; + m_tasks = tasks; + + m_jacobian.newsize(task_size, dof); + m_jacobian = 0; + + m_alpha.newsize(dof); + m_alpha = 0; + + m_null.newsize(dof, dof); + + m_d_theta.newsize(dof); + m_d_theta_tmp.newsize(dof); + + m_norm.newsize(dof); + m_norm = 0.0; + + m_beta.newsize(task_size); + + m_weight.newsize(dof); + m_weight_sqrt.newsize(dof); + m_weight = 1.0; + m_weight_sqrt = 1.0; + + // m_svd_work_space.newsize(dof); // TNT resizes this? + + if (task_size >= dof) { + m_transpose = false; + + m_svd_u.newsize(task_size, dof); + m_svd_v.newsize(dof, dof); + m_svd_w.newsize(dof); + + m_svd_u_t.newsize(dof, task_size); + m_svd_u_beta.newsize(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.newsize(task_size, task_size); + m_svd_v.newsize(dof, task_size); + m_svd_w.newsize(task_size); + + m_svd_u_t.newsize(task_size, task_size); + m_svd_u_beta.newsize(task_size); + } +} + +void IK_QJacobian::SetBetas(int id, int, const MT_Vector3& v) +{ + m_beta[id] = v.x(); + m_beta[id+1] = v.y(); + m_beta[id+2] = v.z(); + + //printf("#: %f %f %f\n", v.x(), v.y(), v.z()); +} + +void IK_QJacobian::SetDerivatives(int id, int dof_id, const MT_Vector3& v) +{ + m_jacobian[id][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]; + + //printf("%d: %f %f %f\n", dof_id, v.x(), v.y(), v.z()); +} + +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 + TNT::transpose(m_jacobian, m_svd_v); + + TNT::SVD(m_svd_v, m_svd_w, m_svd_u, m_svd_work_space); + } + else { + // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal, + // so Jinv = V*Winv*Ut + m_svd_u = m_jacobian; + + TNT::SVD(m_svd_u, m_svd_w, m_svd_v, m_svd_work_space); + } + + if (m_sdls) + InvertSDLS(); + else + InvertDLS(); + +#if 0 + if (!ComputeNullProjection()) + return; + + int i, j; + for (i = 0; i < m_weight.size(); i++) + m_weight[i] = 1.0 - m_weight[i]; + + TNT::matmultdiag(m_null, m_null, m_weight); + + for (i = 0; i < m_null.num_rows(); i++) + for (j = 0; j < m_null.num_cols(); j++) + if (i == j) + m_null[i][j] = 1.0 - m_null[i][j]; + else + m_null[i][j] = -m_null[i][j]; + + TVector ntheta(m_d_theta); + TNT::matmult(ntheta, m_null, m_d_theta); + + cout << "#" << endl; + for (i = 0; i < m_d_theta.size(); i++) + printf("%f >> %f (%f)\n", m_d_theta[i], ntheta[i], m_weight[i]); + m_d_theta = ntheta; + + for (i = 0; i < m_weight.size(); i++) + m_weight[i] = 1.0 - m_weight[i]; +#endif +} + +bool IK_QJacobian::ComputeNullProjection() +{ + MT_Scalar 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; + + TMatrix basis(m_svd_v.num_rows(), rank); + TMatrix basis_t(rank, m_svd_v.num_rows()); + 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.num_rows(); j++) + basis[j][b] = m_svd_v[j][i]; + b++; + } + + TNT::transpose(basis, basis_t); + TNT::matmult(m_null, basis, basis_t); + + for (i = 0; i < m_null.num_rows(); i++) + for (j = 0; j < m_null.num_cols(); j++) + if (i == j) + m_null[i][j] = 1.0 - m_null[i][j]; + else + m_null[i][j] = -m_null[i][j]; + + return true; +} + +void IK_QJacobian::SubTask(IK_QJacobian& jacobian) +{ + if (!ComputeNullProjection()) + return; + +#if 0 + int i, j; + + m_null.newsize(m_d_theta.size(), m_d_theta.size()); + + for (i = 0; i < m_d_theta.size(); i++) + for (j = 0; j < m_d_theta.size(); j++) + if (i == j) + m_null[i][j] = 1.0; + else + m_null[i][j] = 0.0; + + // restrict lower priority jacobian + //jacobian.Restrict(m_d_theta, m_null); + + // add angle update from lower priority + jacobian.Invert(); + + TVector d2(m_d_theta.size()); + TVector n2(m_d_theta.size()); + + for (i = 0; i < m_d_theta.size(); i++) + d2[i] = jacobian.AngleUpdate(i); + + TNT::matmult(n2, m_null, d2); + + m_d_theta = m_d_theta + n2; +#else + int i; + + // restrict lower priority jacobian + jacobian.Restrict(m_d_theta, m_null); + + // 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 + for (i = 0; i < m_d_theta.size(); i++) + m_d_theta[i] = m_d_theta[i] + /*m_min_damp**/jacobian.AngleUpdate(i); +#endif +} + +void IK_QJacobian::Restrict(TVector& d_theta, TMatrix& null) +{ + // subtract part already moved by higher task from beta + TVector beta_sub(m_beta.size()); + + TNT::matmult(beta_sub, m_jacobian, d_theta); + m_beta = m_beta - beta_sub; + + // note: should we be using the norm of the unrestricted jacobian for SDLS? + + // project jacobian on to null space of higher priority task + TMatrix jacobian_copy(m_jacobian); + TNT::matmult(m_jacobian, jacobian_copy, null); +} + +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. + + MT_Scalar max_angle_change = MT_PI/4.0; + MT_Scalar epsilon = 1e-10; + int i, j; + + m_d_theta = 0; + 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) { + MT_Scalar 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 max_dtheta) + max_dtheta = abs_dtheta; + } + + M *= wInv; + + // compute damping term and damp the dTheta's + MT_Scalar gamma = max_angle_change; + if (N < M) + gamma *= N/M; + + MT_Scalar 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 + m_d_theta[j] += 0.80*damp*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 + MT_Scalar max_angle = 0.0, abs_angle; + + for (j = 0; j max_angle) + max_angle = abs_angle; + } + + if (max_angle > max_angle_change) { + MT_Scalar damp = (max_angle_change)/(max_angle_change + max_angle); + + for (j = 0; j epsilon && m_svd_w[i] < w_min) + w_min = m_svd_w[i]; + } + + // compute lambda damping term + + MT_Scalar d = x_length/max_angle_change; + MT_Scalar 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; + + lambda *= lambda; + + if (lambda > 10) + lambda = 10; + + // immediately multiply with Beta, so we can do matrix*vector products + // rather than matrix*matrix products + + // compute Ut*Beta + TNT::transpose(m_svd_u, m_svd_u_t); + TNT::matmult(m_svd_u_beta, m_svd_u_t, m_beta); + + m_d_theta = 0.0; + + for (i = 0; i < m_svd_w.size(); i++) { + if (m_svd_w[i] > epsilon) { + MT_Scalar 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; + + for (j = 0; j mx) + mx = dtheta_abs; + } + + return mx; +} + +void IK_QJacobian::SetDoFWeight(int dof, MT_Scalar 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 new file mode 100644 index 00000000000..b2d990d5489 --- /dev/null +++ b/intern/iksolver/intern/IK_QJacobian.h @@ -0,0 +1,123 @@ + +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Original Author: Laurence + * Contributor(s): Brecht + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#ifndef NAN_INCLUDED_IK_QJacobian_h + +#define NAN_INCLUDED_IK_QJacobian_h + +#include "TNT/cmat.h" +#include "TNT/fmat.h" +#include +#include "MT_Vector3.h" + +class IK_QJacobian +{ +public: + typedef TNT::Matrix TMatrix; + typedef TNT::VectorTVector; + + IK_QJacobian(); + ~IK_QJacobian(); + + // Call once to initialize + void ArmMatrices(int dof, int task_size, int tasks); + void SetDoFWeight(int dof, MT_Scalar weight); + + // Iteratively called + void SetBetas(int id, int size, const MT_Vector3& v); + void SetDerivatives(int id, int dof_id, const MT_Vector3& v); + + void Invert(); + + MT_Scalar AngleUpdate(int dof_id) const; + MT_Scalar AngleUpdateNorm() const; + + // DoF locking for inner clamping loop + void Lock(int dof_id, MT_Scalar delta); + + // Secondary task + bool ComputeNullProjection(); + + void Restrict(TVector& d_theta, TMatrix& null); + void SubTask(IK_QJacobian& jacobian); + +#if 0 + void SetSecondary(int dof_id, MT_Scalar d); + void SolveSecondary(); +#endif + +private: + + void InvertSDLS(); + void InvertDLS(); + + int m_dof, m_task_size, m_tasks; + bool m_transpose; + + // the jacobian matrix and it's null space projector + TMatrix m_jacobian; + TMatrix m_null; + + /// the vector of intermediate betas + TVector m_beta; + + /// the vector of computed angle changes + TVector m_d_theta; + + /// space required for SVD computation + + TVector m_svd_w; + TVector m_svd_work_space; + TMatrix m_svd_v; + TMatrix m_svd_u; + + TMatrix m_svd_u_t; + TVector m_svd_u_beta; + + // space required for SDLS + + bool m_sdls; + TVector m_norm; + TVector m_d_theta_tmp; + MT_Scalar m_min_damp; + + // null space task vector + TVector m_alpha; + + // dof weighting + TVector m_weight; + TVector m_weight_sqrt; +}; + +#endif + diff --git a/intern/iksolver/intern/IK_QTask.cpp b/intern/iksolver/intern/IK_QTask.cpp new file mode 100644 index 00000000000..781a2712885 --- /dev/null +++ b/intern/iksolver/intern/IK_QTask.cpp @@ -0,0 +1,242 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Original Author: Laurence + * Contributor(s): Brecht + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#include "IK_QTask.h" + +// IK_QTask + +#include +using namespace std; + +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 MT_Vector3& goal +) : + IK_QTask(3, primary, true, segment), m_goal(goal) +{ + // computing clamping length + int num; + const IK_QSegment *seg; + + m_clamp_length = 0.0; + num = 0; + + for (seg = m_segment; seg; seg = seg->Parent()) { + m_clamp_length += seg->MaxExtension(); + num++; + } + + m_clamp_length /= 2*num; +} + +void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian) +{ + // compute beta + const MT_Vector3& pos = m_segment->GlobalEnd(); + + MT_Vector3 d_pos = m_goal - pos; + MT_Scalar length = d_pos.length(); + + 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()) { + MT_Vector3 p = seg->GlobalStart() - pos; + + for (i = 0; i < seg->NumberOfDoF(); i++) { + MT_Vector3 axis = seg->Axis(i)*m_weight; + + if (seg->Translational()) + jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis); + else { + MT_Vector3 pa = p.cross(axis); + jacobian.SetDerivatives(m_id, seg->DoFId()+i, pa); + } + } + } +} + +MT_Scalar IK_QPositionTask::Distance() const +{ + const MT_Vector3& pos = m_segment->GlobalEnd(); + MT_Vector3 d_pos = m_goal - pos; + return d_pos.length(); +} + +// IK_QOrientationTask + +IK_QOrientationTask::IK_QOrientationTask( + bool primary, + const IK_QSegment *segment, + const MT_Matrix3x3& goal +) : + IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0) +{ +} + +void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian) +{ + // compute betas + const MT_Matrix3x3& rot = m_segment->GlobalTransform().getBasis(); + + MT_Matrix3x3 d_rotm = m_goal*rot.transposed(); + d_rotm.transpose(); + + MT_Vector3 d_rot; + d_rot = -0.5*MT_Vector3(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.length(); + + jacobian.SetBetas(m_id, m_size, m_weight*d_rot); + + // compute derivatives + int i; + const IK_QSegment *seg; + + 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, MT_Vector3(0, 0, 0)); + else { + MT_Vector3 axis = seg->Axis(i)*m_weight; + jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis); + } + } +} + +// IK_QCenterOfMassTask +// Note: implementation not finished! + +IK_QCenterOfMassTask::IK_QCenterOfMassTask( + bool primary, + const IK_QSegment *segment, + const MT_Vector3& goal_center +) : + IK_QTask(3, primary, true, segment), m_goal_center(goal_center) +{ + m_total_mass_inv = ComputeTotalMass(m_segment); + if (!MT_fuzzyZero(m_total_mass_inv)) + m_total_mass_inv = 1.0/m_total_mass_inv; +} + +MT_Scalar IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment) +{ + MT_Scalar mass = /*seg->Mass()*/ 1.0; + + const IK_QSegment *seg; + for (seg = segment->Child(); seg; seg = seg->Sibling()) + mass += ComputeTotalMass(seg); + + return mass; +} + +MT_Vector3 IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment) +{ + MT_Vector3 center = /*seg->Mass()**/segment->GlobalStart(); + + const IK_QSegment *seg; + for (seg = segment->Child(); seg; seg = seg->Sibling()) + center += ComputeCenter(seg); + + return center; +} + +void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& center, const IK_QSegment *segment) +{ + int i; + MT_Vector3 p = center - segment->GlobalStart(); + + for (i = 0; i < segment->NumberOfDoF(); i++) { + MT_Vector3 axis = segment->Axis(i)*m_weight; + axis *= /*segment->Mass()**/m_total_mass_inv; + + if (segment->Translational()) + jacobian.SetDerivatives(m_id, segment->DoFId()+i, axis); + else { + MT_Vector3 pa = axis.cross(p); + jacobian.SetDerivatives(m_id, segment->DoFId()+i, pa); + } + } + + const IK_QSegment *seg; + for (seg = segment->Child(); seg; seg = seg->Sibling()) + JacobianSegment(jacobian, center, seg); +} + +void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian& jacobian) +{ + MT_Vector3 center = ComputeCenter(m_segment)*m_total_mass_inv; + + // compute beta + MT_Vector3 d_pos = m_goal_center - center; + + m_distance = d_pos.length(); + +#if 0 + 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); +} + +MT_Scalar IK_QCenterOfMassTask::Distance() const +{ + return m_distance; +} + diff --git a/intern/iksolver/intern/IK_QTask.h b/intern/iksolver/intern/IK_QTask.h new file mode 100644 index 00000000000..4f162e183ff --- /dev/null +++ b/intern/iksolver/intern/IK_QTask.h @@ -0,0 +1,146 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Original author: Laurence + * Contributor(s): Brecht + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#ifndef NAN_INCLUDED_IK_QTask_h +#define NAN_INCLUDED_IK_QTask_h + +#include "MT_Vector3.h" +#include "MT_Transform.h" +#include "MT_Matrix4x4.h" +#include "IK_QJacobian.h" +#include "IK_QSegment.h" + +class IK_QTask +{ +public: + IK_QTask( + int size, + bool primary, + bool active, + const IK_QSegment *segment + ); + + 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; } + + MT_Scalar Weight() const + { return m_weight*m_weight; } + + void SetWeight(MT_Scalar weight) + { m_weight = sqrt(weight); } + + virtual void ComputeJacobian(IK_QJacobian& jacobian)=0; + + virtual MT_Scalar Distance() const=0; + +protected: + int m_id; + int m_size; + bool m_primary; + bool m_active; + const IK_QSegment *m_segment; + MT_Scalar m_weight; +}; + +class IK_QPositionTask : public IK_QTask +{ +public: + IK_QPositionTask( + bool primary, + const IK_QSegment *segment, + const MT_Vector3& goal + ); + + void ComputeJacobian(IK_QJacobian& jacobian); + + MT_Scalar Distance() const; + +private: + MT_Vector3 m_goal; + MT_Scalar m_clamp_length; +}; + +class IK_QOrientationTask : public IK_QTask +{ +public: + IK_QOrientationTask( + bool primary, + const IK_QSegment *segment, + const MT_Matrix3x3& goal + ); + + MT_Scalar Distance() const { return m_distance; }; + void ComputeJacobian(IK_QJacobian& jacobian); + +private: + MT_Matrix3x3 m_goal; + MT_Scalar m_distance; +}; + + +class IK_QCenterOfMassTask : public IK_QTask +{ +public: + IK_QCenterOfMassTask( + bool primary, + const IK_QSegment *segment, + const MT_Vector3& center + ); + + void ComputeJacobian(IK_QJacobian& jacobian); + + MT_Scalar Distance() const; + +private: + MT_Scalar ComputeTotalMass(const IK_QSegment *segment); + MT_Vector3 ComputeCenter(const IK_QSegment *segment); + void JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& center, const IK_QSegment *segment); + + MT_Vector3 m_goal_center; + MT_Scalar m_total_mass_inv; + MT_Scalar m_distance; +}; + +#endif + -- cgit v1.2.3