griefith/test2
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

style cleanup

Browse Source
This commit is contained in:
Campbell Barton
2012-07-27 22:35:27 +00:00
parent e2360ab495
commit f23bfdfab4
6 changed files with 324 additions and 324 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
intern/iksolver/intern/IK_QJacobian.cpp
View File

@@ -35,7 +35,7 @@
#include "TNT/svd.h"
IK_QJacobian::IK_QJacobian()
: m_sdls(true), m_min_damp(1.0)
: m_sdls(true), m_min_damp(1.0)
{
}
@@ -106,16 +106,16 @@ void IK_QJacobian::ArmMatrices(int dof, int 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();
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 MT_Vector3& v, MT_Scalar norm_weight)
{
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];
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;
}
@@ -194,7 +194,7 @@ void IK_QJacobian::SubTask(IK_QJacobian& jacobian)
// 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);
m_d_theta[i] = m_d_theta[i] + /*m_min_damp * */ jacobian.AngleUpdate(i);
}
void IK_QJacobian::Restrict(TVector& d_theta, TMatrix& null)
@@ -230,7 +230,7 @@ void IK_QJacobian::InvertSDLS()
// 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 max_angle_change = MT_PI / 4.0;
MT_Scalar epsilon = 1e-10;
int i, j;
@@ -239,35 +239,35 @@ void IK_QJacobian::InvertSDLS()
for (i = 0; i < m_dof; i++) {
m_norm[i] = 0.0;
for (j = 0; j < m_task_size; j+=3) {
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];
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++) {
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 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];
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];
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;
@@ -278,14 +278,14 @@ void IK_QJacobian::InvertSDLS()
for (j = 0; j < m_d_theta.size(); j++) {
MT_Scalar v = m_svd_v[j][i];
M += MT_abs(v)*m_norm[j];
M += MT_abs(v) * m_norm[j];
// compute tmporary dTheta's
m_d_theta_tmp[j] = v*alpha;
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];
abs_dtheta = MT_abs(m_d_theta_tmp[j]) * m_weight_sqrt[j];
if (abs_dtheta > max_dtheta)
max_dtheta = abs_dtheta;
}
@@ -295,19 +295,19 @@ void IK_QJacobian::InvertSDLS()
// compute damping term and damp the dTheta's
MT_Scalar gamma = max_angle_change;
if (N < M)
gamma *= N/M;
gamma *= N / M;
MT_Scalar damp = (gamma < max_dtheta)? gamma/max_dtheta: 1.0;
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
MT_Scalar dofdamp = damp/m_weight[j];
MT_Scalar dofdamp = damp / m_weight[j];
if (dofdamp > 1.0) dofdamp = 1.0;
m_d_theta[j] += 0.80*dofdamp*m_d_theta_tmp[j];
m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j];
}
if (damp < m_min_damp)
@@ -317,7 +317,7 @@ void IK_QJacobian::InvertSDLS()
// 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++) {
for (j = 0; j < m_dof; j++) {
m_d_theta[j] *= m_weight[j];
abs_angle = MT_abs(m_d_theta[j]);
@@ -327,9 +327,9 @@ void IK_QJacobian::InvertSDLS()
}
if (max_angle > max_angle_change) {
MT_Scalar damp = (max_angle_change)/(max_angle_change + max_angle);
MT_Scalar damp = (max_angle_change) / (max_angle_change + max_angle);
for (j = 0; j<m_dof; j++)
for (j = 0; j < m_dof; j++)
m_d_theta[j] *= damp;
}
}
@@ -360,20 +360,20 @@ void IK_QJacobian::InvertDLS()
int i, j;
MT_Scalar w_min = MT_INFINITY;
for (i = 0; i <m_svd_w.size() ; 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
MT_Scalar d = x_length/max_angle_change;
MT_Scalar d = x_length / max_angle_change;
MT_Scalar lambda;
if (w_min <= d/2)
lambda = d/2;
if (w_min <= d / 2)
lambda = d / 2;
else if (w_min < d)
lambda = sqrt(w_min*(d - w_min));
lambda = sqrt(w_min * (d - w_min));
else
lambda = 0.0;
@@ -393,17 +393,17 @@ void IK_QJacobian::InvertDLS()
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);
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_svd_v[j][i] * m_svd_u_beta[i];
}
}
for (j = 0; j<m_d_theta.size(); j++)
for (j = 0; j < m_d_theta.size(); j++)
m_d_theta[j] *= m_weight[j];
}
@@ -412,7 +412,7 @@ 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_beta[i] -= m_jacobian[i][dof_id] * delta;
m_jacobian[i][dof_id] = 0.0;
}
@@ -431,7 +431,7 @@ MT_Scalar IK_QJacobian::AngleUpdateNorm() const
MT_Scalar mx = 0.0, dtheta_abs;
for (i = 0; i < m_d_theta.size(); i++) {
dtheta_abs = MT_abs(m_d_theta[i]*m_d_norm_weight[i]);
dtheta_abs = MT_abs(m_d_theta[i] * m_d_norm_weight[i]);
if (dtheta_abs > mx)
mx = dtheta_abs;
}

74
intern/iksolver/intern/IK_QJacobianSolver.cpp
View File

