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

Cleanup: use braces for sources in intern/

Browse Source 
Omitted intern/itasc as some of these sources are from KDL:
https://www.orocos.org/kdl.html
This commit is contained in:
Campbell Barton
2023-09-17 09:01:48 +10:00
parent e3444fd314
commit 5b9740c913
94 changed files with 2091 additions and 1046 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -101,10 +101,12 @@ void IK_QJacobian::Invert()
m_svd_v = svd.matrixV();
}
if (m_sdls)
if (m_sdls) {
InvertSDLS();
else
}
else {
InvertDLS();
}
}
bool IK_QJacobian::ComputeNullProjection()
@@ -113,39 +115,49 @@ bool IK_QJacobian::ComputeNullProjection()
// compute null space projection based on V
int i, j, rank = 0;
for (i = 0; i < m_svd_w.size(); i++)
if (m_svd_w[i] > epsilon)
for (i = 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] > epsilon) {
rank++;
}
}
if (rank < m_task_size)
if (rank < m_task_size) {
return false;
}
MatrixXd basis(m_svd_v.rows(), rank);
int b = 0;
for (i = 0; i < m_svd_w.size(); i++)
for (i = 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] > epsilon) {
for (j = 0; j < m_svd_v.rows(); j++)
for (j = 0; j < m_svd_v.rows(); j++) {
basis(j, b) = m_svd_v(j, i);
}
b++;
}
}
m_nullspace = basis * basis.transpose();
for (i = 0; i < m_nullspace.rows(); i++)
for (j = 0; j < m_nullspace.cols(); j++)
if (i == j)
for (i = 0; i < m_nullspace.rows(); i++) {
for (j = 0; j < m_nullspace.cols(); j++) {
if (i == j) {
m_nullspace(i, j) = 1.0 - m_nullspace(i, j);
else
}
else {
m_nullspace(i, j) = -m_nullspace(i, j);
}
}
}
return true;
}
void IK_QJacobian::SubTask(IK_QJacobian &jacobian)
{
if (!ComputeNullProjection())
if (!ComputeNullProjection()) {
return;
}
// restrict lower priority jacobian
jacobian.Restrict(m_d_theta, m_nullspace);
@@ -157,8 +169,9 @@ void IK_QJacobian::SubTask(IK_QJacobian &jacobian)
// SDLS, to avoid shaking when the primary task is near singularities,
// doesn't work well at all
int i;
for (i = 0; i < m_d_theta.size(); i++)
for (i = 0; i < m_d_theta.size(); i++) {
m_d_theta[i] = m_d_theta[i] + /*m_min_damp * */ jacobian.AngleUpdate(i);
}
}
void IK_QJacobian::Restrict(VectorXd &d_theta, MatrixXd &nullspace)
@@ -209,8 +222,9 @@ void IK_QJacobian::InvertSDLS()
}
for (i = 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] <= epsilon)
if (m_svd_w[i] <= epsilon) {
continue;
}
double wInv = 1.0 / m_svd_w[i];
double alpha = 0.0;
@@ -246,16 +260,18 @@ void IK_QJacobian::InvertSDLS()
// find largest absolute dTheta
// multiply with weight to prevent unnecessary damping
abs_dtheta = fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j];
if (abs_dtheta > max_dtheta)
if (abs_dtheta > max_dtheta) {
max_dtheta = abs_dtheta;
}
}
M *= wInv;
// compute damping term and damp the dTheta's
double gamma = max_angle_change;
if (N < M)
if (N < M) {
gamma *= N / M;
}
double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0;
@@ -265,14 +281,16 @@ void IK_QJacobian::InvertSDLS()
// better to go a little to slow than to far
double dofdamp = damp / m_weight[j];
if (dofdamp > 1.0)
if (dofdamp > 1.0) {
dofdamp = 1.0;
}
m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j];
}
if (damp < m_min_damp)
if (damp < m_min_damp) {
m_min_damp = damp;
}
}
// weight + prevent from doing angle updates with angles > max_angle_change
@@ -283,15 +301,17 @@ void IK_QJacobian::InvertSDLS()
abs_angle = fabs(m_d_theta[j]);
if (abs_angle > max_angle)
if (abs_angle > max_angle) {
max_angle = abs_angle;
}
}
if (max_angle > max_angle_change) {
double 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;
}
}
}
@@ -322,8 +342,9 @@ void IK_QJacobian::InvertDLS()
double w_min = std::numeric_limits<double>::max();
for (i = 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min)
if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min) {
w_min = m_svd_w[i];
}
}
// compute lambda damping term
@@ -331,17 +352,21 @@ void IK_QJacobian::InvertDLS()
double d = x_length / max_angle_change;
double lambda;
if (w_min <= d / 2)
if (w_min <= d / 2) {
lambda = d / 2;
else if (w_min < d)
}
else if (w_min < d) {
lambda = sqrt(w_min * (d - w_min));
else
}
else {
lambda = 0.0;
}
lambda *= lambda;
if (lambda > 10)
if (lambda > 10) {
lambda = 10;
}
// immediately multiply with Beta, so we can do matrix*vector products
// rather than matrix*matrix products
@@ -358,13 +383,15 @@ void IK_QJacobian::InvertDLS()
// compute V*Winv*Ut*Beta
m_svd_u_beta[i] *= wInv;
for (j = 0; j < m_d_theta.size(); j++)
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];
}
}
void IK_QJacobian::Lock(int dof_id, double delta)
@@ -392,8 +419,9 @@ double IK_QJacobian::AngleUpdateNorm() const
for (i = 0; i < m_d_theta.size(); i++) {
dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]);
if (dtheta_abs > mx)
if (dtheta_abs > mx) {
mx = dtheta_abs;
}
}
return mx;

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

