Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorCampbell Barton <ideasman42@gmail.com>2012-07-28 02:35:27 +0400
committerCampbell Barton <ideasman42@gmail.com>2012-07-28 02:35:27 +0400
commitf23bfdfab41e3824d5dc04b27f6509eae02da7f2 (patch)
tree5630cb8b711f7448f4f7975a9bda7409ba7af937 /intern/iksolver
parente2360ab49544bb2d40308afd0f793bb11f0d474e (diff)
style cleanup
Diffstat (limited to 'intern/iksolver')
-rw-r--r--intern/iksolver/intern/IK_QJacobian.cpp86
-rw-r--r--intern/iksolver/intern/IK_QJacobianSolver.cpp74
-rw-r--r--intern/iksolver/intern/IK_QSegment.cpp193
-rw-r--r--intern/iksolver/intern/IK_QTask.cpp80
-rw-r--r--intern/iksolver/intern/IK_Solver.cpp87
-rw-r--r--intern/iksolver/intern/MT_ExpMap.cpp128
6 files changed, 324 insertions, 324 deletions
diff --git a/intern/iksolver/intern/IK_QJacobian.cpp b/intern/iksolver/intern/IK_QJacobian.cpp
index e85da6eda4a..bb7b7c5c0b8 100644
--- a/intern/iksolver/intern/IK_QJacobian.cpp
+++ b/intern/iksolver/intern/IK_QJacobian.cpp
@@ -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;
}
diff --git a/intern/iksolver/intern/IK_QJacobianSolver.cpp b/intern/iksolver/intern/IK_QJacobianSolver.cpp
index b310f2cdfdf..43d177d0651 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.cpp
+++ b/intern/iksolver/intern/IK_QJacobianSolver.cpp
@@ -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);
diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp
index 710faa061ce..e511d8233a2 100644
--- a/intern/iksolver/intern/IK_QSegment.cpp
+++ b/intern/iksolver/intern/IK_QSegment.cpp
@@ -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)
diff --git a/intern/iksolver/intern/IK_QTask.cpp b/intern/iksolver/intern/IK_QTask.cpp
index e050bb00658..0ba716ff59d 100644
--- a/intern/iksolver/intern/IK_QTask.cpp
+++ b/intern/iksolver/intern/IK_QTask.cpp
@@ -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);
diff --git a/intern/iksolver/intern/IK_Solver.cpp b/intern/iksolver/intern/IK_Solver.cpp
index 7586e9e6057..8c19c0e2a74 100644
--- a/intern/iksolver/intern/IK_Solver.cpp
+++ b/intern/iksolver/intern/IK_Solver.cpp
@@ -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);
}
diff --git a/intern/iksolver/intern/MT_ExpMap.cpp b/intern/iksolver/intern/MT_ExpMap.cpp
index c997d434861..b2b13acebeb 100644
--- a/intern/iksolver/intern/MT_ExpMap.cpp
+++ b/intern/iksolver/intern/MT_ExpMap.cpp
@@ -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));
}
}