78 lines
2.1 KiB
C++
78 lines
2.1 KiB
C++
#include <QtGlobal>
|
|
#include <QMatrix4x4>
|
|
#include <QDebug>
|
|
#include <cmath>
|
|
#include "ccdikresolver.h"
|
|
#include "util.h"
|
|
|
|
CCDIKSolver::CCDIKSolver() :
|
|
m_maxRound(4),
|
|
m_distanceThreshold2(0.001 * 0.001),
|
|
m_distanceCeaseThreshold2(0.001 * 0.001)
|
|
{
|
|
}
|
|
|
|
void CCDIKSolver::setMaxRound(int maxRound)
|
|
{
|
|
m_maxRound = maxRound;
|
|
}
|
|
|
|
void CCDIKSolver::setDistanceThreshod(float threshold)
|
|
{
|
|
m_distanceThreshold2 = threshold * threshold;
|
|
}
|
|
|
|
int CCDIKSolver::addNodeInOrder(const QVector3D &position)
|
|
{
|
|
CCDIKNode node;
|
|
node.position = position;
|
|
int nodeCount = m_nodes.size();
|
|
m_nodes.push_back(node);
|
|
return nodeCount;
|
|
}
|
|
|
|
void CCDIKSolver::solveTo(const QVector3D &position)
|
|
{
|
|
//qDebug() << "solveTo:" << position;
|
|
m_destination = position;
|
|
float lastDistance2 = 0;
|
|
for (int i = 0; i < m_maxRound; i++) {
|
|
const auto &endEffector = m_nodes[m_nodes.size() - 1];
|
|
float distance2 = (endEffector.position - m_destination).lengthSquared();
|
|
//qDebug() << "Round:" << i << " distance2:" << distance2;
|
|
if (distance2 <= m_distanceThreshold2)
|
|
break;
|
|
if (lastDistance2 > 0 && fabs(distance2 - lastDistance2) <= m_distanceCeaseThreshold2)
|
|
break;
|
|
lastDistance2 = distance2;
|
|
iterate();
|
|
}
|
|
}
|
|
|
|
const QVector3D &CCDIKSolver::getNodeSolvedPosition(int index)
|
|
{
|
|
Q_ASSERT(index >= 0 && index < (int)m_nodes.size());
|
|
return m_nodes[index].position;
|
|
}
|
|
|
|
int CCDIKSolver::getNodeCount(void)
|
|
{
|
|
return m_nodes.size();
|
|
}
|
|
|
|
void CCDIKSolver::iterate()
|
|
{
|
|
for (int i = m_nodes.size() - 2; i >= 0; i--) {
|
|
const auto &origin = m_nodes[i];
|
|
const auto &endEffector = m_nodes[m_nodes.size() - 1];
|
|
QVector3D from = (endEffector.position - origin.position).normalized();
|
|
QVector3D to = (m_destination - origin.position).normalized();
|
|
auto quaternion = QQuaternion::rotationTo(from, to);
|
|
for (size_t j = i + 1; j <= m_nodes.size() - 1; j++) {
|
|
auto &next = m_nodes[j];
|
|
const auto offset = next.position - origin.position;
|
|
next.position = origin.position + quaternion.rotatedVector(offset);
|
|
}
|
|
}
|
|
}
|