@@ -45,22 +45,22 @@ IK_QJacobianSolver::IK_QJacobianSolver()
MT_Scalar IK_QJacobianSolver::ComputeScale()
{
std::vector<IK_QSegment*>::iterator seg;
std::vector<IK_QSegment *>::iterator seg;
MT_Scalar length = 0.0f;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
length += (*seg)->MaxExtension();
if(length == 0.0)
if (length == 0.0)
return 1.0;
else
return 1.0 / length;
}
void IK_QJacobianSolver::Scale(MT_Scalar scale, std::list<IK_QTask*>& tasks)
void IK_QJacobianSolver::Scale(MT_Scalar scale, std::list<IK_QTask *>& tasks)
{
std::list<IK_QTask*>::iterator task;
std::vector<IK_QSegment*>::iterator seg;
std::list<IK_QTask *>::iterator task;
std::vector<IK_QSegment *>::iterator seg;
for (task = tasks.begin(); task != tasks.end(); task++)
(*task)->Scale(scale);
@@ -82,13 +82,13 @@ void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
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;
std::vector<IK_QSegment *>::iterator seg;
int num_dof = 0;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
@@ -103,7 +103,7 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks)
int primary_size = 0, primary = 0;
int secondary_size = 0, secondary = 0;
MT_Scalar primary_weight = 0.0, secondary_weight = 0.0;
std::list<IK_QTask*>::iterator task;
std::list<IK_QTask *>::iterator task;
for (task = tasks.begin(); task != tasks.end(); task++) {
IK_QTask *qtask = *task;
@@ -128,20 +128,20 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks)
m_secondary_enabled = (secondary > 0);
// rescale weights of tasks to sum up to 1
MT_Scalar primary_rescale = 1.0/primary_weight;
MT_Scalar primary_rescale = 1.0 / primary_weight;
MT_Scalar secondary_rescale;
if (MT_fuzzyZero(secondary_weight))
secondary_rescale = 0.0;
else
secondary_rescale = 1.0/secondary_weight;
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);
qtask->SetWeight(qtask->Weight() * primary_rescale);
else
qtask->SetWeight(qtask->Weight()*secondary_rescale);
qtask->SetWeight(qtask->Weight() * secondary_rescale);
}
// set matrix sizes
@@ -154,7 +154,7 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks)
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));
m_jacobian.SetDoFWeight((*seg)->DoFId() + i, (*seg)->Weight(i));
return true;
}
@@ -165,7 +165,7 @@ void IK_QJacobianSolver::SetPoleVectorConstraint(IK_QSegment *tip, MT_Vector3& g
m_poletip = tip;
m_goal = goal;
m_polegoal = polegoal;
m_poleangle = (getangle)? 0.0f: poleangle;
m_poleangle = (getangle) ? 0.0f : poleangle;
m_getpoleangle = getangle;
}
@@ -182,7 +182,7 @@ static MT_Vector3 normalize(const MT_Vector3& v)
// a sane normalize function that doesn't give (1, 0, 0) in case
// of a zero length vector, like MT_Vector3.normalize
MT_Scalar len = v.length();
return MT_fuzzyZero(len)? MT_Vector3(0, 0, 0): v/len;
return MT_fuzzyZero(len) ? MT_Vector3(0, 0, 0) : v / len;
}
static float angle(const MT_Vector3& v1, const MT_Vector3& v2)
@@ -190,21 +190,21 @@ static float angle(const MT_Vector3& v1, const MT_Vector3& v2)
return safe_acos(v1.dot(v2));
}
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)
if (!m_poleconstraint)
return;
// disable pole vector constraint in case of multiple position tasks
std::list<IK_QTask*>::iterator task;
std::list<IK_QTask *>::iterator task;
int positiontasks = 0;
for (task = tasks.begin(); task != tasks.end(); task++)
if((*task)->PositionTask())
if ((*task)->PositionTask())
positiontasks++;
if (positiontasks >= 2) {
@@ -223,12 +223,12 @@ void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTa
// 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.
MT_Vector3 dir = normalize(endpos - rootpos);
MT_Vector3 rootx= rootbasis.getColumn(0);
MT_Vector3 rootz= rootbasis.getColumn(2);
MT_Vector3 up = rootx*cos(m_poleangle) + rootz*sin(m_poleangle);
MT_Vector3 rootx = rootbasis.getColumn(0);
MT_Vector3 rootz = rootbasis.getColumn(2);
MT_Vector3 up = rootx * cos(m_poleangle) + rootz *sin(m_poleangle);
// in post, don't rotate towards the goal but only correct the pole up
MT_Vector3 poledir = (m_getpoleangle)? dir: normalize(m_goal - rootpos);
MT_Vector3 poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos);
MT_Vector3 poleup = normalize(m_polegoal - rootpos);
MT_Matrix3x3 mat, polemat;
@@ -241,11 +241,11 @@ void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTa
polemat[1] = MT_cross(polemat[0], poledir);
polemat[2] = -poledir;
if(m_getpoleangle) {
if (m_getpoleangle) {
// we compute the pole angle that to rotate towards the target
m_poleangle = angle(mat[1], polemat[1]);
if(rootz.dot(mat[1]*cos(m_poleangle) + mat[0]*sin(m_poleangle)) > 0.0)
if (rootz.dot(mat[1] * cos(m_poleangle) + mat[0] * sin(m_poleangle)) > 0.0)
m_poleangle = -m_poleangle;
// solve again, with the pole angle we just computed
@@ -257,15 +257,15 @@ void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTa
// 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.
MT_Transform trans(MT_Point3(0, 0, 0), polemat.transposed()*mat);
m_rootmatrix = trans*m_rootmatrix;
MT_Transform trans(MT_Point3(0, 0, 0), polemat.transposed() * mat);
m_rootmatrix = trans * m_rootmatrix;
}
}
bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
{
// assing each segment a unique id for the jacobian
std::vector<IK_QSegment*>::iterator seg;
std::vector<IK_QSegment *>::iterator seg;
IK_QSegment *qseg, *minseg = NULL;
MT_Scalar minabsdelta = 1e10, absdelta;
MT_Vector3 delta, mindelta;
@@ -318,11 +318,11 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
}
bool IK_QJacobianSolver::Solve(
IK_QSegment *root,
std::list<IK_QTask*> tasks,
const MT_Scalar,
const int max_iterations
)
IK_QSegment *root,
std::list<IK_QTask *> tasks,
const MT_Scalar,
const int max_iterations
)
{
float scale = ComputeScale();
bool solved = false;
@@ -339,7 +339,7 @@ bool IK_QJacobianSolver::Solve(
// 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++) {
@@ -367,7 +367,7 @@ bool IK_QJacobianSolver::Solve(
} while (UpdateAngles(norm));
// unlock segments again after locking in clamping loop
std::vector<IK_QSegment*>::iterator seg;
std::vector<IK_QSegment *>::iterator seg;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
(*seg)->UnLock();
@@ -383,10 +383,10 @@ bool IK_QJacobianSolver::Solve(
}
}
if(m_poleconstraint)
if (m_poleconstraint)
root->PrependBasis(m_rootmatrix.getBasis());
Scale(1.0f/scale, tasks);
Scale(1.0f / scale, tasks);
//analyze_add_run(max_iterations, analyze_time()-dt);

193
intern/iksolver/intern/IK_QSegment.cpp
View File

@@ -60,17 +60,18 @@ static MT_Matrix3x3 RotationMatrix(MT_Scalar angle, int axis)
static MT_Scalar EulerAngleFromMatrix(const MT_Matrix3x3& R, int axis)
{
MT_Scalar t = sqrt(R[0][0]*R[0][0] + R[0][1]*R[0][1]);
MT_Scalar t = sqrt(R[0][0] * R[0][0] + R[0][1] * R[0][1]);
if (t > 16.0*MT_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;
}
if (t > 16.0 * MT_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 MT_Scalar safe_acos(MT_Scalar f)
@@ -89,7 +90,7 @@ static MT_Scalar ComputeTwist(const MT_Matrix3x3& R)
MT_Scalar qy = R[0][2] - R[2][0];
MT_Scalar qw = R[0][0] + R[1][1] + R[2][2] + 1;
MT_Scalar tau = 2*atan2(qy, qw);
MT_Scalar tau = 2.0 * atan2(qy, qw);
return tau;
}
@@ -108,7 +109,7 @@ static void RemoveTwist(MT_Matrix3x3& R)
MT_Matrix3x3 T = ComputeTwistMatrix(tau);
// remove twist
R = R*T.transposed();
R = R * T.transposed();
}
static MT_Vector3 SphericalRangeParameters(const MT_Matrix3x3& R)
@@ -117,7 +118,7 @@ static MT_Vector3 SphericalRangeParameters(const MT_Matrix3x3& R)
MT_Scalar tau = ComputeTwist(R);
// compute swing parameters
MT_Scalar num = 2.0*(1.0 + R[1][1]);
MT_Scalar num = 2.0 * (1.0 + R[1][1]);
// singularity at pi
if (MT_abs(num) < MT_EPSILON)
@@ -126,9 +127,9 @@ static MT_Vector3 SphericalRangeParameters(const MT_Matrix3x3& R)
// enforce limits at all then
return MT_Vector3(0.0, tau, 1.0);
num = 1.0/sqrt(num);
MT_Scalar ax = -R[2][1]*num;
MT_Scalar az = R[0][1]*num;
num = 1.0 / sqrt(num);
MT_Scalar ax = -R[2][1] * num;
MT_Scalar az = R[0][1] * num;
return MT_Vector3(ax, tau, az);
}
@@ -136,8 +137,8 @@ static MT_Vector3 SphericalRangeParameters(const MT_Matrix3x3& R)
static MT_Matrix3x3 ComputeSwingMatrix(MT_Scalar ax, MT_Scalar az)
{
// length of (ax, 0, az) = sin(theta/2)
MT_Scalar sine2 = ax*ax + az*az;
MT_Scalar cosine2 = sqrt((sine2 >= 1.0)? 0.0: 1.0-sine2);
MT_Scalar sine2 = ax * ax + az * az;
MT_Scalar cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2);
// compute swing matrix
MT_Matrix3x3 S(MT_Quaternion(ax, 0.0, az, -cosine2));
@@ -151,11 +152,11 @@ static MT_Vector3 MatrixToAxisAngle(const MT_Matrix3x3& R)
R[0][2] - R[2][0],
R[1][0] - R[0][1]);
MT_Scalar c = safe_acos((R[0][0] + R[1][1] + R[2][2] - 1)/2);
MT_Scalar c = safe_acos((R[0][0] + R[1][1] + R[2][2] - 1) / 2);
MT_Scalar l = delta.length();
if (!MT_fuzzyZero(l))
delta *= c/l;
delta *= c / l;
return delta;
}
@@ -192,10 +193,10 @@ static bool EllipseClamp(MT_Scalar& ax, MT_Scalar& az, MT_Scalar *amin, MT_Scala
z = zlim;
}
else {
MT_Scalar invx = 1.0/(xlim*xlim);
MT_Scalar invz = 1.0/(zlim*zlim);
MT_Scalar invx = 1.0 / (xlim * xlim);
MT_Scalar invz = 1.0 / (zlim * zlim);
if ((x*x*invx + z*z*invz) <= 1.0)
if ((x * x * invx + z * z * invz) <= 1.0)
return false;
if (MT_fuzzyZero(x)) {
@@ -203,17 +204,17 @@ static bool EllipseClamp(MT_Scalar& ax, MT_Scalar& az, MT_Scalar *amin, MT_Scala
z = zlim;
}
else {
MT_Scalar rico = z/x;
MT_Scalar rico = z / x;
MT_Scalar old_x = x;
x = sqrt(1.0/(invx + invz*rico*rico));
x = sqrt(1.0 / (invx + invz * rico * rico));
if (old_x < 0.0)
x = -x;
z = rico*x;
z = rico * x;
}
}
ax = (ax < 0.0)? -x: x;
az = (az < 0.0)? -z: z;
ax = (ax < 0.0) ? -x : x;
az = (az < 0.0) ? -z : z;
return true;
}
@@ -221,8 +222,8 @@ static bool EllipseClamp(MT_Scalar& ax, MT_Scalar& az, MT_Scalar *amin, MT_Scala
// 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;
@@ -251,11 +252,11 @@ void IK_QSegment::Reset()
}
void IK_QSegment::SetTransform(
const MT_Vector3& start,
const MT_Matrix3x3& rest_basis,
const MT_Matrix3x3& basis,
const MT_Scalar length
)
const MT_Vector3& start,
const MT_Matrix3x3& rest_basis,
const MT_Matrix3x3& basis,
const MT_Scalar length
)
{
m_max_extension = start.length() + length;
@@ -271,7 +272,7 @@ void IK_QSegment::SetTransform(
MT_Matrix3x3 IK_QSegment::BasisChange() const
{
return m_orig_basis.transposed()*m_basis;
return m_orig_basis.transposed() * m_basis;
}
MT_Vector3 IK_QSegment::TranslationChange() const
@@ -329,7 +330,7 @@ void IK_QSegment::RemoveChild(IK_QSegment *child)
void IK_QSegment::UpdateTransform(const MT_Transform& global)
{
// compute the global transform at the end of the segment
m_global_start = global.getOrigin() + global.getBasis()*m_start;
m_global_start = global.getOrigin() + global.getBasis() * m_start;
m_global_transform.setOrigin(m_global_start);
m_global_transform.setBasis(global.getBasis() * m_rest_basis * m_basis);
@@ -358,7 +359,7 @@ void IK_QSegment::Scale(MT_Scalar 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)
{
}
@@ -386,8 +387,8 @@ void IK_QSphericalSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
lmin = MT_clamp(lmin, -MT_PI, MT_PI);
lmax = MT_clamp(lmax, -MT_PI, MT_PI);
lmin = sin(lmin*0.5);
lmax = sin(lmax*0.5);
lmin = sin(lmin * 0.5);
lmax = sin(lmax * 0.5);
if (axis == 0) {
m_min[0] = -lmax;
@@ -414,8 +415,8 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&
MT_Vector3 dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
dq.y() = jacobian.AngleUpdate(m_DoF_id+1);
dq.z() = jacobian.AngleUpdate(m_DoF_id+2);
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.
@@ -423,29 +424,29 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&
MT_Scalar theta = dq.length();
if (!MT_fuzzyZero(theta)) {
MT_Vector3 w = dq*(1.0/theta);
MT_Vector3 w = dq * (1.0 / theta);
MT_Scalar sine = sin(theta);
MT_Scalar cosine = cos(theta);
MT_Scalar cosineInv = 1-cosine;
MT_Scalar cosineInv = 1 - cosine;
MT_Scalar xsine = w.x()*sine;
MT_Scalar ysine = w.y()*sine;
MT_Scalar zsine = w.z()*sine;
MT_Scalar xsine = w.x() * sine;
MT_Scalar ysine = w.y() * sine;
MT_Scalar zsine = w.z() * sine;
MT_Scalar xxcosine = w.x()*w.x()*cosineInv;
MT_Scalar xycosine = w.x()*w.y()*cosineInv;
MT_Scalar xzcosine = w.x()*w.z()*cosineInv;
MT_Scalar yycosine = w.y()*w.y()*cosineInv;
MT_Scalar yzcosine = w.y()*w.z()*cosineInv;
MT_Scalar zzcosine = w.z()*w.z()*cosineInv;
MT_Scalar xxcosine = w.x() * w.x() * cosineInv;
MT_Scalar xycosine = w.x() * w.y() * cosineInv;
MT_Scalar xzcosine = w.x() * w.z() * cosineInv;
MT_Scalar yycosine = w.y() * w.y() * cosineInv;
MT_Scalar yzcosine = w.y() * w.z() * cosineInv;
MT_Scalar zzcosine = w.z() * w.z() * cosineInv;
MT_Matrix3x3 M(
cosine + xxcosine, -zsine + xycosine, ysine + xzcosine,
zsine + xycosine, cosine + yycosine, -xsine + yzcosine,
-ysine + xzcosine, xsine + yzcosine, cosine + zzcosine);
cosine + xxcosine, -zsine + xycosine, ysine + xzcosine,
zsine + xycosine, cosine + yycosine, -xsine + yzcosine,
-ysine + xzcosine, xsine + yzcosine, cosine + zzcosine);
m_new_basis = m_basis*M;
m_new_basis = m_basis * M;
}
else
m_new_basis = m_basis;
@@ -505,13 +506,13 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3&
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);
m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
return false;
}
m_new_basis = ComputeSwingMatrix(ax, az)*ComputeTwistMatrix(ay);
m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
delta = MatrixToAxisAngle(m_basis.transposed()*m_new_basis);
delta = MatrixToAxisAngle(m_basis.transposed() * m_new_basis);
if (!(m_locked[0] || m_locked[2]) && (clamp[0] || clamp[2])) {
m_locked_ax = ax;
@@ -528,12 +529,12 @@ void IK_QSphericalSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& del
{
if (dof == 1) {
m_locked[1] = true;
jacobian.Lock(m_DoF_id+1, delta[1]);
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]);
jacobian.Lock(m_DoF_id + 2, delta[2]);
}
}
@@ -545,14 +546,14 @@ void IK_QSphericalSegment::UpdateAngleApply()
// IK_QNullSegment
IK_QNullSegment::IK_QNullSegment()
: IK_QSegment(0, false)
: 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)
{
}
@@ -634,7 +635,7 @@ void IK_QRevoluteSegment::SetWeight(int axis, MT_Scalar weight)
// IK_QSwingSegment
IK_QSwingSegment::IK_QSwingSegment()
: IK_QSegment(2, false), m_limit_x(false), m_limit_z(false)
: IK_QSegment(2, false), m_limit_x(false), m_limit_z(false)
{
}
@@ -646,7 +647,7 @@ void IK_QSwingSegment::SetBasis(const MT_Matrix3x3& basis)
MT_Vector3 IK_QSwingSegment::Axis(int dof) const
{
return m_global_transform.getBasis().getColumn((dof==0)? 0: 2);
return m_global_transform.getBasis().getColumn((dof == 0) ? 0 : 2);
}
bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
@@ -657,7 +658,7 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& del
MT_Vector3 dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
dq.y() = 0.0;
dq.z() = jacobian.AngleUpdate(m_DoF_id+1);
dq.z() = jacobian.AngleUpdate(m_DoF_id + 1);
// Directly update the rotation matrix, with Rodrigues' rotation formula,
// to avoid singularities and allow smooth integration.
@@ -665,25 +666,25 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& del
MT_Scalar theta = dq.length();
if (!MT_fuzzyZero(theta)) {
MT_Vector3 w = dq*(1.0/theta);
MT_Vector3 w = dq * (1.0 / theta);
MT_Scalar sine = sin(theta);
MT_Scalar cosine = cos(theta);
MT_Scalar cosineInv = 1-cosine;
MT_Scalar cosineInv = 1 - cosine;
MT_Scalar xsine = w.x()*sine;
MT_Scalar zsine = w.z()*sine;
MT_Scalar xsine = w.x() * sine;
MT_Scalar zsine = w.z() * sine;
MT_Scalar xxcosine = w.x()*w.x()*cosineInv;
MT_Scalar xzcosine = w.x()*w.z()*cosineInv;
MT_Scalar zzcosine = w.z()*w.z()*cosineInv;
MT_Scalar xxcosine = w.x() * w.x() * cosineInv;
MT_Scalar xzcosine = w.x() * w.z() * cosineInv;
MT_Scalar zzcosine = w.z() * w.z() * cosineInv;
MT_Matrix3x3 M(
cosine + xxcosine, -zsine, xzcosine,
zsine, cosine, -xsine,
xzcosine, xsine, cosine + zzcosine);
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);
}
@@ -731,7 +732,7 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& del
m_new_basis = ComputeSwingMatrix(ax, az);
delta = MatrixToAxisAngle(m_basis.transposed()*m_new_basis);
delta = MatrixToAxisAngle(m_basis.transposed() * m_new_basis);
delta[1] = delta[2]; delta[2] = 0.0;
return true;
@@ -741,7 +742,7 @@ void IK_QSwingSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta)
{
m_locked[0] = m_locked[1] = true;
jacobian.Lock(m_DoF_id, delta[0]);
jacobian.Lock(m_DoF_id+1, delta[1]);
jacobian.Lock(m_DoF_id + 1, delta[1]);
}
void IK_QSwingSegment::UpdateAngleApply()
@@ -758,11 +759,11 @@ void IK_QSwingSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
lmin = MT_clamp(lmin, -MT_PI, MT_PI);
lmax = MT_clamp(lmax, -MT_PI, MT_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
MT_Scalar offset = 0.5*(lmin + lmax);
MT_Scalar offset = 0.5 * (lmin + lmax);
//lmax = lmax - offset;
if (axis == 0) {
@@ -794,8 +795,8 @@ void IK_QSwingSegment::SetWeight(int axis, MT_Scalar 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)
{
}
@@ -807,7 +808,7 @@ void IK_QElbowSegment::SetBasis(const MT_Matrix3x3& 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);
}
MT_Vector3 IK_QElbowSegment::Axis(int dof) const
@@ -850,7 +851,7 @@ bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& del
}
if (!m_locked[1]) {
m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id+1);
m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id + 1);
if (m_limit_twist) {
if (m_new_twist > m_max_twist) {
@@ -877,7 +878,7 @@ void IK_QElbowSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
}
else {
m_locked[1] = true;
jacobian.Lock(m_DoF_id+1, delta[1]);
jacobian.Lock(m_DoF_id + 1, delta[1]);
}
}
@@ -892,7 +893,7 @@ void IK_QElbowSegment::UpdateAngleApply()
MT_Matrix3x3 A = RotationMatrix(m_angle, m_axis);
MT_Matrix3x3 T = RotationMatrix(m_sin_twist, m_cos_twist, 1);
m_basis = A*T;
m_basis = A * T;
}
void IK_QElbowSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
@@ -927,7 +928,7 @@ void IK_QElbowSegment::SetWeight(int axis, MT_Scalar weight)
// IK_QTranslateSegment
IK_QTranslateSegment::IK_QTranslateSegment(int axis1)
: IK_QSegment(1, true)
: IK_QSegment(1, true)
{
m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
m_axis_enabled[axis1] = true;
@@ -938,7 +939,7 @@ IK_QTranslateSegment::IK_QTranslateSegment(int axis1)
}
IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2)
: IK_QSegment(2, true)
: IK_QSegment(2, true)
{
m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
m_axis_enabled[axis1] = true;
@@ -951,7 +952,7 @@ IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2)
}
IK_QTranslateSegment::IK_QTranslateSegment()
: IK_QSegment(3, true)
: IK_QSegment(3, true)
{
m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = true;
@@ -1013,7 +1014,7 @@ void IK_QTranslateSegment::UpdateAngleApply()
void IK_QTranslateSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
{
m_locked[dof] = true;
jacobian.Lock(m_DoF_id+dof, delta[dof]);
jacobian.Lock(m_DoF_id + dof, delta[dof]);
}
void IK_QTranslateSegment::SetWeight(int axis, MT_Scalar weight)
@@ -1030,9 +1031,9 @@ void IK_QTranslateSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
if (lmax < lmin)
return;
m_min[axis]= lmin;
m_max[axis]= lmax;
m_limit[axis]= true;
m_min[axis] = lmin;
m_max[axis] = lmax;
m_limit[axis] = true;
}
void IK_QTranslateSegment::Scale(MT_Scalar scale)

