File content as of revision 0:2b80f11eb1d3:

#include "StewartPlatform.h"

//********************************************
//功能：计算矩阵乘法 C=A*B 
//输入参数：A、B：参加运算的矩阵 
//输出参数：C：运算结果
//返回值：计算是否成功 成功返回0 否则返回1 
//调用外部函数：无 
//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
//********************************************
int MatrixDot(MatrixType* A, MatrixType* B, MatrixType* C)
{
    int i, j, k;                                            //循环控制变量 
    int posA, posB, posC;                                   //矩阵下标索引 
    
    //判断异常 
    if(A->Size[1] != B->Size[0])                            //维度不匹配 
        return 1;
    if(A->Size[0] * B->Size[1] > 50)                        //C矩阵规模太大
        return 1;
    
    //计算矩阵乘法
    C->Size[0]  = A->Size[0];                               //得到C的行列数 
    C->Size[1]  = B->Size[1];
    for(i = 1; i <= C->Size[0]; i++)                        //行循环 
    {
        for(j = 1; j <= C->Size[1]; j++)                    //列循环 
        {
            posC = f2(i,j,C->Size[1]);
            C->Elements[posC] = 0;
            for(k = 1; k <= A->Size[1]; k++)                //计算乘法结果 
            {
                posA = f2(i,k,A->Size[1]);
                posB = f2(k,j,B->Size[1]);
                C->Elements[posC]  += A->Elements[posA] * B->Elements[posB];
            }
        }
    } 
    return 0;
}

//********************************************
//功能：计算矩阵转置 
//输入参数：A：被转置的矩阵 
//输出参数：B：转置后的矩阵 
//返回值：无 
//调用外部函数：无 
//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
//********************************************
void MatrixTransposition(MatrixType* A, MatrixType* B)
{
    int i, j;
    int posA, posB;
    
    B->Size[0] = A->Size[1];
    B->Size[1] = A->Size[0];
    
    for(i = 1; i <= A->Size[0]; i++)
    {
        for(j = 1; j <= A->Size[1]; j++)
        {
            posA = f2(i,j,A->Size[1]);
            posB = f2(j,i,B->Size[1]);
            B->Elements[posB] = A->Elements[posA];
        }
    }
} 

//********************************************
//功能：获得矩阵的子阵 
//输入参数：A：原矩阵 StartRow、StartColumn、EndRow、EndColumn：子阵起始元素 子阵终了元素 
//输出参数：B：子阵 
//返回值：无 
//调用外部函数：无 
//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
//********************************************
void MatrixSub(MatrixType* A,int StartRow, int StartColumn, int EndRow, int EndColumn, MatrixType* B)
{
    int i, j;                                               //循环控制变量
    int posA, posB;                                         //矩阵索引 
     
    B->Size[0] = EndRow - StartRow + 1;                     //计算子阵的维度 
    B->Size[1] = EndColumn - StartColumn + 1;
    for(i = 1; i <= B->Size[0]; i++) 
    {
        for(j = 1; j <= B->Size[1]; j++)
        {
            posA = f2(StartRow + i - 1, StartColumn + j - 1, A->Size[1]);
            posB = f2(i, j, B->Size[1]);
            B->Elements[posB] = A->Elements[posA];
        }
    }
} 

//********************************************
//功能：填充矩阵 将一个矩阵填充到另一个矩阵中 
//输入参数：A：被填充的矩阵 Row、Column：矩阵填充的位置 B：要填充到被填充矩阵的矩阵 
//输出参数：A：被填充的矩阵
//返回值：0 代表成功 1代表失败 
//调用外部函数：无 
//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
//********************************************
int MatrixFill(MatrixType* A,int Row, int Column, MatrixType* B)
{
    int i, j, posA, posB;
    
    if((Row + B->Size[0] - 1) > A->Size[0])
    {
        return 1;
    }
    else if((Column + B->Size[1] - 1) > A->Size[1])
    {
        return 1;
    }
    else
    {
        for(i = 1; i <= B->Size[0]; i++)
        {
            for(j = 1; j <= B->Size[1]; j++)
            {
                posA = f2(Row + i - 1, Column + j - 1, A->Size[1]);
                posB = f2(i, j, B->Size[1]);
                A->Elements[posA] = B->Elements[posB];
            }
        }
        return 0;
    }
}

