kannonier8/blender
1
0
Fork 0
forked from bartvdbraak/blender
Code Pull Requests Activity

Forgot to add new files...

Browse Source
This commit is contained in:
Ton Roosendaal 2005-08-27 13:10:41 +00:00
parent ac619bace6
commit 8d36b517f9
4 changed files with 1027 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

516
intern/iksolver/intern/IK_QJacobian.cpp Normal file
View File

@ -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 <iostream>
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<m_svd_w.size(); i++) {
if (m_svd_w[i] <= epsilon)
continue;
MT_Scalar wInv = 1.0/m_svd_w[i];
MT_Scalar alpha = 0.0;
MT_Scalar N = 0.0;
// compute alpha and N
for (j=0; j<m_svd_u.num_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
MT_Scalar 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
MT_Scalar M = 0.0;
MT_Scalar max_dtheta = 0.0, abs_dtheta;
for (j = 0; j < m_d_theta.size(); j++) {
MT_Scalar v = m_svd_v[j][i];
M += MT_abs(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 = MT_abs(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
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<m_dof; j++) {
m_d_theta[j] *= m_weight[j];
abs_angle = MT_abs(m_d_theta[j]);
if (abs_angle > 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<m_dof; j++)
m_d_theta[j] *= damp;
}
}
void IK_QJacobian::InvertDLS()
{
// 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.
// 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
MT_Scalar epsilon = 1e-10;
MT_Scalar max_angle_change = 0.1;
MT_Scalar x_length = sqrt(TNT::dot_prod(m_beta, m_beta));
int i, j;
MT_Scalar w_min = MT_INFINITY;
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
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<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];
}
void IK_QJacobian::Lock(int dof_id, MT_Scalar delta)
{
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;
}
m_norm[dof_id] = 0.0; // unneeded
m_d_theta[dof_id] = 0.0;
}
#if 0
void IK_QJacobian::SetSecondary(int dof_id, MT_Scalar d)
{
m_alpha[dof_id] = d;
}
void IK_QJacobian::SolveSecondary()
{
if (!ComputeNullProjection())
return;
TNT::matmult(m_d_theta, m_null, m_alpha);
m_alpha = 0;
}
#endif
MT_Scalar IK_QJacobian::AngleUpdate(int dof_id) const
{
return m_d_theta[dof_id];
}
MT_Scalar IK_QJacobian::AngleUpdateNorm() const
{
int i;
MT_Scalar mx = 0.0, dtheta_abs;
for (i = 0; i < m_d_theta.size(); i++) {
dtheta_abs = MT_abs(m_d_theta[i]);
if (dtheta_abs > 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);
}

123
intern/iksolver/intern/IK_QJacobian.h Normal file
View File

@ -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 <vector>
#include "MT_Vector3.h"
class IK_QJacobian
{
public:
typedef TNT::Matrix<MT_Scalar> TMatrix;
typedef TNT::Vector<MT_Scalar>TVector;
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

242
intern/iksolver/intern/IK_QTask.cpp Normal file
View File

@ -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 <iostream>
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;
}

146
intern/iksolver/intern/IK_QTask.h Normal file
View File

@ -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