From f23bfdfab41e3824d5dc04b27f6509eae02da7f2 Mon Sep 17 00:00:00 2001 From: Campbell Barton Date: Fri, 27 Jul 2012 22:35:27 +0000 Subject: style cleanup --- intern/iksolver/intern/IK_QJacobian.cpp | 86 ++++++------ intern/iksolver/intern/IK_QJacobianSolver.cpp | 74 +++++----- intern/iksolver/intern/IK_QSegment.cpp | 193 +++++++++++++------------- intern/iksolver/intern/IK_QTask.cpp | 80 +++++------ intern/iksolver/intern/IK_Solver.cpp | 87 ++++++------ intern/iksolver/intern/MT_ExpMap.cpp | 128 +++++++++-------- 6 files changed, 324 insertions(+), 324 deletions(-) (limited to 'intern/iksolver') 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 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 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 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 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::iterator seg; + std::vector::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& tasks) +void IK_QJacobianSolver::Scale(MT_Scalar scale, std::list& tasks) { - std::list::iterator task; - std::vector::iterator seg; + std::list::iterator task; + std::vector::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& tasks) +bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list& tasks) { m_segments.clear(); AddSegmentList(root); // assign each segment a unique id for the jacobian - std::vector::iterator seg; + std::vector::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& 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::iterator task; + std::list::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& 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& 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& tasks) +void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list& 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::iterator task; + std::list::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 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::iterator seg; + std::vector::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 tasks, - const MT_Scalar, - const int max_iterations -) + IK_QSegment *root, + std::list 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::iterator task; + std::list::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::iterator seg; + std::vector::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 tasks; + std::list 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& tasks = qsolver->tasks; - std::list::iterator task; + IK_QSolver *qsolver = (IK_QSolver *)solver; + std::list& tasks = qsolver->tasks; + std::list::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& tasks = qsolver->tasks; + std::list& 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)); } } -- cgit v1.2.3