Implémentation d'un moteur physique avec des contraintes rigides

Les lois de Newton nous donnent l'équation de la conservation de quantité de mouvement et du moment cinétique pour un solide rigide.

et

avecla masse du solide, la position du centre de gravité du solide etles forces appliquées au solide, la matrice d'inertie du solide qui dépend de l'orientation (une matrice orthogonale),la vitesse de rotation du solide et les couples appliqués au solide.

Plus généralement, nous aurons une équation de la forme :

, avec

Il faut passer par cette formulation, car la vitesse de rotation (dans) n'est pas directement la dérivée de la matrice de rotation (dans), il y a seulement une relation linaireentre les deux. Ici, est la vitesse etla position généralisée. est la masse, une matricediagonale par bloc de trois dans notre cas et est inversible. Réécrivons l'équation de la dynamique pour faire apparaître les variables :

est simplement composé des vitesses de rotation et vitesses linéaires. Remarquons que, contrairement à, nous n'avons pas encore défini la position. Dans tous nos calculs, nous allons nous passer de la variable position, puis nous la définirons au moment de l'intégration.

Nous avons aussicontraintes :

.

Dans notre cas, nous ne considérerons que les forces de contraintes (qui maintiennent le pantin en forme, par exemple) et pas les forces extérieures (comme la gravité). Il faut donc calculer ces forces issues des contraintes. Afin d'avoir une expression simple et générale (nous pourrons ainsi modéliser des contraintes exotiques facilement, par exemple définir comme contrainte x=cos(y) sur les coordonnées d'un point du solide).

Nous allons utiliser le principe de D'Alembert, qui dit que les forces de contraintes ne travaillent pas lors d'un déplacement virtuel. Un déplacement virtuelest un déplacement instantané et infinitésimal du système de telle sorte qu'il vérifie toujours ses contraintes. Pour une contrainte holonome, on doit donc avoir. En développant au premier ordre, on obtient une équation linéaire que doit vérifier tout déplacement virtuel :, c'est-à-dire . La matrice est appelée jacobienne de la contrainte. Elle possède lignes (autant que de contraintes) et colonnes (autant que de variables), de sorte qu'elle puisse être multipliée à droite par un déplacement et à gauche par un vecteur deéléments que nous nommerons plus tard multiplicateur de Lagrange.

De plus, le travail des forces de contraintes doit être nul pour tout déplacement virtuel, soit .

Alors, l'ensemble des forces vérifiant cette propriété peut s'écrire sous la forme. En effet, une telle force vérifie toujours, pour tout déplacement virtuel, , car tout déplacement virtuel vérifie. De plus, une analyse dimensionnelle nous montre que cet ensemble est exactement celui que l'on cherche : .

Étant donné que la matrice est plus compliquée à calculer que la jacobienne, nous allons chercher la force de contrainte de la forme : cette manière de procéder est équivalente, car la matriceest inversible.

Nous avons l'espace dans lequel les forces se trouvent, il nous faut encore les calculer. Les inconnues sont l'accélération (variables) et le multiplicateur de Lagrange (variables). Il faut donc considérer les équations de la dynamique et les équations des contraintes pour résoudre le système (équations). Nous dérivons l'équation des contraintes deux fois par rapport au temps afin de faire apparaître l'accélération :

Nous substituons alors l'équation de la dynamique dans l'équation de la contrainte pour faire disparaître l'accélération, nous obtenons alors le système d'équations suivant :

La résolution s'effectuera en calculant d'aborddans la deuxième équation, puis en injectant la valeur detrouvée dans l'équation de la dynamique pour calculer l'accélération. Le résultat est une fonction qui nous donne l'accélération à partir de la position et la vitesse :

En mécanique, une liaison entre deux solides est définie par le nombre de degrés de liberté de la liaison. Un solide possède 6 degrés de liberté : 3 degrés de liberté en rotation et 3 en translation. La liaison rotule supprime les 3 degrés de translation. La contrainte doit être composée du même nombre d'équations que le nombre de degrés de liberté enlevés, soit 3 équations pour la rotule.

La liaison rotule est physiquement réalisée avec une coque sphérique dans laquelle une boule est prisonnière, comme le montre la figure ci-dessus. La description la plus simple de cette contrainte est que le centre de la boule doit coïncider avec le centre de la coque sphérique. D'une manière mathématique, on définit donc un point appartenant à un solide et un autre appartenant à l'autre solide et on pose comme contrainte que la coïncidence de ces deux points donne une liaison rotule.