@@ -23,13 +23,16 @@ double IK_QJacobianSolver::ComputeScale()
std::vector<IK_QSegment *>::iterator seg;
double length = 0.0f;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
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
}
else {
return 1.0 / length;
}
}
void IK_QJacobianSolver::Scale(double scale, std::list<IK_QTask *> &tasks)
@@ -37,11 +40,13 @@ void IK_QJacobianSolver::Scale(double scale, std::list<IK_QTask *> &tasks)
std::list<IK_QTask *>::iterator task;
std::vector<IK_QSegment *>::iterator seg;
for (task = tasks.begin(); task != tasks.end(); task++)
for (task = tasks.begin(); task != tasks.end(); task++) {
(*task)->Scale(scale);
}
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->Scale(scale);
}
m_rootmatrix.translation() *= scale;
m_goal *= scale;
@@ -53,8 +58,9 @@ void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
m_segments.push_back(seg);
IK_QSegment *child;
for (child = seg->Child(); child; child = child->Sibling())
for (child = seg->Child(); child; child = child->Sibling()) {
AddSegmentList(child);
}
}
bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *> &tasks)
@@ -71,8 +77,9 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *> &tasks)
num_dof += (*seg)->NumberOfDoF();
}
if (num_dof == 0)
if (num_dof == 0) {
return false;
}
// compute task ids and assign weights to task
int primary_size = 0;
@@ -96,39 +103,47 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *> &tasks)
}
}
if (primary_size == 0 || FuzzyZero(primary_weight))
if (primary_size == 0 || FuzzyZero(primary_weight)) {
return false;
}
m_secondary_enabled = (secondary > 0);
// rescale weights of tasks to sum up to 1
double primary_rescale = 1.0 / primary_weight;
double secondary_rescale;
if (FuzzyZero(secondary_weight))
if (FuzzyZero(secondary_weight)) {
secondary_rescale = 0.0;
else
}
else {
secondary_rescale = 1.0 / secondary_weight;
}
for (task = tasks.begin(); task != tasks.end(); task++) {
IK_QTask *qtask = *task;
if (qtask->Primary())
if (qtask->Primary()) {
qtask->SetWeight(qtask->Weight() * primary_rescale);
else
}
else {
qtask->SetWeight(qtask->Weight() * secondary_rescale);
}
}
// set matrix sizes
m_jacobian.ArmMatrices(num_dof, primary_size);
if (secondary > 0)
if (secondary > 0) {
m_jacobian_sub.ArmMatrices(num_dof, secondary_size);
}
// set dof weights
int i;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
for (i = 0; i < (*seg)->NumberOfDoF(); i++)
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));
}
}
return true;
}
@@ -150,16 +165,19 @@ void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTa
// 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;
int positiontasks = 0;
for (task = tasks.begin(); task != tasks.end(); task++)
if ((*task)->PositionTask())
for (task = tasks.begin(); task != tasks.end(); task++) {
if ((*task)->PositionTask()) {
positiontasks++;
}
}
if (positiontasks >= 2) {
m_poleconstraint = false;
@@ -200,8 +218,9 @@ void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTa
m_poleangle = angle(mat.row(1), polemat.row(1));
double dt = rootz.dot(mat.row(1) * cos(m_poleangle) + mat.row(0) * sin(m_poleangle));
if (dt > 0.0)
if (dt > 0.0) {
m_poleangle = -m_poleangle;
}
// solve again, with the pole angle we just computed
m_getpoleangle = false;
@@ -259,16 +278,18 @@ bool IK_QJacobianSolver::UpdateAngles(double &norm)
minseg->Lock(mindof, m_jacobian, mindelta);
locked = true;
if (minabsdelta > norm)
if (minabsdelta > norm) {
norm = minabsdelta;
}
}
if (locked == false)
if (locked == false) {
// no locking done, last inner iteration, apply the angles
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->UnLock();
(*seg)->UpdateAngleApply();
}
}
// signal if another inner iteration is needed
return locked;
@@ -298,10 +319,12 @@ bool IK_QJacobianSolver::Solve(IK_QSegment *root,
// compute jacobian
for (task = tasks.begin(); task != tasks.end(); task++) {
if ((*task)->Primary())
if ((*task)->Primary()) {
(*task)->ComputeJacobian(m_jacobian);
else
}
else {
(*task)->ComputeJacobian(m_jacobian_sub);
}
}
double norm = 0.0;
@@ -310,8 +333,9 @@ bool IK_QJacobianSolver::Solve(IK_QSegment *root,
// invert jacobian
try {
m_jacobian.Invert();
if (m_secondary_enabled)
if (m_secondary_enabled) {
m_jacobian.SubTask(m_jacobian_sub);
}
}
catch (...) {
fprintf(stderr, "IK Exception\n");
@@ -323,13 +347,15 @@ bool IK_QJacobianSolver::Solve(IK_QSegment *root,
// unlock segments again after locking in clamping loop
std::vector<IK_QSegment *>::iterator seg;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->UnLock();
}
// compute angle update norm
double maxnorm = m_jacobian.AngleUpdateNorm();
if (maxnorm > norm)
if (maxnorm > norm) {
norm = maxnorm;
}
// check for convergence
if (norm < 1e-3 && iterations > 10) {
@@ -338,8 +364,9 @@ bool IK_QJacobianSolver::Solve(IK_QSegment *root,
}
}
if (m_poleconstraint)
if (m_poleconstraint) {
root->PrependBasis(m_rootmatrix.linear());
}
Scale(1.0f / scale, tasks);

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

@@ -40,8 +40,9 @@ void IK_QSegment::Reset()
m_translation = m_orig_translation;
SetBasis(m_basis);
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) {
seg->Reset();
}
}
void IK_QSegment::SetTransform(const Vector3d &start,
@@ -73,20 +74,24 @@ Vector3d IK_QSegment::TranslationChange() const
IK_QSegment::~IK_QSegment()
{
if (m_parent)
if (m_parent) {
m_parent->RemoveChild(this);
}
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) {
seg->m_parent = NULL;
}
}
void IK_QSegment::SetParent(IK_QSegment *parent)
{
if (m_parent == parent)
if (m_parent == parent) {
return;
}
if (m_parent)
if (m_parent) {
m_parent->RemoveChild(this);
}
if (parent) {
m_sibling = parent->m_child;
@@ -103,18 +108,22 @@ void IK_QSegment::SetComposite(IK_QSegment *seg)
void IK_QSegment::RemoveChild(IK_QSegment *child)
{
if (m_child == NULL)
if (m_child == NULL) {
return;
else if (m_child == child)
}
else if (m_child == child) {
m_child = m_child->m_sibling;
}
else {
IK_QSegment *seg = m_child;
while (seg->m_sibling != child)
while (seg->m_sibling != child) {
seg = seg->m_sibling;
}
if (child == seg->m_sibling)
if (child == seg->m_sibling) {
seg->m_sibling = child->m_sibling;
}
}
}
@@ -128,8 +137,9 @@ void IK_QSegment::UpdateTransform(const Affine3d &global)
m_global_transform.translate(m_translation);
// update child transforms
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) {
seg->UpdateTransform(m_global_transform);
}
}
void IK_QSegment::PrependBasis(const Matrix3d &mat)
@@ -161,8 +171,9 @@ Vector3d IK_QSphericalSegment::Axis(int dof) const
void IK_QSphericalSegment::SetLimit(int axis, double lmin, double lmax)
{
if (lmin > lmax)
if (lmin > lmax) {
return;
}
if (axis == 1) {
lmin = Clamp(lmin, -M_PI, M_PI);
@@ -201,8 +212,9 @@ void IK_QSphericalSegment::SetWeight(int axis, double weight)
bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
{
if (m_locked[0] && m_locked[1] && m_locked[2])
if (m_locked[0] && m_locked[1] && m_locked[2]) {
return false;
}
Vector3d dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
@@ -244,20 +256,25 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &d
m_new_basis = m_basis * M;
}
else
else {
m_new_basis = m_basis;
}
if (m_limit_y == false && m_limit_x == false && m_limit_z == false)
if (m_limit_y == false && m_limit_x == false && m_limit_z == false) {
return false;
}
Vector3d a = SphericalRangeParameters(m_new_basis);
if (m_locked[0])
if (m_locked[0]) {
a.x() = m_locked_ax;
if (m_locked[1])
}
if (m_locked[1]) {
a.y() = m_locked_ay;
if (m_locked[2])
}
if (m_locked[2]) {
a.z() = m_locked_az;
}
double ax = a.x(), ay = a.y(), az = a.z();
@@ -275,8 +292,9 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &d
}
if (m_limit_x && m_limit_z) {
if (EllipseClamp(ax, az, m_min, m_max))
if (EllipseClamp(ax, az, m_min, m_max)) {
clamp[0] = clamp[2] = true;
}
}
else if (m_limit_x) {
if (ax < m_min[0]) {
@@ -300,8 +318,9 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &d
}
if (clamp[0] == false && clamp[1] == false && clamp[2] == false) {
if (m_locked[0] || m_locked[1] || m_locked[2])
if (m_locked[0] || m_locked[1] || m_locked[2]) {
m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
}
return false;
}
@@ -314,8 +333,9 @@ bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &d
m_locked_az = az;
}
if (!m_locked[1] && clamp[1])
if (!m_locked[1] && clamp[1]) {
m_locked_ay = ay;
}
return true;
}
@@ -368,22 +388,27 @@ Vector3d IK_QRevoluteSegment::Axis(int) const
bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
{
if (m_locked[0])
if (m_locked[0]) {
return false;
}
m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
clamp[0] = false;
if (m_limit == false)
if (m_limit == false) {
return false;
}
if (m_new_angle > m_max)
if (m_new_angle > m_max) {
delta[0] = m_max - m_angle;
else if (m_new_angle < m_min)
}
else if (m_new_angle < m_min) {
delta[0] = m_min - m_angle;
else
}
else {
return false;
}
clamp[0] = true;
m_new_angle = m_angle + delta[0];
@@ -405,8 +430,9 @@ void IK_QRevoluteSegment::UpdateAngleApply()
void IK_QRevoluteSegment::SetLimit(int axis, double lmin, double lmax)
{
if (lmin > lmax || m_axis != axis)
if (lmin > lmax || m_axis != axis) {
return;
}
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
@@ -420,8 +446,9 @@ void IK_QRevoluteSegment::SetLimit(int axis, double lmin, double lmax)
void IK_QRevoluteSegment::SetWeight(int axis, double weight)
{
if (axis == m_axis)
if (axis == m_axis) {
m_weight[0] = weight;
}
}
// IK_QSwingSegment
@@ -441,8 +468,9 @@ Vector3d IK_QSwingSegment::Axis(int dof) const
bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
{
if (m_locked[0] && m_locked[1])
if (m_locked[0] && m_locked[1]) {
return false;
}
Vector3d dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
@@ -482,11 +510,13 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta
RemoveTwist(m_new_basis);
}
else
else {
m_new_basis = m_basis;
}
if (m_limit_x == false && m_limit_z == false)
if (m_limit_x == false && m_limit_z == false) {
return false;
}
Vector3d a = SphericalRangeParameters(m_new_basis);
double ax = 0, az = 0;
@@ -497,8 +527,9 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta
ax = a.x();
az = a.z();
if (EllipseClamp(ax, az, m_min, m_max))
if (EllipseClamp(ax, az, m_min, m_max)) {
clamp[0] = clamp[1] = true;
}
}
else if (m_limit_x) {
if (ax < m_min[0]) {
@@ -521,8 +552,9 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta
}
}
if (clamp[0] == false && clamp[1] == false)
if (clamp[0] == false && clamp[1] == false) {
return false;
}
m_new_basis = ComputeSwingMatrix(ax, az);
@@ -547,8 +579,9 @@ void IK_QSwingSegment::UpdateAngleApply()
void IK_QSwingSegment::SetLimit(int axis, double lmin, double lmax)
{
if (lmin > lmax)
if (lmin > lmax) {
return;
}
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
@@ -581,10 +614,12 @@ void IK_QSwingSegment::SetLimit(int axis, double lmin, double lmax)
void IK_QSwingSegment::SetWeight(int axis, double weight)
{
if (axis == 0)
if (axis == 0) {
m_weight[0] = weight;
else if (axis == 2)
}
else if (axis == 2) {
m_weight[1] = weight;
}
}
// IK_QElbowSegment
@@ -616,21 +651,25 @@ Vector3d IK_QElbowSegment::Axis(int dof) const
{
if (dof == 0) {
Vector3d v;
if (m_axis == 0)
if (m_axis == 0) {
v = Vector3d(m_cos_twist, 0, m_sin_twist);
else
}
else {
v = Vector3d(-m_sin_twist, 0, m_cos_twist);
}
return m_global_transform.linear() * v;
}
else
else {
return m_global_transform.linear().col(1);
}
}
bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
{
if (m_locked[0] && m_locked[1])
if (m_locked[0] && m_locked[1]) {
return false;
}
clamp[0] = clamp[1] = false;
@@ -699,8 +738,9 @@ void IK_QElbowSegment::UpdateAngleApply()
void IK_QElbowSegment::SetLimit(int axis, double lmin, double lmax)
{
if (lmin > lmax)
if (lmin > lmax) {
return;
}
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
@@ -720,10 +760,12 @@ void IK_QElbowSegment::SetLimit(int axis, double lmin, double lmax)
void IK_QElbowSegment::SetWeight(int axis, double weight)
{
if (axis == m_axis)
if (axis == m_axis) {
m_weight[0] = weight;
else if (axis == 1)
}
else if (axis == 1) {
m_weight[1] = weight;
}
}
// IK_QTranslateSegment
@@ -819,15 +861,18 @@ void IK_QTranslateSegment::SetWeight(int axis, double weight)
{
int i;
for (i = 0; i < m_num_DoF; i++)
if (m_axis[i] == axis)
for (i = 0; i < m_num_DoF; i++) {
if (m_axis[i] == axis) {
m_weight[i] = weight;
}
}
}
void IK_QTranslateSegment::SetLimit(int axis, double lmin, double lmax)
{
if (lmax < lmin)
if (lmax < lmin) {
return;
}
m_min[axis] = lmin;
m_max[axis] = lmax;

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

@@ -43,8 +43,9 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian &jacobian)
Vector3d d_pos = m_goal - pos;
double length = d_pos.norm();
if (length > m_clamp_length)
if (length > m_clamp_length) {
d_pos = (m_clamp_length / length) * d_pos;
}
jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
@@ -58,8 +59,9 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian &jacobian)
for (i = 0; i < seg->NumberOfDoF(); i++) {
Vector3d axis = seg->Axis(i) * m_weight;
if (seg->Translational())
if (seg->Translational()) {
jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e2);
}
else {
Vector3d pa = p.cross(axis);
jacobian.SetDerivatives(m_id, seg->DoFId() + i, pa, 1e0);
@@ -104,16 +106,18 @@ void IK_QOrientationTask::ComputeJacobian(IK_QJacobian &jacobian)
int i;
const IK_QSegment *seg;
for (seg = m_segment; seg; seg = seg->Parent())
for (seg = m_segment; seg; seg = seg->Parent()) {
for (i = 0; i < seg->NumberOfDoF(); i++) {
if (seg->Translational())
if (seg->Translational()) {
jacobian.SetDerivatives(m_id, seg->DoFId() + i, Vector3d(0, 0, 0), 1e2);
}
else {
Vector3d axis = seg->Axis(i) * m_weight;
jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e0);
}
}
}
}
// IK_QCenterOfMassTask
@@ -125,8 +129,9 @@ IK_QCenterOfMassTask::IK_QCenterOfMassTask(bool primary,
: IK_QTask(3, primary, true, segment), m_goal_center(goal_center)
{
m_total_mass_inv = ComputeTotalMass(m_segment);
if (!FuzzyZero(m_total_mass_inv))
if (!FuzzyZero(m_total_mass_inv)) {
m_total_mass_inv = 1.0 / m_total_mass_inv;
}
}
double IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
@@ -134,8 +139,9 @@ double IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
double mass = /*seg->Mass()*/ 1.0;
const IK_QSegment *seg;
for (seg = segment->Child(); seg; seg = seg->Sibling())
for (seg = segment->Child(); seg; seg = seg->Sibling()) {
mass += ComputeTotalMass(seg);
}
return mass;
}
@@ -145,8 +151,9 @@ Vector3d IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
Vector3d center = /*seg->Mass()**/ segment->GlobalStart();
const IK_QSegment *seg;
for (seg = segment->Child(); seg; seg = seg->Sibling())
for (seg = segment->Child(); seg; seg = seg->Sibling()) {
center += ComputeCenter(seg);
}
return center;
}
@@ -162,8 +169,9 @@ void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian &jacobian,
Vector3d axis = segment->Axis(i) * m_weight;
axis *= /*segment->Mass()**/ m_total_mass_inv;
if (segment->Translational())
if (segment->Translational()) {
jacobian.SetDerivatives(m_id, segment->DoFId() + i, axis, 1e2);
}
else {
Vector3d pa = axis.cross(p);
jacobian.SetDerivatives(m_id, segment->DoFId() + i, pa, 1e0);
@@ -171,8 +179,9 @@ void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian &jacobian,
}
const IK_QSegment *seg;
for (seg = segment->Child(); seg; seg = seg->Sibling())
for (seg = segment->Child(); seg; seg = seg->Sibling()) {
JacobianSegment(jacobian, center, seg);
}
}
void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian &jacobian)

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