80
intern/iksolver/intern/IK_QTask.cpp
View File

@@ -36,11 +36,11 @@
// IK_QTask
IK_QTask::IK_QTask(
int size,
bool primary,
bool active,
const IK_QSegment *segment
) :
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)
{
@@ -49,10 +49,10 @@ IK_QTask::IK_QTask(
// IK_QPositionTask
IK_QPositionTask::IK_QPositionTask(
bool primary,
const IK_QSegment *segment,
const MT_Vector3& goal
) :
bool primary,
const IK_QSegment *segment,
const MT_Vector3& goal
) :
IK_QTask(3, primary, true, segment), m_goal(goal)
{
// computing clamping length
@@ -67,7 +67,7 @@ IK_QPositionTask::IK_QPositionTask(
num++;
}
m_clamp_length /= 2*num;
m_clamp_length /= 2 * num;
}
void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
@@ -79,9 +79,9 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
MT_Scalar length = d_pos.length();
if (length > m_clamp_length)
d_pos = (m_clamp_length/length)*d_pos;
d_pos = (m_clamp_length / length) * d_pos;
jacobian.SetBetas(m_id, m_size, m_weight*d_pos);
jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
// compute derivatives
int i;
@@ -91,13 +91,13 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
MT_Vector3 p = seg->GlobalStart() - pos;
for (i = 0; i < seg->NumberOfDoF(); i++) {
MT_Vector3 axis = seg->Axis(i)*m_weight;
MT_Vector3 axis = seg->Axis(i) * m_weight;
if (seg->Translational())
jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis, 1e2);
jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e2);
else {
MT_Vector3 pa = p.cross(axis);
jacobian.SetDerivatives(m_id, seg->DoFId()+i, pa, 1e0);
jacobian.SetDerivatives(m_id, seg->DoFId() + i, pa, 1e0);
}
}
}
@@ -113,10 +113,10 @@ MT_Scalar IK_QPositionTask::Distance() const
// IK_QOrientationTask
IK_QOrientationTask::IK_QOrientationTask(
bool primary,
const IK_QSegment *segment,
const MT_Matrix3x3& goal
) :
bool primary,
const IK_QSegment *segment,
const MT_Matrix3x3& goal
) :
IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0)
{
}
@@ -126,17 +126,17 @@ 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();
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]);
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);
jacobian.SetBetas(m_id, m_size, m_weight * d_rot);
// compute derivatives
int i;
@@ -146,10 +146,10 @@ void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian)
for (i = 0; i < seg->NumberOfDoF(); i++) {
if (seg->Translational())
jacobian.SetDerivatives(m_id, seg->DoFId()+i, MT_Vector3(0, 0, 0), 1e2);
jacobian.SetDerivatives(m_id, seg->DoFId() + i, MT_Vector3(0, 0, 0), 1e2);
else {
MT_Vector3 axis = seg->Axis(i)*m_weight;
jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis, 1e0);
MT_Vector3 axis = seg->Axis(i) * m_weight;
jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e0);
}
}
}
@@ -158,15 +158,15 @@ void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian)
// Note: implementation not finished!
IK_QCenterOfMassTask::IK_QCenterOfMassTask(
bool primary,
const IK_QSegment *segment,
const MT_Vector3& goal_center
) :
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;
m_total_mass_inv = 1.0 / m_total_mass_inv;
}
MT_Scalar IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
@@ -182,7 +182,7 @@ MT_Scalar IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
MT_Vector3 IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
{
MT_Vector3 center = /*seg->Mass()**/segment->GlobalStart();
MT_Vector3 center = /*seg->Mass()**/ segment->GlobalStart();
const IK_QSegment *seg;
for (seg = segment->Child(); seg; seg = seg->Sibling())
@@ -197,14 +197,14 @@ void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& c
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;
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, 1e2);
jacobian.SetDerivatives(m_id, segment->DoFId() + i, axis, 1e2);
else {
MT_Vector3 pa = axis.cross(p);
jacobian.SetDerivatives(m_id, segment->DoFId()+i, pa, 1e0);
jacobian.SetDerivatives(m_id, segment->DoFId() + i, pa, 1e0);
}
}
@@ -215,7 +215,7 @@ void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& c
void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian& jacobian)
{
MT_Vector3 center = ComputeCenter(m_segment)*m_total_mass_inv;
MT_Vector3 center = ComputeCenter(m_segment) * m_total_mass_inv;
// compute beta
MT_Vector3 d_pos = m_goal_center - center;
@@ -224,10 +224,10 @@ void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian& jacobian)
#if 0
if (m_distance > m_clamp_length)
d_pos = (m_clamp_length/m_distance)*d_pos;
d_pos = (m_clamp_length / m_distance) * d_pos;
#endif
jacobian.SetBetas(m_id, m_size, m_weight*d_pos);
jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
// compute derivatives
JacobianSegment(jacobian, center, m_segment);

