sim_cinematique_inverse/labo_ik/Armature.cpp

106 lines
2.7 KiB
C++
Raw Normal View History

2024-04-01 17:18:18 -04:00
#pragma once
2024-04-12 15:25:52 -04:00
/*
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*/
2024-04-01 17:18:18 -04:00
#include "Armature.h"
using namespace gti320;
// Constructor
//
2024-04-06 18:00:38 -04:00
Link::Link(Link *_parent, const Vector3f &_eulerAng, const Vector3f &_trans) :
parent(_parent), eulerAng(_eulerAng), trans(_trans) {
if (parent != nullptr) {
2024-04-01 17:18:18 -04:00
parent->enfants.push_back(this);
}
M.setIdentity();
}
2024-04-06 18:00:38 -04:00
void Link::forward() {
// Create a rotation matrix from the Euler angles
2024-04-01 17:18:18 -04:00
// of the current link.
2024-04-06 18:00:38 -04:00
// Create a local 4D rigid transformation matrix from the
2024-04-01 17:18:18 -04:00
// 3D rotation matrix and translation of the current link.
2024-04-06 18:00:38 -04:00
Matrix<float, 4, 4> local;
local.setIdentity();
local.block(0, 0, 3, 3) = makeRotation(eulerAng(0), eulerAng(1), eulerAng(2));
local(0, 3) = trans(0);
local(1, 3) = trans(1);
local(2, 3) = trans(2);
// Update the global transformation for the link using the
2024-04-01 17:18:18 -04:00
// parent's rigid transformation matrix and the local transformation.
// Hint : the parent matrix should be post-multiplied.
// Hint : the root does not have a parent. You must consider this case.
2024-04-06 18:00:38 -04:00
if (isRoot()) {
M = local;
} else {
M = parent->M * local;
}
2024-04-01 17:18:18 -04:00
2024-04-06 18:00:38 -04:00
// Update the transformation of child links
2024-04-01 17:18:18 -04:00
// by recursion.
2024-04-06 18:00:38 -04:00
for (const auto &child: enfants) {
child->forward();
}
2024-04-01 17:18:18 -04:00
}
2024-04-06 20:50:36 -04:00
Vector3f Link::globalPosition() {
Vector3f pos;
pos(0) = M(0, 3);
pos(1) = M(1, 3);
pos(2) = M(2, 3);
return pos;
}
2024-04-01 17:18:18 -04:00
2024-04-06 18:00:38 -04:00
Armature::Armature() : links(), root(nullptr) {
2024-04-01 17:18:18 -04:00
}
2024-04-06 18:00:38 -04:00
Armature::~Armature() {
for (Link *l: links) {
2024-04-01 17:18:18 -04:00
delete l;
}
}
2024-04-06 18:00:38 -04:00
void Armature::updateKinematics() {
2024-04-01 17:18:18 -04:00
assert(root != nullptr);
root->forward();
}
2024-04-06 18:00:38 -04:00
void Armature::pack(Vector<float, Dynamic> &theta) {
2024-04-06 20:50:36 -04:00
// Collect the Euler angles of each link and put them
2024-04-01 17:18:18 -04:00
// into the dense vector @a theta
2024-04-06 20:50:36 -04:00
theta.resize(links.size() * 3);
for (int i = 0; i < links.size(); i++) {
Link *link = links[i];
int link_index = i * 3;
2024-04-01 17:18:18 -04:00
2024-04-06 20:50:36 -04:00
theta(link_index) = link->eulerAng(0);
theta(link_index + 1) = link->eulerAng(1);
theta(link_index + 2) = link->eulerAng(2);
}
2024-04-01 17:18:18 -04:00
}
2024-04-06 18:00:38 -04:00
void Armature::unpack(const Vector<float, Dynamic> &theta) {
2024-04-01 17:18:18 -04:00
const int numLinks = links.size();
assert(theta.size() == 3 * numLinks);
2024-04-06 20:50:36 -04:00
// Extract the Euler angles contained in the
2024-04-01 17:18:18 -04:00
// dense vector @a theta and update the angles
// for each link in the armature.
2024-04-06 20:50:36 -04:00
for (int i = 0; i < links.size(); i++) {
Link *link = links[i];
int link_index = i * 3;
2024-04-01 17:18:18 -04:00
2024-04-06 20:50:36 -04:00
link->eulerAng(0) = theta(link_index);
link->eulerAng(1) = theta(link_index + 1);
link->eulerAng(2) = theta(link_index + 2);
}
2024-04-01 17:18:18 -04:00
}