@@ -35,22 +35,28 @@ static IK_QSegment *CreateSegment(int flag, bool translate)
IK_QSegment *seg;
if (ndof == 0)
if (ndof == 0) {
return NULL;
}
else if (ndof == 1) {
int axis;
if (flag & IK_XDOF)
if (flag & IK_XDOF) {
axis = 0;
else if (flag & IK_YDOF)
}
else if (flag & IK_YDOF) {
axis = 1;
else
}
else {
axis = 2;
}
if (translate)
if (translate) {
seg = new IK_QTranslateSegment(axis);
else
}
else {
seg = new IK_QRevoluteSegment(axis);
}
}
else if (ndof == 2) {
int axis1, axis2;
@@ -64,20 +70,25 @@ static IK_QSegment *CreateSegment(int flag, bool translate)
axis2 = 2;
}
if (translate)
if (translate) {
seg = new IK_QTranslateSegment(axis1, axis2);
}
else {
if (axis1 + axis2 == 2)
if (axis1 + axis2 == 2) {
seg = new IK_QSwingSegment();
else
}
else {
seg = new IK_QElbowSegment((axis1 == 0) ? 0 : 2);
}
}
}
else {
if (translate)
if (translate) {
seg = new IK_QTranslateSegment();
else
}
else {
seg = new IK_QSphericalSegment();
}
}
return seg;
@@ -90,10 +101,12 @@ IK_Segment *IK_CreateSegment(int flag)
IK_QSegment *seg;
if (rot == NULL && trans == NULL)
if (rot == NULL && trans == NULL) {
seg = new IK_QNullSegment();
else if (rot == NULL)
}
else if (rot == NULL) {
seg = trans;
}
else {
seg = rot;
@@ -112,8 +125,9 @@ void IK_FreeSegment(IK_Segment *seg)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
if (qseg->Composite())
if (qseg->Composite()) {
delete qseg->Composite();
}
delete qseg;
}
@@ -122,10 +136,12 @@ void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
IK_QSegment *qseg = (IK_QSegment *)seg;
IK_QSegment *qparent = (IK_QSegment *)parent;
if (qparent && qparent->Composite())
if (qparent && qparent->Composite()) {
qseg->SetParent(qparent->Composite());
else
}
else {
qseg->SetParent(qparent);
}
}
void IK_SetTransform(
@@ -163,8 +179,9 @@ void IK_SetTransform(
qseg->SetTransform(mstart, mrest, mbasis, 0.0);
qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
}
else
else {
qseg->SetTransform(mstart, mrest, mbasis, mlength);
}
}
void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
@@ -173,18 +190,23 @@ void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
if (axis >= IK_TRANS_X) {
if (!qseg->Translational()) {
if (qseg->Composite() && qseg->Composite()->Translational())
if (qseg->Composite() && qseg->Composite()->Translational()) {
qseg = qseg->Composite();
else
}
else {
return;
}
}
if (axis == IK_TRANS_X)
if (axis == IK_TRANS_X) {
axis = IK_X;
else if (axis == IK_TRANS_Y)
}
else if (axis == IK_TRANS_Y) {
axis = IK_Y;
else
}
else {
axis = IK_Z;
}
}
qseg->SetLimit(axis, lmin, lmax);
@@ -192,29 +214,36 @@ void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
{
if (stiffness < 0.0f)
if (stiffness < 0.0f) {
return;
}
if (stiffness > (1.0 - IK_STRETCH_STIFF_EPS))
if (stiffness > (1.0 - IK_STRETCH_STIFF_EPS)) {
stiffness = (1.0 - IK_STRETCH_STIFF_EPS);
}
IK_QSegment *qseg = (IK_QSegment *)seg;
double weight = 1.0f - stiffness;
if (axis >= IK_TRANS_X) {
if (!qseg->Translational()) {
if (qseg->Composite() && qseg->Composite()->Translational())
if (qseg->Composite() && qseg->Composite()->Translational()) {
qseg = qseg->Composite();
else
}
else {
return;
}
}
if (axis == IK_TRANS_X)
if (axis == IK_TRANS_X) {
axis = IK_X;
else if (axis == IK_TRANS_Y)
}
else if (axis == IK_TRANS_Y) {
axis = IK_Y;
else
}
else {
axis = IK_Z;
}
}
qseg->SetWeight(axis, weight);
@@ -224,8 +253,9 @@ void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
{
IK_QSegment *qseg = (IK_QSegment *)seg;
if (qseg->Translational() && qseg->Composite())
if (qseg->Translational() && qseg->Composite()) {
qseg = qseg->Composite();
}
const Matrix3d &change = qseg->BasisChange();
@@ -245,8 +275,9 @@ void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
if (!qseg->Translational() && qseg->Composite())
if (!qseg->Translational() && qseg->Composite()) {
qseg = qseg->Composite();
}
const Vector3d &change = qseg->TranslationChange();
@@ -257,8 +288,9 @@ void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
IK_Solver *IK_CreateSolver(IK_Segment *root)
{
if (root == NULL)
if (root == NULL) {
return NULL;
}
IK_QSolver *solver = new IK_QSolver();
solver->root = (IK_QSegment *)root;
@@ -268,30 +300,34 @@ IK_Solver *IK_CreateSolver(IK_Segment *root)
void IK_FreeSolver(IK_Solver *solver)
{
if (solver == NULL)
if (solver == NULL) {
return;
}
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++)
for (task = tasks.begin(); task != tasks.end(); task++) {
delete (*task);
}
delete qsolver;
}
void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
{
if (solver == NULL || tip == NULL)
if (solver == NULL || tip == NULL) {
return;
}
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qtip = (IK_QSegment *)tip;
// in case of composite segment the second segment is the tip
if (qtip->Composite())
if (qtip->Composite()) {
qtip = qtip->Composite();
}
Vector3d pos(goal[0], goal[1], goal[2]);
@@ -302,15 +338,17 @@ void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float w
void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
{
if (solver == NULL || tip == NULL)
if (solver == NULL || tip == NULL) {
return;
}
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qtip = (IK_QSegment *)tip;
// in case of composite segment the second segment is the tip
if (qtip->Composite())
if (qtip->Composite()) {
qtip = qtip->Composite();
}
// convert from blender column major
Matrix3d rot = CreateMatrix(goal[0][0],
@@ -335,15 +373,17 @@ void IK_SolverSetPoleVectorConstraint(IK_Solver *solver,
float poleangle,
int getangle)
{
if (solver == NULL || tip == NULL)
if (solver == NULL || tip == NULL) {
return;
}
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qtip = (IK_QSegment *)tip;
// in case of composite segment the second segment is the tip
if (qtip->Composite())
if (qtip->Composite()) {
qtip = qtip->Composite();
}
Vector3d qgoal(goal[0], goal[1], goal[2]);
Vector3d qpolegoal(polegoal[0], polegoal[1], polegoal[2]);
@@ -353,8 +393,9 @@ void IK_SolverSetPoleVectorConstraint(IK_Solver *solver,
float IK_SolverGetPoleAngle(IK_Solver *solver)
{
if (solver == NULL)
if (solver == NULL) {
return 0.0f;
}
IK_QSolver *qsolver = (IK_QSolver *)solver;
@@ -384,8 +425,9 @@ static void IK_SolverAddCenterOfMass(IK_Solver *solver,
int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
{
if (solver == NULL)
if (solver == NULL) {
return 0;
}
IK_QSolver *qsolver = (IK_QSolver *)solver;
@@ -394,8 +436,9 @@ int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
std::list<IK_QTask *> &tasks = qsolver->tasks;
double tol = tolerance;
if (!jacobian.Setup(root, tasks))
if (!jacobian.Setup(root, tasks)) {
return 0;
}
bool result = jacobian.Solve(root, tasks, tol, max_iterations);