136 lines
2.4 KiB
C++
136 lines
2.4 KiB
C++
/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
|
|
*
|
|
* SPDX-License-Identifier: GPL-2.0-or-later */
|
|
|
|
/** \file
|
|
* \ingroup intern_iksolver
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "IK_Math.h"
|
|
#include "IK_QJacobian.h"
|
|
#include "IK_QSegment.h"
|
|
|
|
class IK_QTask {
|
|
public:
|
|
IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment);
|
|
virtual ~IK_QTask() = default;
|
|
|
|
int Id() const
|
|
{
|
|
return m_size;
|
|
}
|
|
|
|
void SetId(int id)
|
|
{
|
|
m_id = id;
|
|
}
|
|
|
|
int Size() const
|
|
{
|
|
return m_size;
|
|
}
|
|
|
|
bool Primary() const
|
|
{
|
|
return m_primary;
|
|
}
|
|
|
|
bool Active() const
|
|
{
|
|
return m_active;
|
|
}
|
|
|
|
double Weight() const
|
|
{
|
|
return m_weight * m_weight;
|
|
}
|
|
|
|
void SetWeight(double weight)
|
|
{
|
|
m_weight = sqrt(weight);
|
|
}
|
|
|
|
virtual void ComputeJacobian(IK_QJacobian &jacobian) = 0;
|
|
|
|
virtual double Distance() const = 0;
|
|
|
|
virtual bool PositionTask() const
|
|
{
|
|
return false;
|
|
}
|
|
|
|
virtual void Scale(double /*scale*/) {}
|
|
|
|
protected:
|
|
int m_id;
|
|
int m_size;
|
|
bool m_primary;
|
|
bool m_active;
|
|
const IK_QSegment *m_segment;
|
|
double m_weight;
|
|
};
|
|
|
|
class IK_QPositionTask : public IK_QTask {
|
|
public:
|
|
IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal);
|
|
|
|
void ComputeJacobian(IK_QJacobian &jacobian) override;
|
|
|
|
double Distance() const override;
|
|
|
|
bool PositionTask() const override
|
|
{
|
|
return true;
|
|
}
|
|
void Scale(double scale) override
|
|
{
|
|
m_goal *= scale;
|
|
m_clamp_length *= scale;
|
|
}
|
|
|
|
private:
|
|
Vector3d m_goal;
|
|
double m_clamp_length;
|
|
};
|
|
|
|
class IK_QOrientationTask : public IK_QTask {
|
|
public:
|
|
IK_QOrientationTask(bool primary, const IK_QSegment *segment, const Matrix3d &goal);
|
|
|
|
double Distance() const override
|
|
{
|
|
return m_distance;
|
|
}
|
|
void ComputeJacobian(IK_QJacobian &jacobian) override;
|
|
|
|
private:
|
|
Matrix3d m_goal;
|
|
double m_distance;
|
|
};
|
|
|
|
class IK_QCenterOfMassTask : public IK_QTask {
|
|
public:
|
|
IK_QCenterOfMassTask(bool primary, const IK_QSegment *segment, const Vector3d ¢er);
|
|
|
|
void ComputeJacobian(IK_QJacobian &jacobian) override;
|
|
|
|
double Distance() const override;
|
|
|
|
void Scale(double scale) override
|
|
{
|
|
m_goal_center *= scale;
|
|
m_distance *= scale;
|
|
}
|
|
|
|
private:
|
|
double ComputeTotalMass(const IK_QSegment *segment);
|
|
Vector3d ComputeCenter(const IK_QSegment *segment);
|
|
void JacobianSegment(IK_QJacobian &jacobian, Vector3d ¢er, const IK_QSegment *segment);
|
|
|
|
Vector3d m_goal_center;
|
|
double m_total_mass_inv;
|
|
double m_distance;
|
|
};
|