SPS并联机器人可达工作空间绘制的MATLAB程序

原创]根据C.Gosselin的论文编写的6-SPS并联机器人可达工作空间绘制程序，思路、算法与数据均来自论文"Determination of the Workspace of 6-DOF Parallel Manipulators"，算法的实现（如圆弧求交离散，可达工作空间边界判定等）由本人自己编写，最后绘制的图形不仅包括z向横截面的工作空间轮廓图，还包括过z轴平面与工作空间的交线，以通过线

/*
//////////////////////////////////////////////////////////////////////////////////////////
//      tdoa4_mex.c this is the mex translation of tdoa3.m    
//      currently works good! yields the same output “dela“ as tdoa3.m 7/16/2003
//////////////////////////////////////////////////////////////////////////////////////////
*/

#include 
#include 
#include “mex.h“
#include “matrix.h“
#include 

/*
 
call: dela=tdoa3（ffxx（1:fivekfreqmics）-ffxx（1:fivekfreqmics+mlevs）freqbetasz*mlevs）;

            function zz=tdoa3（adfreqbetasz）
               qq=sum（cos（ad*ones（1sz）-freqbeta）1）;
            [ii jj]=max（qq）;
            zz=jj-round（（sz-1）/2+1）; %%% returns a scalar
            
    [delax] = tdoa4_mex（sz mlevs fivekfreq ad freqbetax cos_ref_table cosbinsize）;
    
    ad is “fivekfreq by 1“
    freqbeta is “fivekfreq by sz*mlev“
*/

void mexFunction（int nlhs mxArray *plhs[] int nrhs const mxArray *prhs[]）
{
    int total_sz nsample cosbinsize;
    double* ad;
    double* freqbeta;
    double* cos_ref_table;
    double* qq;
    double max_qq total;
    int count_col count_row max_loc=0;
    double* dela;
    
    total_sz = mxGetScalar（prhs[0]） * mxGetScalar（prhs[1]）;
    nsample = mxGetScalar（prhs[2]）;
    cosbinsize = mxGetScalar（prhs[6]）;
    
    ad = mxGetPr（pr