Finito

labo_ik/Armature.cpp

@@ -1,5 +1,11 @@
 #pragma once
 
+/*
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
+ */
+
 #include "Armature.h"
 
 using namespace gti320;

labo_ik/Armature.h

@@ -1,5 +1,11 @@
 #pragma once
 
+/*
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
+ */
+
 #include "Math3D.h"
 #include <vector>
 
@@ -31,15 +37,6 @@ namespace gti320
         // Gets the position of the link from the global rigid transformation matrix.
         Vector3f globalPosition();
 
-        // NOUVELLE FONCTION
-        // Gets the rotation of the link from the global rigid transformation matrix;
-//        inline Matrix3f globalRotation() const {
-//            Matrix3f rotation(M.block(0, 0, 3, 3));
-//            rotation.resize()
-//            rotation = M.block(0, 0, 3, 3);
-//            return M.block(0, 0, 3, 3);
-//        }
-
         Vector3f eulerAng;                              // Euler angles giving rotation relative to the parent.
         Vector3f trans;                                 // Translation giving position relative to the parent.
         Matrix4f M;                                     // Global rigid transformation of the link, computed in forward().

labo_ik/IKApplication.cpp

@@ -1,4 +1,10 @@
-#include "IKApplication.h"
+/*
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
+ */
+
+#include "IKApplication.h"
 #include "IKGLCanvas.h"
 
 #include <nanogui/window.h>

labo_ik/IKApplication.h

@@ -5,10 +5,9 @@
  *
  * @brief Application class for labo 2.
  *
- * Nom:
+ * Nom: William Nolin
- * Code permanent :
+ * Code permanent : NOLW76060101
- * Email :
+ * Email : william.nolin.1@ens.etsmtl.ca
- *
  */
 
 #include <nanogui/window.h>

labo_ik/IKGLCanvas.cpp

@@ -1,3 +1,9 @@
+/*
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
+ */
+
 #include "IKGLCanvas.h"
 
 #include "IKApplication.h"

labo_ik/IKGLCanvas.h

@@ -5,10 +5,9 @@
  *
  * @brief Canvas class for drawing OpenGL scenes.
  *
- * Nom:
+ * Nom: William Nolin
- * Code permanent :
+ * Code permanent : NOLW76060101
- * Email :
+ * Email : william.nolin.1@ens.etsmtl.ca
- *
  */
 
 #include <nanogui/window.h>

labo_ik/IKSolver.cpp

@@ -1,5 +1,11 @@
 #pragma once
 
+/*
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
+ */
+
 #include "IKSolver.h"
 #include "Armature.h"
 #include "SVD.h"
@@ -14,7 +20,7 @@ IKSolver::IKSolver(Armature *_armature, Vector3f &_targetPos) : m_armature(_arma
 
 
 float IKSolver::getError(Vector3f &dx) const {
-    // TODO Compute the error between the current end effector 
+    // Compute the error between the current end effector
     //      position and the target position
     dx.setZero();
 
@@ -30,8 +36,15 @@ float IKSolver::getError(Vector3f &dx) const {
     return ddx.norm();
 }
 
+// NOUVELLE FONCTION
+// Produit scalaire entre une sous-matrice et un vecteur, permettant d'éviter
+//   la copie de données dans un nouveau vecteur pour utiliser l'opérateur fait dans le premier laboratoire.
+// La sous matrice doit avoir au moins une colonne, et toutes les autres seront ignorées.
 template<typename _Scalar, int _Rows, int _Storage>
-float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> a, Vector<_Scalar, _Rows> b) {
+float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> &a, Vector<_Scalar, _Rows> &b) {
+    assert(a.cols() >= 1);
+    assert(a.rows() == b.rows());
+
     _Scalar product = 0;
     for (int i = 0; i < b.size(); i++) {
         product += a(i, 0) * b(i);
@@ -39,6 +52,21 @@ float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> a, Vector<_Scalar, _Rows
     return product;
 }
 
+// NOUVELLE FONCTION
+// Produit vectoriel entre une sous-matrice et un vecteur.
+// La sous-matrice doit comporter au moins une colonne et trois rangées, et la sous-matrice résultante doit contenir au moins une colonne.
+template<int _Rows, int _Columns, int _Storage>
+void crossP(SubMatrix<float, 3, Dynamic, _Storage> &result,
+            SubMatrix<float, _Rows, _Columns, _Storage> &a, Vector3f &b) {
+    assert(result.cols() >= 1);
+    assert(a.rows() >= 3);
+    assert(a.cols() >= 1);
+
+    result(0, 0) = a(1, 0) * b(2) - a(2, 0) * b(1);
+    result(1, 0) = a(2, 0) * b(0) - a(0, 0) * b(2);
+    result(2, 0) = a(0, 0) * b(1) - a(1, 0) * b(0);
+}
+
 void IKSolver::solve() {
     const int numLinks = m_armature->links.size();
     const int dim = 3 * (numLinks);
@@ -57,9 +85,7 @@ void IKSolver::solve() {
         for (int j = 0; j < 3; j++) {
             auto ji = m_J.block(0, i * 3 + j, 3, 1);
             auto oij = link->M.block(0, j, 3, 1);
-            ji(0, 0) = oij(1, 0) * ri(2) - oij(2, 0) * ri(1);
+            crossP(ji, oij, ri);
-            ji(1, 0) = oij(2, 0) * ri(0) - oij(0, 0) * ri(2);
-            ji(2, 0) = oij(0, 0) * ri(1) - oij(1, 0) * ri(0);
         }
     }
 
@@ -108,5 +134,5 @@ void IKSolver::solve() {
         m_armature->updateKinematics();
 
         alpha /= 2;
-    } while (getError(dx) >= error);
+    } while (getError(dx) >= error && alpha > 0);
 }

labo_ik/IKSolver.h

@@ -1,5 +1,11 @@
 #pragma once
 
+/*
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
+ */
+
 #include "Math3D.h"
 
 namespace gti320

labo_ik/LinkUI.cpp

@@ -1,4 +1,10 @@
-#include "LinkUI.h"
+/*
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
+ */
+
+#include "LinkUI.h"
 
 LinkUI::LinkUI(gti320::Link* _link, int _id, nanogui::Widget* _parent) : link(_link), id(_id)
 {

labo_ik/LinkUI.h

@@ -5,6 +5,9 @@
  *
  * @brief User interface for armature links.
  *
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
  */
 
 #include <nanogui/window.h>

labo_ik/SVD.h

@@ -5,6 +5,9 @@
  *
  * @brief Singular value decomposition.
  *
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
  */
 
 #include "Matrix.h"

labo_ik/TargetUI.cpp

@@ -1,4 +1,10 @@
-#include "TargetUI.h"
+/*
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
+ */
+
+#include "TargetUI.h"
 
 TargetUI::TargetUI(nanogui::Widget* _parent, gti320::Vector3f& _target) : target(_target)
 {

labo_ik/TargetUI.h

@@ -5,6 +5,9 @@
  *
  * @brief User interface for end-effector target.
  *
+ * Nom: William Nolin
+ * Code permanent : NOLW76060101
+ * Email : william.nolin.1@ens.etsmtl.ca
  */
 
 #include <nanogui/window.h>

labo_ik/main.cpp

@@ -3,10 +3,9 @@
  *
  * @brief GTI320 Labo 2 - creates a NanoGUI application.
  *
- * Nom:
+ * Nom: William Nolin
- * Code permanent :
+ * Code permanent : NOLW76060101
- * Email :
+ * Email : william.nolin.1@ens.etsmtl.ca
- *
  */
 
 #include "IKApplication.h"