106 lines
2.7 KiB
C++
106 lines
2.7 KiB
C++
#pragma once
|
|
|
|
/*
|
|
* Nom: William Nolin
|
|
* Code permanent : NOLW76060101
|
|
* Email : william.nolin.1@ens.etsmtl.ca
|
|
*/
|
|
|
|
#include "Armature.h"
|
|
|
|
using namespace gti320;
|
|
|
|
// Constructor
|
|
//
|
|
Link::Link(Link *_parent, const Vector3f &_eulerAng, const Vector3f &_trans) :
|
|
parent(_parent), eulerAng(_eulerAng), trans(_trans) {
|
|
if (parent != nullptr) {
|
|
parent->enfants.push_back(this);
|
|
}
|
|
M.setIdentity();
|
|
}
|
|
|
|
void Link::forward() {
|
|
// Create a rotation matrix from the Euler angles
|
|
// of the current link.
|
|
// Create a local 4D rigid transformation matrix from the
|
|
// 3D rotation matrix and translation of the current link.
|
|
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
|
|
// 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.
|
|
if (isRoot()) {
|
|
M = local;
|
|
} else {
|
|
M = parent->M * local;
|
|
}
|
|
|
|
// Update the transformation of child links
|
|
// by recursion.
|
|
for (const auto &child: enfants) {
|
|
child->forward();
|
|
}
|
|
}
|
|
|
|
Vector3f Link::globalPosition() {
|
|
Vector3f pos;
|
|
pos(0) = M(0, 3);
|
|
pos(1) = M(1, 3);
|
|
pos(2) = M(2, 3);
|
|
return pos;
|
|
}
|
|
|
|
Armature::Armature() : links(), root(nullptr) {
|
|
|
|
}
|
|
|
|
Armature::~Armature() {
|
|
for (Link *l: links) {
|
|
delete l;
|
|
}
|
|
}
|
|
|
|
void Armature::updateKinematics() {
|
|
assert(root != nullptr);
|
|
root->forward();
|
|
}
|
|
|
|
void Armature::pack(Vector<float, Dynamic> &theta) {
|
|
// Collect the Euler angles of each link and put them
|
|
// into the dense vector @a theta
|
|
theta.resize(links.size() * 3);
|
|
|
|
for (int i = 0; i < links.size(); i++) {
|
|
Link *link = links[i];
|
|
int link_index = i * 3;
|
|
|
|
theta(link_index) = link->eulerAng(0);
|
|
theta(link_index + 1) = link->eulerAng(1);
|
|
theta(link_index + 2) = link->eulerAng(2);
|
|
}
|
|
}
|
|
|
|
void Armature::unpack(const Vector<float, Dynamic> &theta) {
|
|
const int numLinks = links.size();
|
|
assert(theta.size() == 3 * numLinks);
|
|
|
|
// Extract the Euler angles contained in the
|
|
// dense vector @a theta and update the angles
|
|
// for each link in the armature.
|
|
for (int i = 0; i < links.size(); i++) {
|
|
Link *link = links[i];
|
|
int link_index = i * 3;
|
|
|
|
link->eulerAng(0) = theta(link_index);
|
|
link->eulerAng(1) = theta(link_index + 1);
|
|
link->eulerAng(2) = theta(link_index + 2);
|
|
}
|
|
}
|