La liaison entre les solidesets'écrit comme suit :

Cette équation revient à dire que le pointappartenant au solidedoit coïncider avec le pointappartenant au solide. Les vecteurs etsont constants ; ce sont les coordonnées des points d'attache par rapport aux centres de gravité des solides, tandis que varient au cours du temps : ce sont nos positions cartésiennes et nos matrices de rotations faisant partie des coordonnées généralisées .

Les variables de vitesses sont les vitesses en translations et les vitesses de rotation des deux solides, c'est-à-dire. Maintenant que les variables de vitesse sont définies nous pouvons calculer la jacobienne de cette contrainte. Pour simplifier le calcul, nous pouvons remarquer que cette jacobienne est donnée par car : il suffit de dériver la contrainte par rapport au temps ; ainsi, le gradient de la contrainte se calcule plus facilement, sans faire apparaître la matrice. En fait, lorsque nous dérivons la contrainte, nous multiplions par la matricesans nous en apercevoir :,

L'application crochet est la matrice de l'application associée au produit vectoriel, car elle vérifie :

La dérivée de la contrainte numérotéeest alors :

En utilisant les propriétés du produit vectoriel, les termes non nuls de la jacobienne sont :

La jacobienne est indexée par matrices de 3 par 3. Cela simplifie et accélère les calculs. est l'indice du bloc de 3 lignes correspondant à la contrainte numéro. est l'indice de la variable de vitesse linéaire du solide, est celui de sa variable de rotation.

On a aussi le terme.

Dans notre cas, l'équation est du second degré, avec un changement de variables. Or, la plupart des méthodes de calcul d'équations différentielles sont développées pour des équations du premier ordre. Cependant, nous pouvons réduire le degré de l'équation différentielle facilement pour trouver la méthode adéquate à appliquer :

etdeviennent. Nous avons donc bien une équation de la forme .

La méthode d'Euler est une manière très simple (voire basique) de résoudre des équations différentielles du premier ordre.

Grâce à la transformation précédente, nous pouvons alors appliquer cette méthode à notre problème de dynamique.

Cependant, l'incrémentation de la matrice de rotationgénère des erreurs et, rapidement, celle-ci n'est plus orthogonale. En particulier, l'erreur due à la rotation est linéaire par rapport au pas de temps, même si une méthode d'intégration avec un ordre plus élevé est utilisée : par exemple, avec Runge Kutta 4, une méthode d'ordre 4, l'erreur reste d'ordre 1.

Ainsi, il faut normaliser cette matrice de rotation à chaque incrémentation pour implémenter des méthodes d'intégration d'ordre plus élevé. Sinon, la méthode sera d'ordre linéaire, car la dégénérescence de la rotation sera prépondérante.

On fait donc intervenir une nouvelle variable secondaire, un quaternion, pour normaliser la matrice dans le schéma d'intégration. Le quaternion est intégré, comme toutes les autres variables, et normalisé :

Finalement, la matrice de rotation est calculée à partir du quaternion.

La méthode de Runge Kutta 4 (RK4) est une méthode d'ordre 4 pour résoudre les équations différentielles ordinaires. Il existe des méthodes de Runge Kutta pour tout ordre, mais la plus connue est celle d'ordre 4 : elle est simple, robuste et offre un bon compromis entre précision et calculs. Considérons le problème suivant :

,

La méthode de RK4 est donnée par l'équation :

avec les coefficients

,,,

Il existe aussi des méthodes d'Adams-Bashforth pour tout ordre, mais nous présenterons ici la méthode d'ordre 4, qui offre un pendant à RK4.

Contrairement aux méthodes de Runge Kutta, où le nombre d'appels de la fonctionest égal au degré de la méthode, ces méthodes utilisent un seul appel par pas, quel que soit l'ordre de la méthode, après l'initialisation. En effet, cette méthode réutilise les évaluations précédentes de la fonction : il suffit donc de stocker les trois derniers appels, le seul à effectuer pour chaque itération est donc, qui sera stocké pour l'itération suivante. C'est une méthode multipas, c'est-à-dire qui utilise plusieurs pas précédents de la solution pour calculer le pas suivant.

