From df98193091ab45bdebdf60cad012cda000c230dd Mon Sep 17 00:00:00 2001 From: Jeremy Hu Date: Thu, 24 May 2018 09:44:40 +0800 Subject: [PATCH] Add end-effector pulling (Shift + Left Button on end-effector). - Implement CCD (Cyclic Coordinate Descent) IK (Inverse Kinematics) algorithm - Add end-effector pulling operation, this would be useful when you want to pull a group of nodes in line to align straight or mimic curl. --- docs/shortcuts.rst | 2 +- dust3d.pro | 6 ++ src/ccdikresolver.cpp | 74 +++++++++++++++++++++++ src/ccdikresolver.h | 31 ++++++++++ src/skeletondocumentwindow.cpp | 1 + src/skeletongraphicswidget.cpp | 106 ++++++++++++++++++++++++++++++++- src/skeletongraphicswidget.h | 8 +++ src/skeletonikmover.cpp | 61 +++++++++++++++++++ src/skeletonikmover.h | 38 ++++++++++++ 9 files changed, 324 insertions(+), 3 deletions(-) create mode 100644 src/ccdikresolver.cpp create mode 100644 src/ccdikresolver.h create mode 100644 src/skeletonikmover.cpp create mode 100644 src/skeletonikmover.h diff --git a/docs/shortcuts.rst b/docs/shortcuts.rst index f7735468..850848c5 100644 --- a/docs/shortcuts.rst +++ b/docs/shortcuts.rst @@ -74,7 +74,7 @@ Mouse +----------------------------+--------------------------------------------------------------------------+ | CTRL + ALT + LEFT BUTTON | Multiple Unselect | +----------------------------+--------------------------------------------------------------------------+ -| SHIFT + LEFT BUTTON | Rotate | +| SHIFT + LEFT BUTTON | Rotate / Pull (End Effector) | +----------------------------+--------------------------------------------------------------------------+ | RIGHT BUTTON | Context Menu | +----------------------------+--------------------------------------------------------------------------+ diff --git a/dust3d.pro b/dust3d.pro index 79b516ac..9848467f 100644 --- a/dust3d.pro +++ b/dust3d.pro @@ -116,6 +116,12 @@ HEADERS += src/exportpreviewwidget.h SOURCES += src/ambientocclusionbaker.cpp HEADERS += src/ambientocclusionbaker.h +SOURCES += src/ccdikresolver.cpp +HEADERS += src/ccdikresolver.h + +SOURCES += src/skeletonikmover.cpp +HEADERS += src/skeletonikmover.h + HEADERS += src/qtlightmapper.h SOURCES += src/main.cpp diff --git a/src/ccdikresolver.cpp b/src/ccdikresolver.cpp new file mode 100644 index 00000000..08fae987 --- /dev/null +++ b/src/ccdikresolver.cpp @@ -0,0 +1,74 @@ +#include +#include +#include +#include "ccdikresolver.h" + +CCDIKSolver::CCDIKSolver() : + m_maxRound(5), + m_distanceThreshold2(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 && distance2 >= lastDistance2) + break; + lastDistance2 = distance2; + iterate(); + } +} + +const QVector3D &CCDIKSolver::getNodeSolvedPosition(int index) +{ + Q_ASSERT(index >= 0 && index < 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; + QVector3D to = m_destination - origin.position; + 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).normalized() * offset.length(); + } + } +} diff --git a/src/ccdikresolver.h b/src/ccdikresolver.h new file mode 100644 index 00000000..c5af0bbc --- /dev/null +++ b/src/ccdikresolver.h @@ -0,0 +1,31 @@ +#ifndef CCD_IK_SOLVER_H +#define CCD_IK_SOLVER_H +#include +#include +#include + +struct CCDIKNode +{ + QVector3D position; +}; + +class CCDIKSolver +{ +public: + CCDIKSolver(); + void setMaxRound(int maxRound); + void setDistanceThreshod(float threshold); + int addNodeInOrder(const QVector3D &position); + void solveTo(const QVector3D &position); + const QVector3D &getNodeSolvedPosition(int index); + int getNodeCount(void); +private: + void iterate(); +private: + std::vector m_nodes; + QVector3D m_destination; + int m_maxRound; + float m_distanceThreshold2; +}; + +#endif diff --git a/src/skeletondocumentwindow.cpp b/src/skeletondocumentwindow.cpp index c43fbb5d..cfd3d785 100644 --- a/src/skeletondocumentwindow.cpp +++ b/src/skeletondocumentwindow.cpp @@ -454,6 +454,7 @@ SkeletonDocumentWindow::SkeletonDocumentWindow() : connect(graphicsWidget, &SkeletonGraphicsWidget::addNode, m_document, &SkeletonDocument::addNode); connect(graphicsWidget, &SkeletonGraphicsWidget::scaleNodeByAddRadius, m_document, &SkeletonDocument::scaleNodeByAddRadius); connect(graphicsWidget, &SkeletonGraphicsWidget::moveNodeBy, m_document, &SkeletonDocument::moveNodeBy); + connect(graphicsWidget, &SkeletonGraphicsWidget::setNodeOrigin, m_document, &SkeletonDocument::setNodeOrigin); connect(graphicsWidget, &SkeletonGraphicsWidget::removeNode, m_document, &SkeletonDocument::removeNode); connect(graphicsWidget, &SkeletonGraphicsWidget::setEditMode, m_document, &SkeletonDocument::setEditMode); connect(graphicsWidget, &SkeletonGraphicsWidget::removeEdge, m_document, &SkeletonDocument::removeEdge); diff --git a/src/skeletongraphicswidget.cpp b/src/skeletongraphicswidget.cpp index 21cfd976..e85c1bbf 100644 --- a/src/skeletongraphicswidget.cpp +++ b/src/skeletongraphicswidget.cpp @@ -37,7 +37,9 @@ SkeletonGraphicsWidget::SkeletonGraphicsWidget(const SkeletonDocument *document) m_mainOriginItem(nullptr), m_sideOriginItem(nullptr), m_hoveredOriginItem(nullptr), - m_checkedOriginItem(nullptr) + m_checkedOriginItem(nullptr), + m_ikMoveUpdateVersion(0), + m_ikMover(nullptr) { setRenderHint(QPainter::Antialiasing, false); setBackgroundBrush(QBrush(QWidget::palette().color(QWidget::backgroundRole()), Qt::SolidPattern)); @@ -609,7 +611,30 @@ bool SkeletonGraphicsWidget::mouseMove(QMouseEvent *event) float byX = mouseScenePos.x() - m_lastScenePos.x(); float byY = mouseScenePos.y() - m_lastScenePos.y(); if (QGuiApplication::queryKeyboardModifiers().testFlag(Qt::ShiftModifier)) { - rotateSelected(byX); + std::set nodeItems; + readMergedSkeletonNodeSetFromRangeSelection(&nodeItems); + if (nodeItems.size() == 1) { + auto &nodeItem = *nodeItems.begin(); + const SkeletonNode *node = m_document->findNode(nodeItem->id()); + if (node->edgeIds.size() == 1) { + const auto origin = nodeItem->origin(); + byX = mouseScenePos.x() - origin.x(); + byY = mouseScenePos.y() - origin.y(); + byX = sceneRadiusToUnified(byX); + byY = sceneRadiusToUnified(byY); + QVector3D target = QVector3D(node->x, node->y, node->z); + if (SkeletonProfile::Main == nodeItem->profile()) { + target.setX(target.x() + byX); + target.setY(target.y() + byY); + } else { + target.setY(target.y() + byY); + target.setZ(target.z() + byX); + } + emit ikMove(nodeItem->id(), target); + } + } else { + rotateSelected(byX); + } } else { moveSelected(byX, byY); } @@ -671,6 +696,8 @@ void SkeletonGraphicsWidget::moveSelected(float byX, float byY) if (m_rangeSelectionSet.empty()) return; + m_ikMoveEndEffectorId = QUuid(); + byX = sceneRadiusToUnified(byX); byY = sceneRadiusToUnified(byY); std::set nodeItems; @@ -1837,4 +1864,79 @@ void SkeletonGraphicsWidget::setItemHoveredOnAllProfiles(QGraphicsItem *item, bo } } +void SkeletonGraphicsWidget::ikMoveReady() +{ + unsigned long long movedUpdateVersion = m_ikMover->getUpdateVersion(); + + if (movedUpdateVersion == m_ikMoveUpdateVersion && + !m_ikMoveEndEffectorId.isNull()) { + emit batchChangeBegin(); + for (const auto &it: m_ikMover->ikNodes()) { + emit setNodeOrigin(it.id, it.newPosition.x(), it.newPosition.y(), it.newPosition.z()); + } + emit batchChangeEnd(); + emit groupOperationAdded(); + } + + delete m_ikMover; + m_ikMover = nullptr; + + if (movedUpdateVersion != m_ikMoveUpdateVersion && + !m_ikMoveEndEffectorId.isNull()) { + ikMove(m_ikMoveEndEffectorId, m_ikMoveTarget); + } +} +void SkeletonGraphicsWidget::ikMove(QUuid endEffectorId, QVector3D target) +{ + m_ikMoveEndEffectorId = endEffectorId; + m_ikMoveTarget = target; + m_ikMoveUpdateVersion++; + if (nullptr != m_ikMover) { + return; + } + + QThread *thread = new QThread; + + m_ikMover = new SkeletonIkMover(); + m_ikMover->setUpdateVersion(m_ikMoveUpdateVersion); + m_ikMover->setTarget(m_ikMoveTarget); + QUuid nodeId = endEffectorId; + std::set historyNodeIds; + std::vector> appendNodes; + for (;;) { + historyNodeIds.insert(nodeId); + const auto node = m_document->findNode(nodeId); + if (nullptr == node) + break; + appendNodes.push_back(std::make_pair(nodeId, QVector3D(node->x, node->y, node->z))); + if (node->edgeIds.size() < 1 || node->edgeIds.size() > 2) + break; + QUuid choosenNodeId; + for (const auto &edgeId: node->edgeIds) { + const auto edge = m_document->findEdge(edgeId); + if (nullptr == edge) + break; + QUuid neighborNodeId = edge->neighborOf(nodeId); + if (historyNodeIds.find(neighborNodeId) != historyNodeIds.end()) + continue; + choosenNodeId = neighborNodeId; + break; + } + if (choosenNodeId.isNull()) + break; + nodeId = choosenNodeId; + } + qDebug() << "ik move nodes:"; + for (int i = appendNodes.size() - 1; i >= 0; i--) { + qDebug() << i << appendNodes[i].first << appendNodes[i].second; + m_ikMover->addNode(appendNodes[i].first, appendNodes[i].second); + } + qDebug() << "target:" << m_ikMoveTarget; + m_ikMover->moveToThread(thread); + connect(thread, &QThread::started, m_ikMover, &SkeletonIkMover::process); + connect(m_ikMover, &SkeletonIkMover::finished, this, &SkeletonGraphicsWidget::ikMoveReady); + connect(m_ikMover, &SkeletonIkMover::finished, thread, &QThread::quit); + connect(thread, &QThread::finished, thread, &QThread::deleteLater); + thread->start(); +} diff --git a/src/skeletongraphicswidget.h b/src/skeletongraphicswidget.h index d39b303d..195ccc3a 100644 --- a/src/skeletongraphicswidget.h +++ b/src/skeletongraphicswidget.h @@ -15,6 +15,7 @@ #include "turnaroundloader.h" #include "theme.h" #include "dust3dutil.h" +#include "skeletonikmover.h" class SkeletonGraphicsOriginItem : public QGraphicsPolygonItem { @@ -366,6 +367,7 @@ signals: void setXlockState(bool locked); void setYlockState(bool locked); void setZlockState(bool locked); + void setNodeOrigin(QUuid nodeId, float x, float y, float z); public: SkeletonGraphicsWidget(const SkeletonDocument *document); std::map> nodeItemMap; @@ -431,6 +433,8 @@ public slots: void addSelectEdge(QUuid edgeId); void enableBackgroundBlur(); void disableBackgroundBlur(); + void ikMove(QUuid endEffectorId, QVector3D target); + void ikMoveReady(); private slots: void turnaroundImageReady(); private: @@ -475,6 +479,10 @@ private: //need initalize SkeletonGraphicsOriginItem *m_sideOriginItem; SkeletonGraphicsOriginItem *m_hoveredOriginItem; SkeletonGraphicsOriginItem *m_checkedOriginItem; + unsigned long long m_ikMoveUpdateVersion; + SkeletonIkMover *m_ikMover; + QVector3D m_ikMoveTarget; + QUuid m_ikMoveEndEffectorId; private: std::set m_rangeSelectionSet; QPoint m_lastGlobalPos; diff --git a/src/skeletonikmover.cpp b/src/skeletonikmover.cpp new file mode 100644 index 00000000..e198179b --- /dev/null +++ b/src/skeletonikmover.cpp @@ -0,0 +1,61 @@ +#include +#include "skeletonikmover.h" +#include "ccdikresolver.h" + +SkeletonIkMover::SkeletonIkMover() : + m_updateVersion(0) +{ +} + +SkeletonIkMover::~SkeletonIkMover() +{ +} + +void SkeletonIkMover::setTarget(const QVector3D &target) +{ + m_target = target; +} + +void SkeletonIkMover::setUpdateVersion(unsigned long long version) +{ + m_updateVersion = version; +} + +unsigned long long SkeletonIkMover::getUpdateVersion() +{ + return m_updateVersion; +} + +void SkeletonIkMover::addNode(QUuid id, QVector3D position) +{ + SkeletonIkNode ikNode; + ikNode.id = id; + ikNode.position = ikNode.newPosition = position; + m_ikNodes.push_back(ikNode); +} + +const std::vector &SkeletonIkMover::ikNodes() +{ + return m_ikNodes; +} + +void SkeletonIkMover::process() +{ + resolve(); + + this->moveToThread(QGuiApplication::instance()->thread()); + emit finished(); +} + +void SkeletonIkMover::resolve() +{ + CCDIKSolver solver; + for (auto i = 0u; i < m_ikNodes.size(); i++) { + solver.addNodeInOrder(m_ikNodes[i].position); + } + solver.solveTo(m_target); + for (auto i = 0u; i < m_ikNodes.size(); i++) { + m_ikNodes[i].newPosition = solver.getNodeSolvedPosition(i); + } + +} diff --git a/src/skeletonikmover.h b/src/skeletonikmover.h new file mode 100644 index 00000000..bcb5a207 --- /dev/null +++ b/src/skeletonikmover.h @@ -0,0 +1,38 @@ +#ifndef SKELETON_IK_MOVER_H +#define SKELETON_IK_MOVER_H +#include +#include +#include +#include + +struct SkeletonIkNode +{ + QUuid id; + QVector3D position; + QVector3D newPosition; +}; + +class SkeletonIkMover : public QObject +{ + Q_OBJECT +public: + SkeletonIkMover(); + ~SkeletonIkMover(); + void setTarget(const QVector3D &target); + void setUpdateVersion(unsigned long long version); + unsigned long long getUpdateVersion(); + void addNode(QUuid id, QVector3D position); + const std::vector &ikNodes(); +signals: + void finished(); +public slots: + void process(); +private: + void resolve(); +private: + unsigned long long m_updateVersion; + std::vector m_ikNodes; + QVector3D m_target; +}; + +#endif