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

目录

前言：   

实现：

寻找路径：

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

整体实现矩阵：

调用：

---

---

前言：   

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

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

        总的输入参数包括 铰链的序号、及每一个铰链的起点刚体、终点刚体，以及全部的铰链数目。采取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
        %若找到了 则存储 并把该列加进closeTable
        %nowPoint是起点，则下一层存储终点；反之亦然
        if(isNowPoint(1,j) == 1)
            nextLayerPoints = [nextLayerPoints,hingleInOut(2,j)];
            closeTable = [closeTable, j];
        end
        if(isNowPoint(2,j) == 1)
            nextLayerPoints = [nextLayerPoints,hingleInOut(1,j)];
            closeTable = [closeTable, j];
        end
    end
end

2、得到从B0 到终点的搜寻到的全部点 及其父节点 (存储父节点方便查找路)

        输入：终点序号、全部点的数据(2行n列，矩阵，第一行是起点，第二行终点)；

        输出：查询过得全部点、及其父节点(从零刚体B0，到终点所在的层) 

%得到从B0 到终点的搜寻到的全部点 及其父节点
function [res,resLast] = getSearchRes(endPoint, hingleInOut)
    searchedAllPoints = [0];%从B0到终点 搜寻到的全部点
    searchedAllPoints_Last = [0];%搜寻到的点 的父节点
    nowLayerAllPoints = [0];%当前层的点
    nextLayerAllPoints = [];%下一层的点
    nextLayerAllPoints_Last = [];%下一层的点 对应的父节点
    closeTable = [];%哪些列已经遍历过了 不再遍历

    cycleState = 1;
    while(cycleState)
        for i = 1:size(nowLayerAllPoints, 2)
            nowLayer_Point = nowLayerAllPoints(i);%取出当前层其中一个点
            %搜到终点 则跳出
            if(nowLayer_Point == endPoint)
                cycleState = 0;
                break;
            end
            [nextLayerPoints, closeTable] = getNextLayerPoints(nowLayer_Point, hingleInOut, closeTable);
            %nectLayerPoints只是 一个点搜寻到的下一层中的点
            nextLayerAllPoints = [nextLayerAllPoints, nextLayerPoints];
            %首先得到下一层点的个数
            nextLayerNums = size(nextLayerAllPoints, 2);
            nextLayerAllPoints_Last = [nextLayerAllPoints_Last, ones(1,nextLayerNums) .* nowLayer_Point];
        end
        %若下一层为空 则结束 证明搜完了
        if(isempty(nextLayerAllPoints))
            cycleState = 0;
            break;
        end
        %下一层的点存入整个队列里面
        searchedAllPoints = [searchedAllPoints, nextLayerAllPoints];
        %把该层点 的父节点存入
        searchedAllPoints_Last = [searchedAllPoints_Last, nextLayerAllPoints_Last];
        nowLayerAllPoints = nextLayerAllPoints;
        nextLayerAllPoints = [];
        nextLayerAllPoints_Last = [];
    end
    res = searchedAllPoints;
    resLast = searchedAllPoints_Last;
end 

3、寻找到终点得唯一路

        输入：终点序号、全部点数据

        输出：路序列 

%寻找到终点的路径
function res = GetPath(endPoint,hingleInOut)
    [searchedAllPoints, searchedAllPoints_Last] = getSearchRes(endPoint,hingleInOut);
    %获得 路径
    res = [endPoint];
    nowPoint = endPoint;
    while(nowPoint ~= 0)
        nowPoint_Index = find(searchedAllPoints == nowPoint);
        lastPoint = searchedAllPoints_Last(nowPoint_Index);
        res = [lastPoint,res];
        nowPoint = lastPoint;
    end
end

上述三个函数封装在一个函数文件。GetPath.m 

---

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

        从路序列中 查询每一对邻接刚体中的铰链是否包含该铰链。并根据起点终点方向来判断是否背离零刚体。封装在IsInPath.m文件中

        输入：铰链序号、刚体序号、整体数据的结构体

        输出：-1、1、0状态

%判断是否在通路上 据规则返回-1 1 0
function res = IsInPath(hingleNum, endPoint,AdjacentBody)
    %得到 至终点的 路径
    hingleInOut = [AdjacentBody.hingleIn;AdjacentBody.hingleOut];
    Channel = GetPath(endPoint, hingleInOut);

    hingleIndex = find(AdjacentBody.hingleNumber == hingleNum);
    hingleIn = hingleInOut(1,hingleIndex);
    hingleOut = hingleInOut(2,hingleIndex);
    res = 0; %不在通路上则 返回0
    %邻接刚体之间是否有 该铰链
    for i = 1:size(Channel, 2)-1
        %若有 且指向 顺序， 表示指向外侧(背离B0) 为-1
        if((Channel(i)==hingleIn)&&(Channel(i+1)==hingleOut))
            res = -1;
        end
        %若有 且指向 逆序， 表示指向内侧(B0) 为1
        if((Channel(i)==hingleOut)&&(Channel(i+1)==hingleIn))
            res = 1;
        end
    end
end 

---

整体实现矩阵：

存在MultiBody.m文件中

classdef MultiBody
    properties
        hingleNumber
        hingleIn
        hingleOut
        hingleNums
    end
    methods
        function obj = MultiBody(AdjacentBody_Struct)
            obj.hingleNumber = AdjacentBody_Struct.hingleNumber;
            obj.hingleIn = AdjacentBody_Struct.hingleIn;
            obj.hingleOut = AdjacentBody_Struct.hingleOut;
            obj.hingleNums = size(AdjacentBody_Struct.hingleNumber,2);
        end

        function S_Total = getTotalCorrMat(obj)
            %全关联矩阵
            total_correlation_matrix = zeros(obj.hingleNums+1, obj.hingleNums);
            for j = 1:obj.hingleNums
                hingleInBody = obj.hingleIn(j);
                hingleOutBody = obj.hingleOut(j);
                total_correlation_matrix(hingleInBody+1,j) = 1;
                total_correlation_matrix(hingleOutBody+1,j) = -1;
            end
            S_Total = total_correlation_matrix;
        end

        function T = getChannelMat(obj)
            %通路矩阵
            channel_matrix = zeros(obj.hingleNums, obj.hingleNums);
            for j = 1:obj.hingleNums
                for i = 1:obj.hingleNums
                    channel_matrix(j,i) = IsInPath(j, i, obj);
                end
            end
            T = channel_matrix;
        end
    end
end

---

调用：

AdjacentBody.hingleNumber = [1,2,3,4,5];
AdjacentBody.hingleIn = [0,3,1,5,4];
AdjacentBody.hingleOut = [1,1,5,2,5];
AdjacentBody.hingleNums = 5;

MultiBodySystem = MultiBody(AdjacentBody);
S  = MultiBodySystem.getTotalCorrMat();
T = MultiBodySystem.getChannelMat();
S(2:end,:) * T