87
intern/iksolver/intern/IK_Solver.cpp
View File

@@ -42,20 +42,21 @@ using namespace std;
class IK_QSolver {
public:
IK_QSolver() : root(NULL) {};
IK_QSolver() : root(NULL) {
};
IK_QJacobianSolver solver;
IK_QSegment *root;
std::list<IK_QTask*> tasks;
std::list<IK_QTask *> tasks;
};
// FIXME: locks still result in small "residual" changes to the locked axes...
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;
ndof += (flag & IK_XDOF) ? 1 : 0;
ndof += (flag & IK_YDOF) ? 1 : 0;
ndof += (flag & IK_ZDOF) ? 1 : 0;
IK_QSegment *seg;
@@ -78,7 +79,7 @@ IK_QSegment *CreateSegment(int flag, bool translate)
if (flag & IK_XDOF) {
axis1 = 0;
axis2 = (flag & IK_YDOF)? 1: 2;
axis2 = (flag & IK_YDOF) ? 1 : 2;
}
else {
axis1 = 1;
@@ -91,7 +92,7 @@ IK_QSegment *CreateSegment(int flag, bool translate)
if (axis1 + axis2 == 2)
seg = new IK_QSwingSegment();
else
seg = new IK_QElbowSegment((axis1 == 0)? 0: 2);
seg = new IK_QElbowSegment((axis1 == 0) ? 0 : 2);
}
}
else {
@@ -131,7 +132,7 @@ IK_Segment *IK_CreateSegment(int flag)
void IK_FreeSegment(IK_Segment *seg)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
if (qseg->Composite())
delete qseg->Composite();
@@ -140,8 +141,8 @@ void IK_FreeSegment(IK_Segment *seg)
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());
@@ -151,7 +152,7 @@ void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
MT_Vector3 mstart(start);
// convert from blender column major to moto row major
@@ -177,19 +178,19 @@ void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float bas
void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
if (axis >= IK_TRANS_X) {
if(!qseg->Translational()) {
if(qseg->Composite() && qseg->Composite()->Translational())
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;
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);
@@ -203,19 +204,19 @@ void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
if (stiffness > 0.999f)
stiffness = 0.999f;
IK_QSegment *qseg = (IK_QSegment*)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
MT_Scalar weight = 1.0f - stiffness;
if (axis >= IK_TRANS_X) {
if(!qseg->Translational()) {
if(qseg->Composite() && qseg->Composite()->Translational())
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;
if (axis == IK_TRANS_X) axis = IK_X;
else if (axis == IK_TRANS_Y) axis = IK_Y;
else axis = IK_Z;
}
@@ -224,7 +225,7 @@ void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
{
IK_QSegment *qseg = (IK_QSegment*)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
if (qseg->Translational() && qseg->Composite())
qseg = qseg->Composite();
@@ -245,7 +246,7 @@ void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
if (!qseg->Translational() && qseg->Composite())
qseg = qseg->Composite();
@@ -263,9 +264,9 @@ IK_Solver *IK_CreateSolver(IK_Segment *root)
return NULL;
IK_QSolver *solver = new IK_QSolver();
solver->root = (IK_QSegment*)root;
solver->root = (IK_QSegment *)root;
return (IK_Solver*)solver;
return (IK_Solver *)solver;
}
void IK_FreeSolver(IK_Solver *solver)
@@ -273,9 +274,9 @@ void IK_FreeSolver(IK_Solver *solver)
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);
@@ -288,8 +289,8 @@ void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float w
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;
if (qtip->Composite())
qtip = qtip->Composite();
@@ -306,8 +307,8 @@ void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[
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;
if (qtip->Composite())
qtip = qtip->Composite();
@@ -327,14 +328,14 @@ void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float
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;
MT_Vector3 qgoal(goal);
MT_Vector3 qpolegoal(polegoal);
qsolver->solver.SetPoleVectorConstraint(
qtip, qgoal, qpolegoal, poleangle, getangle);
qtip, qgoal, qpolegoal, poleangle, getangle);
}
float IK_SolverGetPoleAngle(IK_Solver *solver)
@@ -342,7 +343,7 @@ float IK_SolverGetPoleAngle(IK_Solver *solver)
if (solver == NULL)
return 0.0f;
IK_QSolver *qsolver = (IK_QSolver*)solver;
IK_QSolver *qsolver = (IK_QSolver *)solver;
return qsolver->solver.GetPoleAngle();
}
@@ -352,8 +353,8 @@ void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3]
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 to moto row major
MT_Vector3 center(goal);
@@ -368,18 +369,18 @@ int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
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;
std::list<IK_QTask *>& tasks = qsolver->tasks;
MT_Scalar tol = tolerance;
if(!jacobian.Setup(root, tasks))
if (!jacobian.Setup(root, tasks))
return 0;
bool result = jacobian.Solve(root, tasks, tol, max_iterations);
return ((result)? 1: 0);
return ((result) ? 1 : 0);
}

