树系统的全关联矩阵、通路矩阵

目录

前言：   

实现：

寻找路径：

判断是否在路上，且返回值

整体实现矩阵：

调用：

---

---

前言：   

        有向图，多刚体动力学的基本图论思想、铰链(弧)、邻接刚体、整标函数、内外侧刚体等基本概念已经知道。

        因为是树系统，构建就比较简单，每一个刚体到零刚体的路只有一条。非树系统通过切铰、切刚体转为树系统。

        总的输入参数包括 铰链的序号、及每一个铰链的起点刚体、终点刚体，以及全部的铰链数目。采取matlab2021a编程，设置为结构体输入。且铰链序号从1→n 顺序排列。

        全部代码链接：树系统全关联矩阵通路矩阵.7z-其它文档类资源-CSDN文库

         代码和文章里的一样，不用下载。

        bilibil链接：树系统的全关联矩阵、通路矩阵 - 哔哩哔哩 (bilibili.com)

---

实现：

        全关联矩阵是比较容易实现的，因为每一个铰链对应两个刚体，且必为起点、终点，也就是全关联矩阵中没一列只有两个值是-1，1，其余为0。即每一列和为0。

        通路矩阵相较难实现。当然可以利用TS = ST =E，计算，但是本意是想用该式验证通路矩阵正确性。通路矩阵设计以下几个分步骤：      

        1、得到零刚体B0到任一刚体Bi的路径

        2、判断铰链是否在该该路上，且是否指向零刚体

其中第一步采取广度优先的搜索策略。matlab中  [0,1,2,[3,4],[],5] = [0,1,2,3,4,5] 比较方便

---

寻找路径：

1、实现每一个点的子节点。

        输入：一个点的编号；全部点的数据(2行n列，矩阵，第一行是起点，第二行终点）、closeTable(存储已经遍历过的列，不重复)

        输出：子节点(向量)， 新的closeTable 

%得到下一层的点 输入:当前点(一个数)、整体点的数据(2,n)、closeTable
function [nextLayerPoints, closeTable] = getNextLayerPoints(nowPoint, hingleInOut, closeTable)
    isNowPoint = hingleInOut == nowPoint;%返回整体数据中包含当前点的状态
    nextLayerPoints = [];
    for j = 1:size(hingleInOut, 2)
        %若已经遍历过了 则跳过
        if(sum(closeTable == j))
            continue;
        end
        %若找到了 则存储 并