//********************************************
//功能：指定动平台变换矩阵参数x,y,z,a,b,c，计算动平台上的点A在绝对坐标系下的坐标B  A可以是多个点 一行一个点 
//输入参数：x,y,z,a,b,c：动平台变换矩阵参数  A：动平台上点的相对坐标 
//输出参数：B：点在绝对坐标系下的坐标
//返回值：无
//调用外部函数：无 
//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
//********************************************
void Inverse(float x, float y, float z, float a, float b, float c, MatrixType* A, MatrixType* B)
{
    int i;
    
    MatrixType T;                                   //原点平移
    MatrixType Ra;                                  //俯仰 YAW
    MatrixType Rb;                                  //横滚 ROLL 
    MatrixType Rc;                                  //偏航 PITCH 
    MatrixType temp1, temp2, temp3;
    
    T.Size[0] = 4;
    T.Size[1] = 4;
    T.Elements[f2(1,1,T.Size[1])] = 1;
    T.Elements[f2(1,2,T.Size[1])] = 0;
    T.Elements[f2(1,3,T.Size[1])] = 0;
    T.Elements[f2(1,4,T.Size[1])] = x;
    T.Elements[f2(2,1,T.Size[1])] = 0;
    T.Elements[f2(2,2,T.Size[1])] = 1;
    T.Elements[f2(2,3,T.Size[1])] = 0;
    T.Elements[f2(2,4,T.Size[1])] = y;
    T.Elements[f2(3,1,T.Size[1])] = 0;
    T.Elements[f2(3,2,T.Size[1])] = 0;
    T.Elements[f2(3,3,T.Size[1])] = 1;
    T.Elements[f2(3,4,T.Size[1])] = z;
    T.Elements[f2(4,1,T.Size[1])] = 0;
    T.Elements[f2(4,2,T.Size[1])] = 0;
    T.Elements[f2(4,3,T.Size[1])] = 0;
    T.Elements[f2(4,4,T.Size[1])] = 1;
    
    Ra.Size[0] = 4;
    Ra.Size[1] = 4;
    Ra.Elements[f2(1,1,Ra.Size[1])] = 1;
    Ra.Elements[f2(1,2,Ra.Size[1])] = 0;
    Ra.Elements[f2(1,3,Ra.Size[1])] = 0;
    Ra.Elements[f2(1,4,Ra.Size[1])] = 0;
    Ra.Elements[f2(2,1,Ra.Size[1])] = 0;
    Ra.Elements[f2(2,2,Ra.Size[1])] = cosd(a);
    Ra.Elements[f2(2,3,Ra.Size[1])] = -sind(a);
    Ra.Elements[f2(2,4,Ra.Size[1])] = 0;
    Ra.Elements[f2(3,1,Ra.Size[1])] = 0;
    Ra.Elements[f2(3,2,Ra.Size[1])] = sind(a);
    Ra.Elements[f2(3,3,Ra.Size[1])] = cosd(a);
    Ra.Elements[f2(3,4,Ra.Size[1])] = 0;
    Ra.Elements[f2(4,1,Ra.Size[1])] = 0;
    Ra.Elements[f2(4,2,Ra.Size[1])] = 0;
    Ra.Elements[f2(4,3,Ra.Size[1])] = 0;
    Ra.Elements[f2(4,4,Ra.Size[1])] = 1;
    
    Rb.Size[0] = 4;
    Rb.Size[1] = 4;
    Rb.Elements[f2(1,1,Rb.Size[1])] = cosd(b);
    Rb.Elements[f2(1,2,Rb.Size[1])] = 0;
    Rb.Elements[f2(1,3,Rb.Size[1])] = sind(b);
    Rb.Elements[f2(1,4,Rb.Size[1])] = 0;
    Rb.Elements[f2(2,1,Rb.Size[1])] = 0;
    Rb.Elements[f2(2,2,Rb.Size[1])] = 1;
    Rb.Elements[f2(2,3,Rb.Size[1])] = 0;
    Rb.Elements[f2(2,4,Rb.Size[1])] = 0;
    Rb.Elements[f2(3,1,Rb.Size[1])] = -sind(b);
    Rb.Elements[f2(3,2,Rb.Size[1])] = 0;
    Rb.Elements[f2(3,3,Rb.Size[1])] = cosd(b);
    Rb.Elements[f2(3,4,Rb.Size[1])] = 0;
    Rb.Elements[f2(4,1,Rb.Size[1])] = 0;
    Rb.Elements[f2(4,2,Rb.Size[1])] = 0;
    Rb.Elements[f2(4,3,Rb.Size[1])] = 0;
    Rb.Elements[f2(4,4,Rb.Size[1])] = 1;
    
    Rc.Size[0] = 4;
    Rc.Size[1] = 4;
    Rc.Elements[f2(1,1,Rc.Size[1])] = cosd(c);
    Rc.Elements[f2(1,2,Rc.Size[1])] = -sind(c);
    Rc.Elements[f2(1,3,Rc.Size[1])] = 0;
    Rc.Elements[f2(1,4,Rc.Size[1])] = 0;
    Rc.Elements[f2(2,1,Rc.Size[1])] = sind(c);
    Rc.Elements[f2(2,2,Rc.Size[1])] = cosd(c);
    Rc.Elements[f2(2,3,Rc.Size[1])] = 0;
    Rc.Elements[f2(2,4,Rc.Size[1])] = 0;
    Rc.Elements[f2(3,1,Rc.Size[1])] = 0;
    Rc.Elements[f2(3,2,Rc.Size[1])] = 0;
    Rc.Elements[f2(3,3,Rc.Size[1])] = 1;
    Rc.Elements[f2(3,4,Rc.Size[1])] = 0;
    Rc.Elements[f2(4,1,Rc.Size[1])] = 0;
    Rc.Elements[f2(4,2,Rc.Size[1])] = 0;
    Rc.Elements[f2(4,3,Rc.Size[1])] = 0;
    Rc.Elements[f2(4,4,Rc.Size[1])] = 1;
    
    B->Size[0] = A->Size[0]; 
    B->Size[1] = A->Size[1]; 
    MatrixDot(&T, &Rc, &temp1);                     //相当于T * Rc * Rb * Ra 
    MatrixDot(&temp1, &Rb, &temp2);
    MatrixDot(&temp2, &Ra, &temp1);
    for(i = 1; i <= B->Size[0]; i++)
    {
        MatrixSub(A, i, 1, i, A->Size[1], &temp2);
        MatrixTransposition(&temp2, &temp3);
        MatrixDot(&temp1, &temp3, &temp2);
        MatrixTransposition(&temp2, &temp3);
        MatrixFill(B, i, 1, &temp3);
    }
}