Cette méthode apparaît donc plus efficace que la méthode de Runge Kutta. Cependant, la raison pour laquelle elle est peu connue est que le démarrage de la méthode nécessite de calculer les trois premiers pas à l'ordre 4. Ainsi, il faut, par exemple, effectuer trois itérations de la méthode RK4 pour démarrer la méthode, ce qui rend l'implémentation un peu lourde. De plus, il est difficile de changer le pas de temps d'une telle méthode en cours de route.

Un autre avantage de la méthode de Runge Kutta sur la méthode d'Adams-Bashforth est son rayon de convergence, plus de quatre fois plus élevé. En d'autres termes, le pas de temps minimal est plus de quatre fois plus élevé pour la méthode de Runge Kutta que pour la méthode d'Adams-Bashforth.

De plus, la méthode d'Adams-Bashforth n'est pas quatre fois moins chère que la méthode de Runge Kutta (au vu du nombre d'évaluations de la fonction réduit), mais seulement 1,67 fois : ses propriétés de convergence sont légèrement moins bonnes, le pas de temps doit être plus petit. En effet, pour obtenir la même précision entre les méthodes, il faut que le pas de temps de la méthode d'Adams-Bashforth soit 2,4 fois plus petit que le pas de temps de la méthode RK4. Sachant qu'un pas de la méthode de Runge Kutta est quatre fois plus cher (quatre calculs de la dérivée) qu'un pas de la méthode d'Adams-Bashforth, on trouve le facteur 1,67.

Les méthodes d'Euler, de Runge Kutta d'ordre 2 (RK2) et 4 (RK4) ont été implémentées afin de vérifier leur convergence. Elles ont respectivement une convergence d'ordre 1, d'ordre 2 et d'ordre 4. Voilà le résultat de quelques simulations.

L'erreur est calculée à partir de l'énergie initiale et de l'énergie finale du système comme. Cela nous procure une bonne idée de l'erreur d'intégration du système. Nous pouvons voir que la méthode RK4 fait diminuer l'énergie et que les méthodes d'Euler et RK2 la font augmenter. Cette propriété donne un caractère stable à la méthode RK4, un peu comme les méthodes implicites.

Erreur d'intégration lorsque le bonhomme fait un tour.

Nous pouvons voir que, rapidement, la méthode d'Euler est inutilisable (nan).

Beaucoup de moteurs physiques utilisent la méthode « leapfrog » pour intégrer le mouvement, une variation de la méthode d'Euler. Cette méthode d'ordre 2 symplectique (qui conserve l'énergie) fonctionne pour les équations de la forme et est aussi simple à implémenter que la méthode d'Euler. En effet, il suffit de calculer la vitesse au pas de temps suivant à partir de la position précédente, puis de calculer la position au pas de temps suivant avec la vitesse qui vient d'être calculée (et non pas celle au pas de temps précédent).

Seulement, l'équation du mouvement est rarement de cette forme (elle est de cette forme pour la balistique, la mécanique faisant intervenir les forces à potentiel comme l'interaction gravitationnelle). Cependant, on peut quand même l'utiliser dans le cas général, mais la méthode retombe alors au premier ordre.

Si l'on cherche seulement une solution plausible, d'après le tableau ci-dessus, la méthode RK4 apparaît être la plus efficace. Il suffit de 20 pas pour résoudre notre problème, ce qui revient à 80 évaluations de la fonction dérivée. Avec RK2, il faut 80 pas, ce qui revient à 160 appels de la fonction dérivée. Euler nécessite 320 appels de la fonction dérivée. Seulement, dans les jeux vidéo, il y a des frottements dans le système qui assurent que le système ne diverge pas et le nombre d'images par seconde à afficher à l'écran oblige à avoir un pas de temps petit, là où la méthode d'Euler fonctionne. Ainsi, la plupart des moteurs physiques utilisent une méthode d'Euler, malgré ses problèmes de convergence : Comparaison de différents moteurs physiques.

Par contre, les logiciels de CAO et d'ingénierie utilisent plutôt la méthode RK4, qui offre une précision très élevée pour un coût faible, sans hypothèse restrictive sur les situations à simuler pour garantir une convergence décente.

Pour corriger ces erreurs numériques, il y a plusieurs méthodes. Dans un premier temps, on peut facilement corriger une erreur sur un système où les contraintes ne sont pas bouclées, c'est-à-dire qu'il n'y a pas de chaîne fermée de contraintes. Dans ce cas, les contraintes forment des liaisons comme dans un graphe et ce graphe est arborescent. On peut définir une racine et corriger les contraintes une par une.

Pour éviter d'avoir trop d'erreurs à corriger, dans ce cas particulier de contraintes bouclées, il est aussi possible d'utiliser des coordonnées réduites (ou coordonnées généralisées). Par exemple, pour une liaison pivot, on définit la position d'un objet par rapport à l'autre par seulement un angle. Au lieu d'avoir cinq inconnues en force, on a une inconnue en déplacement. Par cette définition, il ne peut pas y avoir d'erreurs sur les contraintes. En coordonnées réduites, il convient d'utiliser les équations de Lagrange pour trouver les équations qui décrivent le système. C'est un peu une formulation duale du problème : à la place de résoudre les forces (les degrés de liberté enlevés), on résout directement le déplacement (les degrés de liberté restants). Cependant, considérer les équations de Lagrange donne dans ce cas des matrices pleines et non creuses comme dans notre cas. Les calculs deviennent alors plus longs et ralentissent les performances du moteur physique.

Dès qu'il y a des contraintes bouclées, cette réécriture du problème ne suffit pas.

Nous pouvons aussi utiliser des forces fictives qui agiront comme des ressorts entre les liaisons. Pour cela, il faut bien choisir la rigidité de notre ressort et son amortissement. Ces derniers devront être réglés en fonction du pas de temps utilisé. Cependant, cette méthode n'est pas très précise et, si elle a l'air simple, le choix des ressorts et amortissements est difficile à réaliser. Cette méthode est connue sous le nom de penalty force method.

La convergence d'une telle méthode est linéaire, c'est-à-dire que l'erreur diminue à chaque itération d'un facteur constant (0,5, dans le cas où le système est bien réglé).

Une autre méthode est la méthode de Newton-Raphson, qui consiste à suivre le schéma jusqu'à ce que, avec le système à corriger et l'itération d'arrêtle système corrigé.

La convergence d'un tel schéma est très rapide, elle est dite quadratique. À chaque itération, l'erreur suivante sera équivalente au carré de l'erreur précédente.

Nous allons utiliser cette dernière méthode, car, si elle peut paraître difficile à résoudre (inversion d'un système linéaire), nous allons montrer que le système linéaire est le même que celui introduit précédemment.

Dans notre cas, pour retrouver notre matrice, nous allons poser : .

Nous voulons résoudre le système, àéquations etinconnues, avec. Nous ne pouvons pas directement résoudre ce système, puisqu'il y a plusieurs solutions possibles. Nous allons donc essayer de réduire le nombre d'inconnues.

Tout d'abord, nous pouvons remarquer que les forces (multiplicateur de Lagrange) que nous appliquons à notre système ont pour rôle de conserver cette contrainte : on peut alors se dire que ces forces feront de très bonnes inconnues pour notre système. Nous allons donc essayer de faire intervenir de telles forces à la place de nos inconnues en déplacement.

Pour cela, nous allons considérer le problème sous un autre angle : il s'agit de minimiser, qui représente une énergie de déformation, sous la contrainte, qui est la correction que nous voulons obtenir.

Nous allons donc trouver un extremum à la fonction en annulant son gradient. Ce dernier est donné par, l'extremum correspond donc à l'équation .

De la même manière, en substituant cette équation dans notre contrainte, on obtient une solution qui ressemble beaucoup à ce que nous avons déjà vu :

Pour la correction en vitesse, la correction est directe, car l'équation est linéaire. Nous pouvons y trouver un parallèle avec la théorie de l'impulsion, qui autorise des discontinuités de vitesses. Une impulsion est une force infiniment grande appliquée pendant un temps infiniment court, de sorte qu'il en résulte une différence de vitesse finie. Nous pourrons d'ailleurs grâce à cette correction simuler des impulsions.

La correction en vitesse permet effectivement de simuler des chocs. Par exemple, supposons que nous voulons qu'un solide de notre système subisse un choc absorbant sur un élément statique. Cela signifie que le solide perdra toute sa vitesse. Il suffit d'ajouter une contrainte (non satisfaite) qui impose que ce solide ait une liaison rotule avec le sol. Le solide et le sol ont donc des vitesses différentes. En appliquant la correction en vitesse sur ce nouveau système, des forces vont s'appliquer entre les joints et entre le sol et le solide qui subit le choc, de sorte que le solide perde toute sa vitesse par rapport au sol et que les autres contraintes restent satisfaites. Le choc a donc bien été calculé.

Si nous voulons un rebond conservatif, il faut alors appliquer deux fois les forces de cette correction. Il devient alors intéressant de vérifier que cette méthode fonctionne bien en regardant la variation d'énergie totale du système.

Si nous voulons un rebond partiel, il suffit d'appliquer un coefficient entre 1 et 2.

La cinématique inverse désigne l'ensemble des méthodes de calcul des positions et rotations d'un modèle articulaire afin qu'il satisfasse des contraintes en plus. Il faut par exemple que notre bonhomme place sa main à une certaine position et nous voulons une valeur des variables de position et de rotation du bonhomme qui vérifie cette propriété.

Pour cela, nous pouvons poser une nouvelle contrainte à notre bonhomme et utiliser la méthode de Newton Raphson décrite ci-dessus pour trouver une solution. Il faut alors itérer la méthode pour trouver le résultat — rappelons que ce n'est pas une méthode directe, mais à convergence quadratique. Cela fonctionne à condition que l'initialisation des variables de position soit assez proche du résultat, sinon, la méthode ne convergera pas. Il faut, dans ce cas-là, utiliser des méthodes plus complexes pour trouver le résultat. Habituellement, la méthode de Denavit-Hartenberg est utilisée pour les systèmes articulaires (avec des rotules ou pivots) et arborescents. La méthode que nous allons présenter n'est pas une approche habituelle, mais est générale et assez intuitive et n'introduit pas de nouveaux calculs.

Dans notre cas, nous allons simplement borner lecalculé par Newton-Raphson à chaque pas de temps. La borne se fera sur les rotations : si le maximum est supérieur à 2, par exemple, on divise letrouvé par. Ceci est réalisé comme suit :

Alors, si la solution est éloignée (c'est-à-dire), l'itération utilisée sera du type descente de gradient, c'est-à-dire, on fixe la longueur du pas (ici égale à 2) et l'on suit simplement le gradient de la fonction afin de minimiser l'erreur sur la contrainte. Donner un pas élevé permettra que les variables sautent les creux de la fonction à satisfaire, de telle sorte que les variables ne restent pas dans une configuration où la méthode de Raphson ne converge pas. Lorsque l'on sera assez proche de la solution, alors l'itération utilisée sera du type Newton-Raphson, avec une convergence quadratique.

Les équations que nous avons proviennent de contraintes de position entre deux solides. Les forces appliquées aux solides ont été remplacées à la place des inconnues de déplacement à l'intérieur des équations, de sorte que l'équation finale pour une contrainte fait intervenir toutes les forces qui s'appliquent sur les deux solides contraints, comme le montrent les flèches vertes. Cela ne fait pas beaucoup de termes par équation, c'est pour cela que nous pouvons parler de matrice creuse.

L'algorithme des corps articulé, plus connu sous le nom de « articulated body algorithm » ou la méthode de Featherstone est une méthode efficace pour résoudre ce problème lorsque les contraintes sont arborescentes. Sa complexité est linéaire en fonction du nombre d'inconnues, alors qu'une inversion classique à l'aide d'un pivot de Gauss a une complexité cubique en fonction du nombre d'inconnues.

Si vous n'êtes pas familier du pivot de Gauss, je vous invite à lire cet article sur les systèmes linéaires.

L'algorithme de Featherstone est une méthode directe pour inverser un système linéaire de forme arborescente. C'est en fait un pivot de Gauss par bloc (de 6 par 6) dans lequel on sait quels termes il faut éliminer, il se compose d'une descente et d'une remontée. Les pivots sont choisis sur la diagonale. La descente du pivot de Gauss sur une telle matrice lorsque les inconnues sont rangées dans le bon ordre (les pères après les fils) ne remplit pas la matrice. On sait exactement quels sont les termes modifiés. Nous n'avons donc pas besoin de considérer une matrice pleine. La remontée se fait aussi sans problème puisque l'on sait quels termes sont non nuls. La partie sur le pivot de Gauss creux permet de comprendre comment un tel algorithme peut fonctionner.

L'algorithme est accessible ici.

Pour traiter le cas où il y a des boucles dans les contraintes (le système n'est plus arborescent), on peut utiliser d'autres algorithmes tels que la méthode de Gauss Seidel, très utilisée pour ce genre d'équations ou une combinaison de celle-ci et de l'algorithme des corps articulés. On peut aussi penser à la méthode des gradients conjugués, puisque notre matrice à inverser est symétrique définie positive.

Par exemple si on note le système

, avecle système arborescent, on peut inverser le système par blocs. Il suffit alors d'inverseret le complément de Schur de notre matrice :(qui est toujours inversible, carest définie positive).

On peut alors inverser le complément de Schur avec la méthode du gradient conjugué ou un pivot de Gauss si sa dimension est petite. On a alors une complexité maximum deavecla dimension deetle nombre de termes non nuls danset .

On peut aussi utiliser une variante de la méthode de Gauss Seidel, une méthode itérative qui profite de la propriété creuse des matrices, ce qui permet d'inverser des gros systèmes là où les méthodes directes prendraient trop de temps. Il y a aussi une version spécialement conçue pour les problèmes avec des contraintes d'inégalité (Projected Gauss Seidel et dérivées), très utilisée :

avec initialementetla partie triangulaire inférieure, la partie triangulaire supérieure etla diagonale de notre matrice initiale.

Il suffit de poser, et de suivre l'algorithme précédent pour résoudre notre système avec une vitesse de convergence plus élevée.

Habituellement, le pivot de Gauss n'est pas utilisé pour résoudre des systèmes linéaires creux, car, lors de la descente du pivot de Gauss, la matrice se remplit et ne devient plus du tout creuse, mais pas dans notre cas.

Lorsque l'on réalise la descente du pivot de Gauss, on élimine les termes sous la diagonale par des additions de lignes de la matrice. On choisit un pivot sur une ligne, puis on l'utilise pour annuler les termes qui sont sur la colonne du pivot. La complexité du pivot de Gauss vient principalement de la descente. Ainsi, pour que notre pivot de Gauss creux soit efficace, il faut que la matrice reste creuse lors de la descente du pivot de Gauss.

Éliminer une inconnue dans le système d'équations revient à enlever un terme aux équations et à le remplacer par d'autres. Pour que le système d'équations ne grandisse pas lorsque l'on élimine les inconnues, il faut que l'équation utilisée pour éliminer l'inconnue soit petite et que cette équation soit substituée dans un nombre minimum d'équations. Dans les deux exemples suivants, nous voyons que les matrices après descente du pivot de Gauss ne se sont pas remplies. Dans le cas d'une chaîne et d'une boucle, le pivot de Gauss creux est efficace.

De manière plus générale, la substitution d'une inconnue ne modifiera que les équations des contraintes voisines, car les équations ne font intervenir que des contraintes voisines. Ainsi, si nous substituons une zone d'inconnues, les équations modifiées seront toutes celles en bordure de cette zone et le nombre de termes dans ces équations sera équivalent au nombre d'équations de la bordure.

Cela explique pourquoi, dans notre cas, cet algorithme est efficace. Si nous avons substitué les équations de toute une branche de contrainte, il n'y aura qu'une seule équation en bordure, là où la branche a été coupée. Au contraire, dans un maillage d'éléments finis, prenons par exemple un cube, supposons que la moitié des inconnues du cube aient déjà été substituées. Alors, la surface en bordure de ces équations est beaucoup plus importante. Ainsi, nous aurons des complexités différentes :

• pour un corps articulé arborescent ;
• pour un maillage d'éléments finis 3D.

De plus, cela nous renseigne quant au choix de l'ordre des pivots.

Il faut donc substituer les inconnues en partant des extrémités des branches.

Comme évoqué précédemment, les équations seront rangées sous forme de listes chaînées ordonnées de matrices trois par trois. À chaque terme (matrice trois par trois) sera associé le numéro de sa force ; les termes nuls ne font pas partie de la liste. Les équations sont rangées dans un tableau. Notre matrice creuse est donc un tableau de listes de termes.

Exemple du système à inverser pour cinq corps articulés bouclés.

Les termes sont chaînés par ordre croissant de la colonne du terme. L'utilisation de listes chaînées ordonnées nous permet d'additionner deux lignes facilement. Pour additionner deux lignes (deux blocs de trois lignes), il suffit de parcourir en même temps les chaînes des deux équations. On insère alors les éléments par ordre croissant dans une nouvelle liste.

Si le terme de l'équation 1 se place avant le terme de l'équation 2, on place le terme de l'équation 1 à la suite de la liste et on réitère avec le prochain terme de l'équation 1. On fait de même si c'est le terme de l'équation 2 qui se place avant. Si les termes des deux équations sont de la même colonne, on les additionne avant de les insérer. Ainsi, il suffit de parcourir une fois les deux équations dans l'ordre pour les additionner.

Il reste un problème embêtant : savoir quelle équation contient un terme qui doit être éliminé par le pivot. On pourrait parcourir l'ensemble des équations, mais la complexité serait mauvaise. La solution est de ne pas substituer directement l'inconnue du pivot dans toutes les équations, mais de le faire uniquement lorsque l'équation arrive : il suffit alors de parcourir l'équation ; les termes ayant le numéro de leur colonne, on trouve les équations correspondant aux pivots à éliminer. Puis on élimine les termes dans l'ordre.

La classe qui représentera l'articulation sera appelée Joint(qui vient de l'anglais). Celle-ci permet de définir chaque contrainte. L'approche que nous avons choisie est un joint défini par sa fonction contrainte. Nous allons donc utiliser une fonction virtuelle (ou pointeur de fonction) qui donnera cette fonction contrainte. Pour accélérer les calculs, cette fonction virtuelle calculera les gradients et les autres termes utilisés. Ces variables seront stockées dans la classe joint pour plus de simplicité.

Une autre approche moins flexible aurait été de considérer les liaisons comme une liaison encastrée à laquelle on supprime des degrés de liberté au choix. Il n'y a alors plus besoin de pointeur de fonction, il y a seulement les équations d'une liaison encastrée auxquelles on supprime des lignes et colonnes. Certains moteurs physiques pour jeux comme bullet proposent de telles liaisons. Cependant, elles ne permettent pas de décrire autant de liaisons qu'une fonction contrainte quelconque comme la nôtre. Cela reste un point intéressant au niveau de l'interface utilisateur, choisir la contrainte en choisissant les degrés de liberté de la liaison.

class Joint{ 
    public: 
    Joint(Solide *p1,Solide *p2,Vect3d const&bar1A1,Vect3d const&A2bar2); 
    inline bool is_pere(Solide &s){return &s==part1;} 

    // précalcule les gradients, les erreurs de contrainte, et le second terme 
    virtual void updatevar(); 
    // remplissage de l'équation associée à la contrainte 
    void fillequa(Matr9d *equa); 

    Vect3d A()const; // donne la position d'attache du joint, utile pour l'affichage 
    Vect3d AG2()const; // donne la position d'attache du joint par rapport au solide 2 
    Vect3d AG1()const; // donne la position d'attache du joint par rapport au solide 1 
    void corrigevar(); // corrige la contrainte pour un système arborescent 

    //variables de stockage 
    Vect3d errc; // valeur de la fonction contrainte (doit valoir 0) 
    Vect3d errv; // dérivée de la fonction contrainte (doit valoir 0) 
    Vect3d terme2; // second terme: J'u' qui apparait dans l'équation 
    Matr9d Gq1,Gq2,Gp1,Gp2; // les gradients 

    //caractéristiques de la liaison (constantes) 
    Matr9d base0; // si la contrainte s'exprime dans une certaine base 
    Quaternion quat0; 

    int iforce; // numéro de la force associé au joint 
    int libertes; // degrés de liberté (entre 0 et 3) 

    Solide* part1; // pointeur vers le solide 1 
    Solide* part2; // pointeur vers le solide 2 
    Vect3d G1A1; // position de la liaison dans la base locale au solide 1 
    Vect3d G2A2; // position de la liaison dans la base locale au solide 2 
};

La classeSoliden'aura pas de méthodes importantes associées. Cette classe est surtout une structure pour stocker l'état du solide au cours du temps (position, orientation, vitesses) et stocker ses caractéristiques physiques, comme son moment d'inertie, sa masse et ses liaisons avec les autres solides.

Pour simplifier le code, des variables secondaires ont été ajoutées pour utiliser la méthode RK4 directement. Bien évidemment, il vaut mieux éviter de procéder de la sorte pour du code de production, afin de séparer clairement l'affichage des calculs.

class Solide{ 
    public: 
    Solide(double m,Vect3d const& dimensions); 
    inline Matr9d getinertie()const {return R*inertie0*transpo(R);} 

    inline Vect3d A()const{if(liens.end()>0){return (*liens(0)).A();} else return p;} 

    /** caractéristiques constantes **/ 
    double masse; 
    Matr9d inertie0; 
    dynamictab<Joint*> liens; 
    int numero; // numéro du solide 

    /** caractéristiques variables **/ 
    // termes pour stocker l'itération actuelle ou pour calculer les accélérations 
    Quaternion q;// passage de la base terrestre à la base locale 
    Matr9d R;// passage de la base terrestre à la base locale 
    Vect3d w;// dans la base terrestre, vitesse de rotation 
    Vect3d p;// dans la base terrestre, position du barycentre 
    Vect3d v;// dans la base terrestre, vitesse du barycentre 

    // termes pour stocker les accélérations 
    Vect3d acceleration;// dans la base terrestre, accélération du barycentre 
    Vect3d a_rot;// dans la base terrestre, accélération en rotation 

    // termes pour stocker l'itération précédente 
    Quaternion q0; 
    Vect3d w0; 
    Vect3d p0; 
    Vect3d v0; 

    // termes pour stocker la somme (k1 + 2*k2 + 2*k3 + k4) 
    Quaternion q1; 
    Vect3d w1; 
    Vect3d p1; 
    Vect3d v1; 
};

La classeSolveurest en fait un système de solides sous contrainte. On peut ajouter n'importe quelle contrainte ou solide au système.

Elle comprend un tableau de solides et de joints, nous y mettrons aussi les fonctions d'affichage graphique. Lorsque nous avons des contraintes arborescentes, nous avons tendance à penser que des fonctions récursives pourraient être plus efficaces, mais ce n'est pas le cas. En rangeant les éléments de façon ordonnée, par exemple les fils derrière les pères, certains algorithmes récursifs deviennent alors plus faciles à écrire et plus efficaces, car ils deviennent itératifs : par exemple, il suffit de parcourir le tableau des solides et de corriger la position du solide fils, pour chaque solide, pour corriger de façon directe tout le système comme évoqué dans IV.A.

Cette classe comprendra toutes les fonctions de résolution, comme les méthodes d'intégration, les corrections et l'impulsion.

Nous avons ici choisi d'utiliser un pivot de Gauss pour inverser les systèmes d'équations.

// pivot de Gauss 
void gauss(dynamictab<Matr9d> &s, dynamictab<Vect3d> &v); 

class Solveur 
{ 
    public: 
    inline Solveur(){} 
    Solveur(double poids); // constructeur 
    ~Solveur(); // destructeur 

    void accelere_rot(Vect3d const& dw); // permet de mettre en rotation tout le bonhomme 

    int add(Solide *s);// ajoute un solide 
    int link(unsigned int p1,unsigned int p2, Vect3d const&d1, Vect3d const&d2);// ajoute une liaison rotule 


    void solvesystemeforce(); // calcule les multiplicateurs de Lagrange et les accélérations (forces) 
    void incrementeforce(double dt); // Euler 
    void incrementeforce2(double dt); // RK2 
    void incrementeforce4(double dt); // RK4 


    void corrigevars();// corrige pour des liaisons rotule arborescente 
    double corrigepos();// correction en position (une itération de la méthode de Newton), retourne l'erreur 
    void corrigev();// corrige en vitesse 
    void impulsion(); // génère une impulsion si le solveur sort de la zone 
    void corrige();// corrige la position et la vitesse en effectuant plusieurs itérations (méthode de Newton) 

    double geterreur(); // retourne la norme cartésienne de la contrainte en position 
    void inversedyn(); // dynamique inverse 
    double getenergie(); // retourne l'énergie totale du système 
    void recentre(); // positionne le bonhomme au centre et supprime sa vitesse de groupe 
    void affiche(); // affiche les segments 

    // quantité totale du système 
    double massetot; 
    Vect3d barycentretot; 
    Vect3d vtot; 

    // liste des joints et solides 
    dynamictab<Solide*> listepartie; 
    dynamictab<Joint*> listejoints; 

    // système à inverser préaloué 
    dynamictab<Vect3d> constante; 
    dynamictab<Matr9d> sys; 
};