style cleanup
authorCampbell Barton <ideasman42@gmail.com>
Fri, 27 Jul 2012 22:35:27 +0000 (22:35 +0000)
committerCampbell Barton <ideasman42@gmail.com>
Fri, 27 Jul 2012 22:35:27 +0000 (22:35 +0000)
intern/iksolver/intern/IK_QJacobian.cpp
intern/iksolver/intern/IK_QJacobianSolver.cpp
intern/iksolver/intern/IK_QSegment.cpp
intern/iksolver/intern/IK_QTask.cpp
intern/iksolver/intern/IK_Solver.cpp
intern/iksolver/intern/MT_ExpMap.cpp

index e85da6eda4adb3dd9c356ae65b5c50e868491397..bb7b7c5c0b85c88e916853e131435a5e6f4b4f9a 100644 (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;
        }
index b310f2cdfdf2f60d32afa1e79a9050743303a9c3..43d177d06511422734c8f8aca10f2b0393b877c9 100644 (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);
 
index 710faa061cec8ba21e41e40640b6878da4889e10..e511d8233a2566928a47501feac300492112ec7d 100644 (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)
index e050bb0065876824452c845f90314c16766829da..0ba716ff59da0417af6c797f6fe256e952fd6e84 100644 (file)
 // 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);
index 7586e9e605739aa38f25cec1d4ab38e11b553215..8c19c0e2a74a322084481f567fc6f828e03b7da9 100644 (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);
 }
 
index c997d4348618f68d70806468e4a71536e503042f..b2b13acebeb5aabb2374310f5399b4ed83394bc2 100644 (file)
  * 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));
        }
 }