128
intern/iksolver/intern/MT_ExpMap.cpp
View File

@@ -37,11 +37,11 @@
* Set the exponential map from a quaternion. The quaternion must be non-zero.
*/
void
void
MT_ExpMap::
setRotation(
const MT_Quaternion &q
) {
const MT_Quaternion &q)
{
// ok first normalize the quaternion
// then compute theta the axis-angle and the normalized axis v
// scale v by theta and that's it hopefully!
@@ -53,7 +53,7 @@ setRotation(
m_sinp = m_v.length();
m_v /= m_sinp;
m_theta = atan2(double(m_sinp),double(cosp));
m_theta = atan2(double(m_sinp), double(cosp));
m_v *= m_theta;
}
@@ -62,10 +62,10 @@ setRotation(
* representation
*/
const MT_Quaternion&
const MT_Quaternion&
MT_ExpMap::
getRotation(
) const {
getRotation() const
{
return m_q;
}
@@ -73,10 +73,10 @@ getRotation(
* Convert the exponential map to a 3x3 matrix
*/
MT_Matrix3x3
MT_Matrix3x3
MT_ExpMap::
getMatrix(
) const {
getMatrix() const
{
return MT_Matrix3x3(m_q);
}
@@ -84,11 +84,11 @@ getMatrix(
* Update & reparameterizate the exponential map
*/
void
void
MT_ExpMap::
update(
const MT_Vector3& dv
){
const MT_Vector3& dv)
{
m_v += dv;
angleUpdated();
@@ -100,14 +100,13 @@ update(
* from the map) and return them as a 3x3 matrix
*/
void
void
MT_ExpMap::
partialDerivatives(
MT_Matrix3x3& dRdx,
MT_Matrix3x3& dRdy,
MT_Matrix3x3& dRdz
) const {
MT_Matrix3x3& dRdx,
MT_Matrix3x3& dRdy,
MT_Matrix3x3& dRdz) const
{
MT_Quaternion dQdx[3];
compute_dQdVi(dQdx);
@@ -117,29 +116,28 @@ partialDerivatives(
compute_dRdVi(dQdx[2], dRdz);
}
void
void
MT_ExpMap::
compute_dRdVi(
const MT_Quaternion &dQdvi,
MT_Matrix3x3 & dRdvi
) const {
MT_Scalar prod[9];
const MT_Quaternion &dQdvi,
MT_Matrix3x3 & dRdvi) const
{
MT_Scalar prod[9];
/* This efficient formulation is arrived at by writing out the
* entire chain rule product dRdq * dqdv in terms of 'q' and
* noticing that all the entries are formed from sums of just
* nine products of 'q' and 'dqdv' */
prod[0] = -MT_Scalar(4)*m_q.x()*dQdvi.x();
prod[1] = -MT_Scalar(4)*m_q.y()*dQdvi.y();
prod[2] = -MT_Scalar(4)*m_q.z()*dQdvi.z();
prod[3] = MT_Scalar(2)*(m_q.y()*dQdvi.x() + m_q.x()*dQdvi.y());
prod[4] = MT_Scalar(2)*(m_q.w()*dQdvi.z() + m_q.z()*dQdvi.w());
prod[5] = MT_Scalar(2)*(m_q.z()*dQdvi.x() + m_q.x()*dQdvi.z());
prod[6] = MT_Scalar(2)*(m_q.w()*dQdvi.y() + m_q.y()*dQdvi.w());
prod[7] = MT_Scalar(2)*(m_q.z()*dQdvi.y() + m_q.y()*dQdvi.z());
prod[8] = MT_Scalar(2)*(m_q.w()*dQdvi.x() + m_q.x()*dQdvi.w());
prod[0] = -MT_Scalar(4) * m_q.x() * dQdvi.x();
prod[1] = -MT_Scalar(4) * m_q.y() * dQdvi.y();
prod[2] = -MT_Scalar(4) * m_q.z() * dQdvi.z();
prod[3] = MT_Scalar(2) * (m_q.y() * dQdvi.x() + m_q.x() * dQdvi.y());
prod[4] = MT_Scalar(2) * (m_q.w() * dQdvi.z() + m_q.z() * dQdvi.w());
prod[5] = MT_Scalar(2) * (m_q.z() * dQdvi.x() + m_q.x() * dQdvi.z());
prod[6] = MT_Scalar(2) * (m_q.w() * dQdvi.y() + m_q.y() * dQdvi.w());
prod[7] = MT_Scalar(2) * (m_q.z() * dQdvi.y() + m_q.y() * dQdvi.z());
prod[8] = MT_Scalar(2) * (m_q.w() * dQdvi.x() + m_q.x() * dQdvi.w());
/* first row, followed by second and third */
dRdvi[0][0] = prod[1] + prod[2];
@@ -157,61 +155,60 @@ compute_dRdVi(
// compute partial derivatives dQ/dVi
void
void
MT_ExpMap::
compute_dQdVi(
MT_Quaternion *dQdX
) const {
MT_Quaternion *dQdX) const
{
/* This is an efficient implementation of the derivatives given
* in Appendix A of the paper with common subexpressions factored out */
MT_Scalar sinc, termCoeff;
if (m_theta < MT_EXPMAP_MINANGLE) {
sinc = 0.5 - m_theta*m_theta/48.0;
termCoeff = (m_theta*m_theta/40.0 - 1.0)/24.0;
sinc = 0.5 - m_theta * m_theta / 48.0;
termCoeff = (m_theta * m_theta / 40.0 - 1.0) / 24.0;
}
else {
MT_Scalar cosp = m_q.w();
MT_Scalar ang = 1.0/m_theta;
MT_Scalar ang = 1.0 / m_theta;
sinc = m_sinp*ang;
termCoeff = ang*ang*(0.5*cosp - sinc);
sinc = m_sinp * ang;
termCoeff = ang * ang * (0.5 * cosp - sinc);
}
for (int i = 0; i < 3; i++) {
MT_Quaternion& dQdx = dQdX[i];
int i2 = (i+1)%3;
int i3 = (i+2)%3;
int i2 = (i + 1) % 3;
int i3 = (i + 2) % 3;
MT_Scalar term = m_v[i]*termCoeff;
MT_Scalar term = m_v[i] * termCoeff;
dQdx[i] = term*m_v[i] + sinc;
dQdx[i2] = term*m_v[i2];
dQdx[i3] = term*m_v[i3];
dQdx.w() = -0.5*m_v[i]*sinc;
dQdx[i] = term * m_v[i] + sinc;
dQdx[i2] = term * m_v[i2];
dQdx[i3] = term * m_v[i3];
dQdx.w() = -0.5 * m_v[i] * sinc;
}
}
// reParametize away from singularity, updating
// m_v and m_theta
void
void
MT_ExpMap::
reParametrize(
){
reParametrize()
{
if (m_theta > MT_PI) {
MT_Scalar scl = m_theta;
if (m_theta > MT_2_PI){ /* first get theta into range 0..2PI */
if (m_theta > MT_2_PI) { /* first get theta into range 0..2PI */
m_theta = MT_Scalar(fmod(m_theta, MT_2_PI));
scl = m_theta/scl;
scl = m_theta / scl;
m_v *= scl;
}
if (m_theta > MT_PI){
if (m_theta > MT_PI) {
scl = m_theta;
m_theta = MT_2_PI - m_theta;
scl = MT_Scalar(1.0) - MT_2_PI/scl;
scl = MT_Scalar(1.0) - MT_2_PI / scl;
m_v *= scl;
}
}
@@ -219,10 +216,10 @@ reParametrize(
// compute cached variables
void
void
MT_ExpMap::
angleUpdated(
){
angleUpdated()
{
m_theta = m_v.length();
reParametrize();
@@ -233,20 +230,21 @@ angleUpdated(
m_sinp = MT_Scalar(0.0);
/* Taylor Series for sinc */
MT_Vector3 temp = m_v * MT_Scalar(MT_Scalar(.5) - m_theta*m_theta/MT_Scalar(48.0));
MT_Vector3 temp = m_v * MT_Scalar(MT_Scalar(.5) - m_theta * m_theta / MT_Scalar(48.0));
m_q.x() = temp.x();
m_q.y() = temp.y();
m_q.z() = temp.z();
m_q.w() = MT_Scalar(1.0);
} else {
m_sinp = MT_Scalar(sin(.5*m_theta));
}
else {
m_sinp = MT_Scalar(sin(.5 * m_theta));
/* Taylor Series for sinc */
MT_Vector3 temp = m_v * (m_sinp/m_theta);
MT_Vector3 temp = m_v * (m_sinp / m_theta);
m_q.x() = temp.x();
m_q.y() = temp.y();
m_q.z() = temp.z();
m_q.w() = MT_Scalar(cos(.5*m_theta));
m_q.w() = MT_Scalar(cos(0.5 * m_theta));
}
}