//********************************************
//功能：计算矩阵行向量所表示的坐标点之间的距离 
//输入参数：A, B：要计算距离的矩阵 
//输出参数：C：包含距离值信息的列向量 
//返回值：无
//调用外部函数：无 
//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
//********************************************
int  Distance2Point(MatrixType* A, MatrixType* B, MatrixType* C)
{
    float distance = 0;
    int i = 0, j = 0;
    if((A->Size[0] != B->Size[0]) || (A->Size[1] != 4) || (B->Size[1] != 4))
    {
        return 1;
    }
    else
    {
        C->Size[0] = A->Size[0];
        C->Size[1] = 1;
        for(i = 1; i <= A->Size[0]; i++)
        {
            for(j = 1; j <= A->Size[1]; j++)
            {
                distance = distance + (A->Elements[f2(i,j,A->Size[1])] - B->Elements[f2(i,j,B->Size[1])]) * (A->Elements[f2(i,j,A->Size[1])] - B->Elements[f2(i,j,B->Size[1])]);
            }
            C->Elements[f2(i,1,C->Size[1])] = sqrt(distance);
            distance = 0;
        }
        return 0;
    }
}

//********************************************
//功能：解析动感平台 
//输入参数：Platform：动感平台数据结构 包含各种输入输出 
//输出参数：无 
//返回值：无 
//调用外部函数：无 
//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
//********************************************
void CalStewartPlatform(StewartPlatformType* Platform)
{
    int x, y, z, a, b, c;                                           //动平台变换矩阵参数
    float xx, yy, zz, r, l, AA, BB, CC, DD, EE, delta, mytheta1, mytheta2, d, e, f, l1, l2, l3, l4;                                 //计算电机角度用的参数 
    float theta1, theta2, theta3, theta4, theta5, theta6;
    float topRadius, topInterval, bottomRadius, bottomInterval, lengthOfSteelWheel, lengthOfCardan, lengthOfBar;
                                                                    //平台机械尺寸定义 
    float theta0;                                                   //舵盘结构的倾角 
    MatrixType temp1, temp2; 
    
    MatrixType topPlatform;                                         //动平台上的6个参考点
    MatrixType Rc;
    MatrixType bottomPlatform;                                      //定平台上的6个参考点
    MatrixType lengthOfBar1;
    
    topRadius = Platform->topRadius;                                                //平台参数初始化 
    topInterval = Platform->topInterval;
    bottomRadius = Platform->bottomRadius;
    bottomInterval = Platform->bottomInterval;
    lengthOfSteelWheel = Platform->lengthOfSteelWheel;
    lengthOfCardan = Platform->lengthOfCardan;
    lengthOfBar = Platform->lengthOfBar;
    
    topPlatform.Size[0] = 6;
    topPlatform.Size[1] = 4;
    topPlatform.Elements[f2(1,1,topPlatform.Size[1])] = -topInterval / 2;
    topPlatform.Elements[f2(1,2,topPlatform.Size[1])] = -topRadius;
    topPlatform.Elements[f2(1,3,topPlatform.Size[1])] = 0;
    topPlatform.Elements[f2(1,4,topPlatform.Size[1])] = 1;
    topPlatform.Elements[f2(2,1,topPlatform.Size[1])] = topInterval / 2;
    topPlatform.Elements[f2(2,2,topPlatform.Size[1])] = -topRadius;
    topPlatform.Elements[f2(2,3,topPlatform.Size[1])] = 0;
    topPlatform.Elements[f2(2,4,topPlatform.Size[1])] = 1;
    
    Rc.Size[0] = 4;
    Rc.Size[1] = 4;
    Rc.Elements[f2(1,1,Rc.Size[1])] = cosd(120);
    Rc.Elements[f2(1,2,Rc.Size[1])] = -sind(120);
    Rc.Elements[f2(1,3,Rc.Size[1])] = 0;
    Rc.Elements[f2(1,4,Rc.Size[1])] = 0;
    Rc.Elements[f2(2,1,Rc.Size[1])] = sind(120);
    Rc.Elements[f2(2,2,Rc.Size[1])] = cosd(120);
    Rc.Elements[f2(2,3,Rc.Size[1])] = 0;
    Rc.Elements[f2(2,4,Rc.Size[1])] = 0;
    Rc.Elements[f2(3,1,Rc.Size[1])] = 0;
    Rc.Elements[f2(3,2,Rc.Size[1])] = 0;
    Rc.Elements[f2(3,3,Rc.Size[1])] = 1;
    Rc.Elements[f2(3,4,Rc.Size[1])] = 0;
    Rc.Elements[f2(4,1,Rc.Size[1])] = 0;
    Rc.Elements[f2(4,2,Rc.Size[1])] = 0;
    Rc.Elements[f2(4,3,Rc.Size[1])] = 0;
    Rc.Elements[f2(4,4,Rc.Size[1])] = 1; 
    
    MatrixSub(&topPlatform, 1, 1, 1, 4, &temp1);                    //等效于topPlatform(3,:) = (Rc * topPlatform(1, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixTransposition(&temp1, &temp2);
    MatrixFill(&topPlatform, 3, 1, &temp2);
    MatrixSub(&topPlatform, 2, 1, 2, 4, &temp1);                    //等效于topPlatform(4,:) = (Rc * topPlatform(2, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixTransposition(&temp1, &temp2);
    MatrixFill(&topPlatform, 4, 1, &temp2);
    MatrixSub(&topPlatform, 3, 1, 3, 4, &temp1);                    //等效于topPlatform(5,:) = (Rc * topPlatform(3, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixTransposition(&temp1, &temp2);
    MatrixFill(&topPlatform, 5, 1, &temp2);
    MatrixSub(&topPlatform, 4, 1, 4, 4, &temp1);                    //等效于topPlatform(5,:) = (Rc * topPlatform(3, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixTransposition(&temp1, &temp2);
    MatrixFill(&topPlatform, 6, 1, &temp2);
    
    x = Platform->x;
    y = Platform->y;
    z = Platform->z;
    a = Platform->a;
    b = Platform->b;
    c = Platform->c;
    Inverse(x, y, z, a, b, c, &topPlatform, &temp1);                //计算出动平台参考点的实际位置
    MatrixFill(&topPlatform, 1, 1, &temp1);
    
    bottomPlatform.Size[0] = 6;
    bottomPlatform.Size[1] = 4;
    bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])] = -bottomInterval / 2;
    bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])] = -bottomRadius;
    bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])] = 0;
    bottomPlatform.Elements[f2(1,4,bottomPlatform.Size[1])] = 1;
    bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])] = bottomInterval / 2;
    bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])] = -bottomRadius;
    bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])] = 0;
    bottomPlatform.Elements[f2(2,4,bottomPlatform.Size[1])] = 1;
    
    MatrixSub(&bottomPlatform, 1, 1, 1, 4, &temp1);                 //等效于bottomPlatform(3,:) = (Rc * bottomPlatform(1, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixTransposition(&temp1, &temp2);
    MatrixFill(&bottomPlatform, 3, 1, &temp2);
    MatrixSub(&bottomPlatform, 2, 1, 2, 4, &temp1);                 //等效于bottomPlatform(4,:) = (Rc * bottomPlatform(2, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixTransposition(&temp1, &temp2);
    MatrixFill(&bottomPlatform, 4, 1, &temp2);
    MatrixSub(&bottomPlatform, 3, 1, 3, 4, &temp1);                 //等效于bottomPlatform(5,:) = (Rc * bottomPlatform(3, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixTransposition(&temp1, &temp2);
    MatrixFill(&bottomPlatform, 5, 1, &temp2);
    MatrixSub(&bottomPlatform, 4, 1, 4, 4, &temp1);                 //等效于bottomPlatform(6,:) = (Rc * bottomPlatform(3, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixTransposition(&temp1, &temp2);
    MatrixFill(&bottomPlatform, 6, 1, &temp2);
    
    Distance2Point(&topPlatform, &bottomPlatform, &lengthOfBar1);
    
    Platform->BarLength[0] = lengthOfBar1.Elements[f2(1,1,lengthOfBar1.Size[1])];       //赋值计算出的杆长 
    Platform->BarLength[1] = lengthOfBar1.Elements[f2(2,1,lengthOfBar1.Size[1])];
    Platform->BarLength[2] = lengthOfBar1.Elements[f2(3,1,lengthOfBar1.Size[1])];
    Platform->BarLength[3] = lengthOfBar1.Elements[f2(4,1,lengthOfBar1.Size[1])];
    Platform->BarLength[4] = lengthOfBar1.Elements[f2(5,1,lengthOfBar1.Size[1])];
    Platform->BarLength[5] = lengthOfBar1.Elements[f2(6,1,lengthOfBar1.Size[1])];
    
    //计算角度
    r = sqrt(lengthOfCardan * lengthOfCardan + lengthOfSteelWheel * lengthOfSteelWheel);
    l = lengthOfBar;
    //点1
    xx =  topPlatform.Elements[f2(1,1,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])];
    yy =  topPlatform.Elements[f2(1,2,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])];
    zz =  topPlatform.Elements[f2(1,3,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])];
    AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
    BB = 2 * r * xx / (2 * r * zz);
    CC = BB * BB + 1;
    DD = 2 * AA * BB;
    EE = AA * AA - 1;
    delta = DD * DD - 4 * CC * EE;
    mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
    mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
    d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
    l1 = sqrt(d + e + f);
    d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
    l2 = sqrt(d + e + f);
    d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
    l3 = sqrt(d + e + f);
    d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
    l4 = sqrt(d + e + f);
    mytheta1 = mytheta1 / 3.1415926 * 180;
    mytheta2 = mytheta2 / 3.1415926 * 180;
    if(abs(l1 - l) <= 0.0001)
    {
        theta1 = mytheta1;
    }
    else if(abs(l2 - l) <= 0.0001)
    {
        theta1 = -mytheta1;
    }
    else if(abs(l3 - l) <= 0.0001)
    {
        theta1 = mytheta2;
    }
    else
    {
        theta1 = -mytheta2;
    }
    //点2
    xx =  topPlatform.Elements[f2(2,1,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])];
    yy =  topPlatform.Elements[f2(2,2,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])];
    zz =  topPlatform.Elements[f2(2,3,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])];
    AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
    BB = -2 * r * xx / (2 * r * zz);
    CC = BB * BB + 1;
    DD = 2 * AA * BB;
    EE = AA * AA - 1;
    delta = DD * DD - 4 * CC * EE;
    mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
    mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
    d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
    l1 = sqrt(d + e + f);
    d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
    l2 = sqrt(d + e + f);
    d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
    l3 = sqrt(d + e + f);
    d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
    l4 = sqrt(d + e + f);
    mytheta1 = mytheta1 / 3.1415926 * 180;
    mytheta2 = mytheta2 / 3.1415926 * 180;
    if(abs(l1 - l) <= 0.0001)
    {
        theta2 = mytheta1;
    }
    else if(abs(l2 - l) <= 0.0001)
    {
        theta2 = -mytheta1;
    }
    else if(abs(l4 - l) <= 0.0001)
    {
        theta2 = mytheta2;
    }
    else
    {
        theta2 = -mytheta2;
    }
    //点3
    MatrixSub(&topPlatform, 3, 1, 3, 4, &temp1);                    //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixDot(&Rc, &temp1, &temp2);
    MatrixTransposition(&temp2, &temp1);
    xx =  temp1.Elements[f2(1,1,temp1.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])];
    yy =  temp1.Elements[f2(1,2,temp1.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])];
    zz =  temp1.Elements[f2(1,3,temp1.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])];
    AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
    BB = 2 * r * xx / (2 * r * zz);
    CC = BB * BB + 1;
    DD = 2 * AA * BB;
    EE = AA * AA - 1;
    delta = DD * DD - 4 * CC * EE;
    mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
    mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
    d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
    l1 = sqrt(d + e + f);
    d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
    l2 = sqrt(d + e + f);
    d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
    l3 = sqrt(d + e + f);
    d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
    l4 = sqrt(d + e + f);
    mytheta1 = mytheta1 / 3.1415926 * 180;
    mytheta2 = mytheta2 / 3.1415926 * 180;
    if(abs(l1 - l) <= 0.0001)
    {
        theta3 = mytheta1;
    }
    else if(abs(l2 - l) <= 0.0001)
    {
        theta3 = -mytheta1;
    }
    else if(abs(l4 - l) <= 0.0001)
    {
        theta3 = mytheta2;
    }
    else
    {
        theta3 = -mytheta2;
    }
    //点4 
    MatrixSub(&topPlatform, 4, 1, 4, 4, &temp1);                    //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixDot(&Rc, &temp1, &temp2);
    MatrixTransposition(&temp2, &temp1);
    xx =  temp1.Elements[f2(1,1,temp1.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])];
    yy =  temp1.Elements[f2(1,2,temp1.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])];
    zz =  temp1.Elements[f2(1,3,temp1.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])];
    AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
    BB = -2 * r * xx / (2 * r * zz);
    CC = BB * BB + 1;
    DD = 2 * AA * BB;
    EE = AA * AA - 1;
    delta = DD * DD - 4 * CC * EE;
    mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
    mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
    d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
    l1 = sqrt(d + e + f);
    d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
    l2 = sqrt(d + e + f);
    d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
    l3 = sqrt(d + e + f);
    d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
    l4 = sqrt(d + e + f);
    mytheta1 = mytheta1 / 3.1415926 * 180;
    mytheta2 = mytheta2 / 3.1415926 * 180;
    if(abs(l1 - l) <= 0.0001)
    {
        theta4 = mytheta1;
    }
    else if(abs(l2 - l) <= 0.0001)
    {
        theta4 = -mytheta1;
    }
    else if(abs(l4 - l) <= 0.0001)
    {
        theta4 = mytheta2;
    }
    else
    {
        theta4 = -mytheta2;
    }
    //点5
    MatrixSub(&topPlatform, 5, 1, 5, 4, &temp1);                    //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixTransposition(&temp1, &temp2);
    xx =  temp2.Elements[f2(1,1,temp2.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])];
    yy =  temp2.Elements[f2(1,2,temp2.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])];
    zz =  temp2.Elements[f2(1,3,temp2.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])];
    AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
    BB = 2 * r * xx / (2 * r * zz);
    CC = BB * BB + 1;
    DD = 2 * AA * BB;
    EE = AA * AA - 1;
    delta = DD * DD - 4 * CC * EE;
    mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
    mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
    d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
    l1 = sqrt(d + e + f);
    d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
    l2 = sqrt(d + e + f);
    d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
    l3 = sqrt(d + e + f);
    d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
    l4 = sqrt(d + e + f);
    mytheta1 = mytheta1 / 3.1415926 * 180;
    mytheta2 = mytheta2 / 3.1415926 * 180;
    if(abs(l1 - l) <= 0.0001)
    {
        theta5 = mytheta1;
    }
    else if(abs(l2 - l) <= 0.0001)
    {
        theta5 = -mytheta1;
    }
    else if(abs(l4 - l) <= 0.0001)
    {
        theta5 = mytheta2;
    }
    else
    {
        theta5 = -mytheta2;
    }
    //点6
    MatrixSub(&topPlatform, 6, 1, 6, 4, &temp1);                    //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')';
    MatrixTransposition(&temp1, &temp2);
    MatrixDot(&Rc, &temp2, &temp1);
    MatrixTransposition(&temp1, &temp2);
    xx =  temp2.Elements[f2(1,1,temp2.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])];
    yy =  temp2.Elements[f2(1,2,temp2.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])];
    zz =  temp2.Elements[f2(1,3,temp2.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])];
    AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
    BB = -2 * r * xx / (2 * r * zz);
    CC = BB * BB + 1;
    DD = 2 * AA * BB;
    EE = AA * AA - 1;
    delta = DD * DD - 4 * CC * EE;
    mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
    mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
    d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
    l1 = sqrt(d + e + f);
    d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
    e = yy * yy;
    f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
    l2 = sqrt(d + e + f);
    d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
    l3 = sqrt(d + e + f);
    d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
    e = yy * yy;
    f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
    l4 = sqrt(d + e + f);
    mytheta1 = mytheta1 / 3.1415926 * 180;
    mytheta2 = mytheta2 / 3.1415926 * 180;
    if(abs(l1 - l) <= 0.0001)
    {
        theta6 = mytheta1;
    }
    else if(abs(l2 - l) <= 0.0001)
    {
        theta6 = -mytheta1;
    }
    else if(abs(l4 - l) <= 0.0001)
    {
        theta6 = mytheta2;
    }
    else
    {
        theta6 = -mytheta2;
    }
    Platform->theta[0] = theta1; 
    Platform->theta[1] = theta2; 
    Platform->theta[2] = theta3; 
    Platform->theta[3] = theta4; 
    Platform->theta[4] = theta5; 
    Platform->theta[5] = theta6; 
    theta0 = atan(lengthOfCardan / lengthOfSteelWheel) / 3.1415926 * 180;
    Platform->theta_servo[0] = theta1 - theta0;
    Platform->theta_servo[1] = theta2 - theta0;
    Platform->theta_servo[2] = theta3 - theta0;
    Platform->theta_servo[3] = theta4 - theta0;
    Platform->theta_servo[4] = theta5 - theta0;
    Platform->theta_servo[5] = theta6 - theta0;
}



//********************************************
//功能：角度制的三角函数 余弦 
//输入参数：angle：角度 
//输出参数：无 
//返回值：三角函数值 
//调用外部函数：无 
//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
//********************************************
float cosd(float angle)
{
    return cos(angle/180*3.1415926);
}

//********************************************
//功能：角度制的三角函数 正弦 
//输入参数：angle：角度 
//输出参数：无 
//返回值：三角函数值 
//调用外部函数：无 
//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
//********************************************
float sind(float angle)
{
    return sin(angle/180*3.1415926);
}
 

//********************************************
//功能：在命令行打印矩阵 
//输入参数：A：要打印的矩阵 
//输出参数：无 
//返回值：无
//调用外部函数：无 
//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
//********************************************
void PrintMatrix(MatrixType* A)
{
    int i, j;
    int pos2 = 0;
    printf("\r\nThe Matrix is:\r\n");
    for(i = 1; i <= A->Size[0]; i++)
    {   
        for(j = 1; j <= A->Size[1]; j++)
        {
            pos2 = f2(i,j,A->Size[1]);
            printf(" %f ", A->Elements[pos2]);
        }
        printf("\r\n");
    }
}