Cleanup: Various clang-tidy warnings in iksolver
Pull Request: https://projects.blender.org/blender/blender/pulls/133734
This commit is contained in:
8
intern/iksolver/extern/IK_solver.h
vendored
8
intern/iksolver/extern/IK_solver.h
vendored
@@ -77,7 +77,7 @@ extern "C" {
|
||||
* start * rest_basis * basis * basis_change * translation_change * translate(0,length,0)
|
||||
*/
|
||||
|
||||
typedef void IK_Segment;
|
||||
using IK_Segment = void;
|
||||
|
||||
enum IK_SegmentFlag {
|
||||
IK_XDOF = 1,
|
||||
@@ -88,14 +88,14 @@ enum IK_SegmentFlag {
|
||||
IK_TRANS_ZDOF = 32
|
||||
};
|
||||
|
||||
typedef enum IK_SegmentAxis {
|
||||
enum IK_SegmentAxis {
|
||||
IK_X = 0,
|
||||
IK_Y = 1,
|
||||
IK_Z = 2,
|
||||
IK_TRANS_X = 3,
|
||||
IK_TRANS_Y = 4,
|
||||
IK_TRANS_Z = 5
|
||||
} IK_SegmentAxis;
|
||||
};
|
||||
|
||||
extern IK_Segment *IK_CreateSegment(int flag);
|
||||
extern void IK_FreeSegment(IK_Segment *seg);
|
||||
@@ -122,7 +122,7 @@ extern void IK_GetTranslationChange(IK_Segment *seg, float *translation_change);
|
||||
* It returns 1 if the system converged, 0 otherwise.
|
||||
*/
|
||||
|
||||
typedef void IK_Solver;
|
||||
using IK_Solver = void;
|
||||
|
||||
IK_Solver *IK_CreateSolver(IK_Segment *root);
|
||||
void IK_FreeSolver(IK_Solver *solver);
|
||||
|
||||
@@ -11,6 +11,7 @@
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
using Eigen::Affine3d;
|
||||
@@ -59,12 +60,10 @@ static inline Eigen::Matrix3d RotationMatrix(double sine, double cosine, int axi
|
||||
if (axis == 0) {
|
||||
return CreateMatrix(1.0, 0.0, 0.0, 0.0, cosine, -sine, 0.0, sine, cosine);
|
||||
}
|
||||
else if (axis == 1) {
|
||||
if (axis == 1) {
|
||||
return CreateMatrix(cosine, 0.0, sine, 0.0, 1.0, 0.0, -sine, 0.0, cosine);
|
||||
}
|
||||
else {
|
||||
return CreateMatrix(cosine, -sine, 0.0, sine, cosine, 0.0, 0.0, 0.0, 1.0);
|
||||
}
|
||||
return CreateMatrix(cosine, -sine, 0.0, sine, cosine, 0.0, 0.0, 0.0, 1.0);
|
||||
}
|
||||
|
||||
static inline Eigen::Matrix3d RotationMatrix(double angle, int axis)
|
||||
@@ -80,24 +79,19 @@ static inline double EulerAngleFromMatrix(const Eigen::Matrix3d &R, int axis)
|
||||
if (axis == 0) {
|
||||
return -atan2(R(1, 2), R(2, 2));
|
||||
}
|
||||
else if (axis == 1) {
|
||||
if (axis == 1) {
|
||||
return atan2(-R(0, 2), t);
|
||||
}
|
||||
else {
|
||||
return -atan2(R(0, 1), R(0, 0));
|
||||
}
|
||||
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 (axis == 0) {
|
||||
return -atan2(-R(2, 1), R(1, 1));
|
||||
}
|
||||
if (axis == 1) {
|
||||
return atan2(-R(0, 2), t);
|
||||
}
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
static inline double safe_acos(double f)
|
||||
@@ -106,12 +100,10 @@ static inline double safe_acos(double f)
|
||||
if (f <= -1.0) {
|
||||
return M_PI;
|
||||
}
|
||||
else if (f >= 1.0) {
|
||||
if (f >= 1.0) {
|
||||
return 0.0;
|
||||
}
|
||||
else {
|
||||
return acos(f);
|
||||
}
|
||||
return acos(f);
|
||||
}
|
||||
|
||||
static inline Eigen::Vector3d normalize(const Eigen::Vector3d &v)
|
||||
@@ -204,7 +196,7 @@ static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d &R)
|
||||
return delta;
|
||||
}
|
||||
|
||||
static inline bool EllipseClamp(double &ax, double &az, double *amin, double *amax)
|
||||
static inline bool EllipseClamp(double &ax, double &az, const double *amin, const double *amax)
|
||||
{
|
||||
double xlim, zlim, x, z;
|
||||
|
||||
@@ -231,12 +223,8 @@ static inline bool EllipseClamp(double &ax, double &az, double *amin, double *am
|
||||
return false;
|
||||
}
|
||||
|
||||
if (x > xlim) {
|
||||
x = xlim;
|
||||
}
|
||||
if (z > zlim) {
|
||||
z = zlim;
|
||||
}
|
||||
x = std::min(x, xlim);
|
||||
z = std::min(z, zlim);
|
||||
}
|
||||
else {
|
||||
double invx = 1.0 / (xlim * xlim);
|
||||
|
||||
@@ -8,9 +8,11 @@
|
||||
|
||||
#include "IK_QJacobian.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
IK_QJacobian::IK_QJacobian() : m_sdls(true), m_min_damp(1.0) {}
|
||||
|
||||
IK_QJacobian::~IK_QJacobian() {}
|
||||
IK_QJacobian::~IK_QJacobian() = default;
|
||||
|
||||
void IK_QJacobian::ArmMatrices(int dof, int task_size)
|
||||
{
|
||||
@@ -65,7 +67,7 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size)
|
||||
}
|
||||
}
|
||||
|
||||
void IK_QJacobian::SetBetas(int id, int, const Vector3d &v)
|
||||
void IK_QJacobian::SetBetas(int id, int /*size*/, const Vector3d &v)
|
||||
{
|
||||
m_beta[id + 0] = v.x();
|
||||
m_beta[id + 1] = v.y();
|
||||
@@ -260,9 +262,7 @@ 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) {
|
||||
max_dtheta = abs_dtheta;
|
||||
}
|
||||
max_dtheta = std::max(abs_dtheta, max_dtheta);
|
||||
}
|
||||
|
||||
M *= wInv;
|
||||
@@ -281,16 +281,12 @@ void IK_QJacobian::InvertSDLS()
|
||||
// better to go a little to slow than to far
|
||||
|
||||
double dofdamp = damp / m_weight[j];
|
||||
if (dofdamp > 1.0) {
|
||||
dofdamp = 1.0;
|
||||
}
|
||||
dofdamp = std::min(dofdamp, 1.0);
|
||||
|
||||
m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j];
|
||||
}
|
||||
|
||||
if (damp < m_min_damp) {
|
||||
m_min_damp = damp;
|
||||
}
|
||||
m_min_damp = std::min(damp, m_min_damp);
|
||||
}
|
||||
|
||||
// weight + prevent from doing angle updates with angles > max_angle_change
|
||||
@@ -301,9 +297,7 @@ void IK_QJacobian::InvertSDLS()
|
||||
|
||||
abs_angle = fabs(m_d_theta[j]);
|
||||
|
||||
if (abs_angle > max_angle) {
|
||||
max_angle = abs_angle;
|
||||
}
|
||||
max_angle = std::max(abs_angle, max_angle);
|
||||
}
|
||||
|
||||
if (max_angle > max_angle_change) {
|
||||
@@ -364,9 +358,7 @@ void IK_QJacobian::InvertDLS()
|
||||
|
||||
lambda *= lambda;
|
||||
|
||||
if (lambda > 10) {
|
||||
lambda = 10;
|
||||
}
|
||||
lambda = std::min<double>(lambda, 10);
|
||||
|
||||
// immediately multiply with Beta, so we can do matrix*vector products
|
||||
// rather than matrix*matrix products
|
||||
@@ -419,9 +411,7 @@ 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) {
|
||||
mx = dtheta_abs;
|
||||
}
|
||||
mx = std::max(dtheta_abs, mx);
|
||||
}
|
||||
|
||||
return mx;
|
||||
|
||||
@@ -6,7 +6,8 @@
|
||||
* \ingroup intern_iksolver
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <algorithm>
|
||||
#include <cstdio>
|
||||
|
||||
#include "IK_QJacobianSolver.h"
|
||||
|
||||
@@ -30,9 +31,7 @@ double IK_QJacobianSolver::ComputeScale()
|
||||
if (length == 0.0) {
|
||||
return 1.0;
|
||||
}
|
||||
else {
|
||||
return 1.0 / length;
|
||||
}
|
||||
return 1.0 / length;
|
||||
}
|
||||
|
||||
void IK_QJacobianSolver::Scale(double scale, std::list<IK_QTask *> &tasks)
|
||||
@@ -242,7 +241,7 @@ bool IK_QJacobianSolver::UpdateAngles(double &norm)
|
||||
{
|
||||
// assign each segment a unique id for the jacobian
|
||||
std::vector<IK_QSegment *>::iterator seg;
|
||||
IK_QSegment *qseg, *minseg = NULL;
|
||||
IK_QSegment *qseg, *minseg = nullptr;
|
||||
double minabsdelta = 1e10, absdelta;
|
||||
Vector3d delta, mindelta;
|
||||
bool locked = false, clamp[3];
|
||||
@@ -278,9 +277,7 @@ bool IK_QJacobianSolver::UpdateAngles(double &norm)
|
||||
minseg->Lock(mindof, m_jacobian, mindelta);
|
||||
locked = true;
|
||||
|
||||
if (minabsdelta > norm) {
|
||||
norm = minabsdelta;
|
||||
}
|
||||
norm = std::max(minabsdelta, norm);
|
||||
}
|
||||
|
||||
if (locked == false) {
|
||||
@@ -297,7 +294,7 @@ bool IK_QJacobianSolver::UpdateAngles(double &norm)
|
||||
|
||||
bool IK_QJacobianSolver::Solve(IK_QSegment *root,
|
||||
std::list<IK_QTask *> tasks,
|
||||
const double,
|
||||
const double /*tolerance*/,
|
||||
const int max_iterations)
|
||||
{
|
||||
float scale = ComputeScale();
|
||||
@@ -353,9 +350,7 @@ bool IK_QJacobianSolver::Solve(IK_QSegment *root,
|
||||
|
||||
// compute angle update norm
|
||||
double maxnorm = m_jacobian.AngleUpdateNorm();
|
||||
if (maxnorm > norm) {
|
||||
norm = maxnorm;
|
||||
}
|
||||
norm = std::max(maxnorm, norm);
|
||||
|
||||
// check for convergence
|
||||
if (norm < 1e-3 && iterations > 10) {
|
||||
|
||||
@@ -24,7 +24,7 @@
|
||||
class IK_QJacobianSolver {
|
||||
public:
|
||||
IK_QJacobianSolver();
|
||||
~IK_QJacobianSolver() {}
|
||||
~IK_QJacobianSolver() = default;
|
||||
|
||||
// setup pole vector constraint
|
||||
void SetPoleVectorConstraint(
|
||||
@@ -51,7 +51,6 @@ class IK_QJacobianSolver {
|
||||
double ComputeScale();
|
||||
void Scale(double scale, std::list<IK_QTask *> &tasks);
|
||||
|
||||
private:
|
||||
IK_QJacobian m_jacobian;
|
||||
IK_QJacobian m_jacobian_sub;
|
||||
|
||||
|
||||
@@ -11,10 +11,10 @@
|
||||
// IK_QSegment
|
||||
|
||||
IK_QSegment::IK_QSegment(int num_DoF, bool translational)
|
||||
: m_parent(NULL),
|
||||
m_child(NULL),
|
||||
m_sibling(NULL),
|
||||
m_composite(NULL),
|
||||
: m_parent(nullptr),
|
||||
m_child(nullptr),
|
||||
m_sibling(nullptr),
|
||||
m_composite(nullptr),
|
||||
m_num_DoF(num_DoF),
|
||||
m_translational(translational)
|
||||
{
|
||||
@@ -79,7 +79,7 @@ IK_QSegment::~IK_QSegment()
|
||||
}
|
||||
|
||||
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) {
|
||||
seg->m_parent = NULL;
|
||||
seg->m_parent = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -108,10 +108,10 @@ void IK_QSegment::SetComposite(IK_QSegment *seg)
|
||||
|
||||
void IK_QSegment::RemoveChild(IK_QSegment *child)
|
||||
{
|
||||
if (m_child == NULL) {
|
||||
if (m_child == nullptr) {
|
||||
return;
|
||||
}
|
||||
else if (m_child == child) {
|
||||
if (m_child == child) {
|
||||
m_child = m_child->m_sibling;
|
||||
}
|
||||
else {
|
||||
@@ -381,7 +381,7 @@ void IK_QRevoluteSegment::SetBasis(const Matrix3d &basis)
|
||||
}
|
||||
}
|
||||
|
||||
Vector3d IK_QRevoluteSegment::Axis(int) const
|
||||
Vector3d IK_QRevoluteSegment::Axis(int /*dof*/) const
|
||||
{
|
||||
return m_global_transform.linear().col(m_axis);
|
||||
}
|
||||
@@ -416,7 +416,7 @@ bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &de
|
||||
return true;
|
||||
}
|
||||
|
||||
void IK_QRevoluteSegment::Lock(int, IK_QJacobian &jacobian, Vector3d &delta)
|
||||
void IK_QRevoluteSegment::Lock(int /*dof*/, IK_QJacobian &jacobian, Vector3d &delta)
|
||||
{
|
||||
m_locked[0] = true;
|
||||
jacobian.Lock(m_DoF_id, delta[0]);
|
||||
@@ -565,7 +565,7 @@ bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta
|
||||
return true;
|
||||
}
|
||||
|
||||
void IK_QSwingSegment::Lock(int, IK_QJacobian &jacobian, Vector3d &delta)
|
||||
void IK_QSwingSegment::Lock(int /*dof*/, IK_QJacobian &jacobian, Vector3d &delta)
|
||||
{
|
||||
m_locked[0] = m_locked[1] = true;
|
||||
jacobian.Lock(m_DoF_id, delta[0]);
|
||||
@@ -660,9 +660,7 @@ Vector3d IK_QElbowSegment::Axis(int dof) const
|
||||
|
||||
return m_global_transform.linear() * v;
|
||||
}
|
||||
else {
|
||||
return m_global_transform.linear().col(1);
|
||||
}
|
||||
return m_global_transform.linear().col(1);
|
||||
}
|
||||
|
||||
bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
|
||||
|
||||
@@ -11,8 +11,6 @@
|
||||
#include "IK_Math.h"
|
||||
#include "IK_QJacobian.h"
|
||||
|
||||
#include <vector>
|
||||
|
||||
/**
|
||||
* An IK_Qsegment encodes information about a segments
|
||||
* local coordinate system.
|
||||
@@ -91,7 +89,7 @@ class IK_QSegment {
|
||||
}
|
||||
|
||||
// the max distance of the end of this bone from the local origin.
|
||||
const double MaxExtension() const
|
||||
double MaxExtension() const
|
||||
{
|
||||
return m_max_extension;
|
||||
}
|
||||
@@ -101,12 +99,12 @@ class IK_QSegment {
|
||||
Vector3d TranslationChange() const;
|
||||
|
||||
// the start and end of the segment
|
||||
const Vector3d GlobalStart() const
|
||||
Vector3d GlobalStart() const
|
||||
{
|
||||
return m_global_start;
|
||||
}
|
||||
|
||||
const Vector3d GlobalEnd() const
|
||||
Vector3d GlobalEnd() const
|
||||
{
|
||||
return m_global_transform.translation();
|
||||
}
|
||||
@@ -154,14 +152,14 @@ class IK_QSegment {
|
||||
|
||||
// update the angles using the dTheta's computed using the jacobian matrix
|
||||
virtual bool UpdateAngle(const IK_QJacobian &, Vector3d &, bool *) = 0;
|
||||
virtual void Lock(int, IK_QJacobian &, Vector3d &) {}
|
||||
virtual void Lock(int /*dof*/, IK_QJacobian & /*jacobian*/, Vector3d & /*delta*/) {}
|
||||
virtual void UpdateAngleApply() = 0;
|
||||
|
||||
// set joint limits
|
||||
virtual void SetLimit(int, double, double) {}
|
||||
virtual void SetLimit(int /*axis*/, double /*lmin*/, double /*lmmax*/) {}
|
||||
|
||||
// set joint weights (per axis)
|
||||
virtual void SetWeight(int, double) {}
|
||||
virtual void SetWeight(int /*axis*/, double /*weight*/) {}
|
||||
|
||||
virtual void SetBasis(const Matrix3d &basis)
|
||||
{
|
||||
@@ -180,6 +178,7 @@ class IK_QSegment {
|
||||
IK_QSegment(int num_DoF, bool translational);
|
||||
|
||||
// remove child as a child of this segment
|
||||
void extracted();
|
||||
void RemoveChild(IK_QSegment *child);
|
||||
|
||||
// tree structure variables
|
||||
@@ -218,16 +217,16 @@ class IK_QSphericalSegment : public IK_QSegment {
|
||||
public:
|
||||
IK_QSphericalSegment();
|
||||
|
||||
Vector3d Axis(int dof) const;
|
||||
Vector3d Axis(int dof) const override;
|
||||
|
||||
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp);
|
||||
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta);
|
||||
void UpdateAngleApply();
|
||||
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) override;
|
||||
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) override;
|
||||
void UpdateAngleApply() override;
|
||||
|
||||
bool ComputeClampRotation(Vector3d &clamp);
|
||||
|
||||
void SetLimit(int axis, double lmin, double lmax);
|
||||
void SetWeight(int axis, double weight);
|
||||
void SetLimit(int axis, double lmin, double lmax) override;
|
||||
void SetWeight(int axis, double weight) override;
|
||||
|
||||
private:
|
||||
Matrix3d m_new_basis;
|
||||
@@ -241,17 +240,19 @@ class IK_QNullSegment : public IK_QSegment {
|
||||
public:
|
||||
IK_QNullSegment();
|
||||
|
||||
bool UpdateAngle(const IK_QJacobian &, Vector3d &, bool *)
|
||||
bool UpdateAngle(const IK_QJacobian & /*jacobian*/,
|
||||
Vector3d & /*delta*/,
|
||||
bool * /*clamp*/) override
|
||||
{
|
||||
return false;
|
||||
}
|
||||
void UpdateAngleApply() {}
|
||||
void UpdateAngleApply() override {}
|
||||
|
||||
Vector3d Axis(int) const
|
||||
Vector3d Axis(int /*dof*/) const override
|
||||
{
|
||||
return Vector3d(0, 0, 0);
|
||||
}
|
||||
void SetBasis(const Matrix3d &)
|
||||
void SetBasis(const Matrix3d & /*basis*/) override
|
||||
{
|
||||
m_basis.setIdentity();
|
||||
}
|
||||
@@ -262,15 +263,15 @@ class IK_QRevoluteSegment : public IK_QSegment {
|
||||
// axis: the axis of the DoF, in range 0..2
|
||||
IK_QRevoluteSegment(int axis);
|
||||
|
||||
Vector3d Axis(int dof) const;
|
||||
Vector3d Axis(int dof) const override;
|
||||
|
||||
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp);
|
||||
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta);
|
||||
void UpdateAngleApply();
|
||||
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) override;
|
||||
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) override;
|
||||
void UpdateAngleApply() override;
|
||||
|
||||
void SetLimit(int axis, double lmin, double lmax);
|
||||
void SetWeight(int axis, double weight);
|
||||
void SetBasis(const Matrix3d &basis);
|
||||
void SetLimit(int axis, double lmin, double lmax) override;
|
||||
void SetWeight(int axis, double weight) override;
|
||||
void SetBasis(const Matrix3d &basis) override;
|
||||
|
||||
private:
|
||||
int m_axis;
|
||||
@@ -284,15 +285,15 @@ class IK_QSwingSegment : public IK_QSegment {
|
||||
// XZ DOF, uses one direct rotation
|
||||
IK_QSwingSegment();
|
||||
|
||||
Vector3d Axis(int dof) const;
|
||||
Vector3d Axis(int dof) const override;
|
||||
|
||||
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp);
|
||||
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta);
|
||||
void UpdateAngleApply();
|
||||
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) override;
|
||||
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) override;
|
||||
void UpdateAngleApply() override;
|
||||
|
||||
void SetLimit(int axis, double lmin, double lmax);
|
||||
void SetWeight(int axis, double weight);
|
||||
void SetBasis(const Matrix3d &basis);
|
||||
void SetLimit(int axis, double lmin, double lmax) override;
|
||||
void SetWeight(int axis, double weight) override;
|
||||
void SetBasis(const Matrix3d &basis) override;
|
||||
|
||||
private:
|
||||
Matrix3d m_new_basis;
|
||||
@@ -307,15 +308,15 @@ class IK_QElbowSegment : public IK_QSegment {
|
||||
// X or Z, then rotate around Y (twist)
|
||||
IK_QElbowSegment(int axis);
|
||||
|
||||
Vector3d Axis(int dof) const;
|
||||
Vector3d Axis(int dof) const override;
|
||||
|
||||
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp);
|
||||
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta);
|
||||
void UpdateAngleApply();
|
||||
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) override;
|
||||
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) override;
|
||||
void UpdateAngleApply() override;
|
||||
|
||||
void SetLimit(int axis, double lmin, double lmax);
|
||||
void SetWeight(int axis, double weight);
|
||||
void SetBasis(const Matrix3d &basis);
|
||||
void SetLimit(int axis, double lmin, double lmax) override;
|
||||
void SetWeight(int axis, double weight) override;
|
||||
void SetBasis(const Matrix3d &basis) override;
|
||||
|
||||
private:
|
||||
int m_axis;
|
||||
@@ -334,16 +335,16 @@ class IK_QTranslateSegment : public IK_QSegment {
|
||||
IK_QTranslateSegment(int axis1, int axis2);
|
||||
IK_QTranslateSegment();
|
||||
|
||||
Vector3d Axis(int dof) const;
|
||||
Vector3d Axis(int dof) const override;
|
||||
|
||||
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp);
|
||||
void Lock(int, IK_QJacobian &, Vector3d &);
|
||||
void UpdateAngleApply();
|
||||
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) override;
|
||||
void Lock(int /*dof*/, IK_QJacobian & /*jacobian*/, Vector3d & /*delta*/) override;
|
||||
void UpdateAngleApply() override;
|
||||
|
||||
void SetWeight(int axis, double weight);
|
||||
void SetLimit(int axis, double lmin, double lmax);
|
||||
void SetWeight(int axis, double weight) override;
|
||||
void SetLimit(int axis, double lmin, double lmax) override;
|
||||
|
||||
void Scale(double scale);
|
||||
void Scale(double scale) override;
|
||||
|
||||
private:
|
||||
int m_axis[3];
|
||||
|
||||
@@ -15,7 +15,7 @@
|
||||
class IK_QTask {
|
||||
public:
|
||||
IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment);
|
||||
virtual ~IK_QTask() {}
|
||||
virtual ~IK_QTask() = default;
|
||||
|
||||
int Id() const
|
||||
{
|
||||
@@ -61,7 +61,7 @@ class IK_QTask {
|
||||
return false;
|
||||
}
|
||||
|
||||
virtual void Scale(double) {}
|
||||
virtual void Scale(double /*scale*/) {}
|
||||
|
||||
protected:
|
||||
int m_id;
|
||||
@@ -76,15 +76,15 @@ class IK_QPositionTask : public IK_QTask {
|
||||
public:
|
||||
IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal);
|
||||
|
||||
void ComputeJacobian(IK_QJacobian &jacobian);
|
||||
void ComputeJacobian(IK_QJacobian &jacobian) override;
|
||||
|
||||
double Distance() const;
|
||||
double Distance() const override;
|
||||
|
||||
bool PositionTask() const
|
||||
bool PositionTask() const override
|
||||
{
|
||||
return true;
|
||||
}
|
||||
void Scale(double scale)
|
||||
void Scale(double scale) override
|
||||
{
|
||||
m_goal *= scale;
|
||||
m_clamp_length *= scale;
|
||||
@@ -99,11 +99,11 @@ class IK_QOrientationTask : public IK_QTask {
|
||||
public:
|
||||
IK_QOrientationTask(bool primary, const IK_QSegment *segment, const Matrix3d &goal);
|
||||
|
||||
double Distance() const
|
||||
double Distance() const override
|
||||
{
|
||||
return m_distance;
|
||||
}
|
||||
void ComputeJacobian(IK_QJacobian &jacobian);
|
||||
void ComputeJacobian(IK_QJacobian &jacobian) override;
|
||||
|
||||
private:
|
||||
Matrix3d m_goal;
|
||||
@@ -114,11 +114,11 @@ class IK_QCenterOfMassTask : public IK_QTask {
|
||||
public:
|
||||
IK_QCenterOfMassTask(bool primary, const IK_QSegment *segment, const Vector3d ¢er);
|
||||
|
||||
void ComputeJacobian(IK_QJacobian &jacobian);
|
||||
void ComputeJacobian(IK_QJacobian &jacobian) override;
|
||||
|
||||
double Distance() const;
|
||||
double Distance() const override;
|
||||
|
||||
void Scale(double scale)
|
||||
void Scale(double scale) override
|
||||
{
|
||||
m_goal_center *= scale;
|
||||
m_distance *= scale;
|
||||
|
||||
@@ -12,16 +12,17 @@
|
||||
#include "IK_QSegment.h"
|
||||
#include "IK_QTask.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <list>
|
||||
using namespace std;
|
||||
|
||||
class IK_QSolver {
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
IK_QSolver() : root(NULL) {}
|
||||
IK_QSolver() = default;
|
||||
|
||||
IK_QJacobianSolver solver;
|
||||
IK_QSegment *root;
|
||||
IK_QSegment *root = nullptr;
|
||||
std::list<IK_QTask *> tasks;
|
||||
};
|
||||
|
||||
@@ -36,9 +37,9 @@ static IK_QSegment *CreateSegment(int flag, bool translate)
|
||||
IK_QSegment *seg;
|
||||
|
||||
if (ndof == 0) {
|
||||
return NULL;
|
||||
return nullptr;
|
||||
}
|
||||
else if (ndof == 1) {
|
||||
if (ndof == 1) {
|
||||
int axis;
|
||||
|
||||
if (flag & IK_XDOF) {
|
||||
@@ -101,10 +102,10 @@ IK_Segment *IK_CreateSegment(int flag)
|
||||
|
||||
IK_QSegment *seg;
|
||||
|
||||
if (rot == NULL && trans == NULL) {
|
||||
if (rot == nullptr && trans == nullptr) {
|
||||
seg = new IK_QNullSegment();
|
||||
}
|
||||
else if (rot == NULL) {
|
||||
else if (rot == nullptr) {
|
||||
seg = trans;
|
||||
}
|
||||
else {
|
||||
@@ -218,9 +219,7 @@ void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
|
||||
return;
|
||||
}
|
||||
|
||||
if (stiffness > (1.0 - IK_STRETCH_STIFF_EPS)) {
|
||||
stiffness = (1.0 - IK_STRETCH_STIFF_EPS);
|
||||
}
|
||||
stiffness = std::min<double>(stiffness, 1.0 - IK_STRETCH_STIFF_EPS);
|
||||
|
||||
IK_QSegment *qseg = (IK_QSegment *)seg;
|
||||
double weight = 1.0f - stiffness;
|
||||
@@ -288,8 +287,8 @@ void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
|
||||
|
||||
IK_Solver *IK_CreateSolver(IK_Segment *root)
|
||||
{
|
||||
if (root == NULL) {
|
||||
return NULL;
|
||||
if (root == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
IK_QSolver *solver = new IK_QSolver();
|
||||
@@ -300,7 +299,7 @@ IK_Solver *IK_CreateSolver(IK_Segment *root)
|
||||
|
||||
void IK_FreeSolver(IK_Solver *solver)
|
||||
{
|
||||
if (solver == NULL) {
|
||||
if (solver == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -317,7 +316,7 @@ void IK_FreeSolver(IK_Solver *solver)
|
||||
|
||||
void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
|
||||
{
|
||||
if (solver == NULL || tip == NULL) {
|
||||
if (solver == nullptr || tip == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -338,7 +337,7 @@ 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 == nullptr || tip == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -373,7 +372,7 @@ void IK_SolverSetPoleVectorConstraint(IK_Solver *solver,
|
||||
float poleangle,
|
||||
int getangle)
|
||||
{
|
||||
if (solver == NULL || tip == NULL) {
|
||||
if (solver == nullptr || tip == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -393,7 +392,7 @@ void IK_SolverSetPoleVectorConstraint(IK_Solver *solver,
|
||||
|
||||
float IK_SolverGetPoleAngle(IK_Solver *solver)
|
||||
{
|
||||
if (solver == NULL) {
|
||||
if (solver == nullptr) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
@@ -408,7 +407,7 @@ static void IK_SolverAddCenterOfMass(IK_Solver *solver,
|
||||
float goal[3],
|
||||
float weight)
|
||||
{
|
||||
if (solver == NULL || root == NULL)
|
||||
if (solver == nullptr || root == nullptr)
|
||||
return;
|
||||
|
||||
IK_QSolver *qsolver = (IK_QSolver *)solver;
|
||||
@@ -425,7 +424,7 @@ static void IK_SolverAddCenterOfMass(IK_Solver *solver,
|
||||
|
||||
int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
|
||||
{
|
||||
if (solver == NULL) {
|
||||
if (solver == nullptr) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user