error LNK2019: 无法解决的外部符号 "public: __thiscall OpenCSG::Primitive::Primitive(enum OpenCSG::Operation,unsigned int)" (??0Primitive@OpenCSG@@QAE@W4Operation@1@I@Z)函数 "public: __thiscall OpenCSG::DisplayListPrimitive::DisplayListPrimitive(unsigned int,enum OpenCSG::Operation,unsigned int)" (??0DisplayListPrimitive@OpenCSG@@QAE@IW4Operation@1@I@Z) 中被引用。   

error LNK2019: 无法解决的外部符号"public: virtual __thiscall OpenCSG::Primitive::~Primitive(void)" (??1Primitive@OpenCSG@@UAE@XZ) 函数 "public: virtual __thiscall OpenCSG::DisplayListPrimitive::~DisplayListPrimitive(void)" (??1DisplayListPrimitive@OpenCSG@@UAE@XZ) 中被引用。

@wp231957谢谢你能够回答我的难题，能不能具体告诉我解决的方法和操作，如果可以的话我们可以私下联系，我是真的很着急解决这个问题，因为这个问题已经困扰我很多天了。

/*  chen.cpp   Copyright (c) 2015 by T.  */
/*                                     */
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <glut.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <opencsg.h>
#include <math.h>
//#include <windows.h>
//#include <memory.h>
#include "myShape.h"
#include "displaylistPrimitive.h"
#define KEY_ESC 27
#define PI 3.1415
#define N 4

#pragma comment(lib,"C:\\OpenGL包\\opengl32.lib")
#pragma comment(lib,"C:\\OpenGL包\\glu32.lib")
#pragma comment(lib,"C:\\OpenGL包\\glaux.lib")
#pragma comment(lib,"C:\\OpenGL包\\glut.lib")
#pragma comment(lib,"C:\\OpenGL包\\glut32.lib")
#pragma comment(lib,"C:\\OpenGL包\\OpenCSG.lib")

#pragma comment(lib,"C:\\OpenGL包\\glew32.lib")



typedef struct  {
    float x;
    float y;
    float z;
    float shita;
    float alpha;
} US_POS;

typedef struct  {
    float x;
    float y;
    float z;
} SH_POS;

const int shoe_point = 25433;
const int us_point_left = 4;
const int us_point_right = 4;
char *file_left="3.txt";
char *file_right="4.txt";
float theta = 0.0;
float dis=200.0, tws=0.0, ele=0.0, azi=0.0;
FILE *fp_left;
FILE *fp_right;
FILE *fp_us_left;
FILE *fp_us_right;
FILE *fp;
FILE *input_0;
//FILE *input_1;
//FILE *input_2;
//FILE *input_3;
//FILE *input_4;
//FILE *input_5;
FILE *input_right;
//float x[25433],y[25433],z[25433];
SH_POS sh_left[25433];
SH_POS sh_right[25433];
US_POS us_left[us_point_left];
US_POS us_right[us_point_right];
US_POS in_us_left[us_point_left];
US_POS in_us_right[us_point_right];
SH_POS sh_left4cm[88];
SH_POS sh_right4cm[97];
SH_POS full_sh_left4cm[10000];
SH_POS full_sh_right4cm[10000];
double left_us_matrix[us_point_left][4][4];
double right_us_matrix[us_point_right][4][4];
double left_gl_matrix[us_point_left][16];
double right_gl_matrix[us_point_right][16];
int us_flag_left=1;
int us_flag_right=1;
int camera_flag = 0;
float lx=0.0, ly=-170.0, lz=0.0;
float dx=0.0, dy=0.0, dz=0.0;
float sx=0.0, sy=0.0, sz=1.0;
float left_sh_angle=0.0;
float left_sh_height=0.0;
//US_POS right_sh_pos={51.5, 51.5, 0.0, 0.0, 0.0};
US_POS left_sh_pos={0.0, 0.0, 0.0, 0.0, 0.0};
US_POS right_sh_pos={10.0, 0.0, 0.0,-90.0, 0.0};
char ss[10];
int mot_flag=0;
int stop_mot=0;
int roop_flag=0;
int solid_flag=1;
float S1,S2;
SH_POS cone_point[18772];
//double length;
int left_count_4cm = 0;
int right_count_4cm = 0;
int p_us_left=0;
int p_us_right=0;

std::vector<OpenCSG::Primitive*> primitives;

void polarview(float distance, float twist, float elevation, float azimuth);
void CreateDisplayList(void);
void SetLight( GLenum light );
void SetMaterial( GLfloat R, GLfloat G, GLfloat B , GLfloat Alpha);
void display(void);
void idle(void);
void myKbd( unsigned char key, int x, int y );
void myInit (char *progname);
void camera_pos_reset();
void shoe_pos_reset();
void DrawString( char *string, GLfloat x, GLfloat y );
void Motion(void);
void Cone(void);
void matrixmultiply(double a[N][N],double b[N][N],double c[N][N]);
void transmatrix(double a[N][N],US_POS us,double c[4]);
void para2matrix(double x, double y, double z, double shita, double c[4][4], int para);
void inverseMatrix(double a[N][N], double inv_a[N][N]);
double msr_length(US_POS point1, SH_POS point2);

void polarview(float distance, float twist, float elevation, float azimuth){
    glTranslatef(0.0, 0.0, -distance);
    glRotatef(-twist, 0.0, 0.0, 1.0);
    glRotatef(-elevation, 1.0, 0.0, 0.0);
    glRotatef(-azimuth, 0.0, 1.0, 0.0);
}

//ディスプレイリスト作成

void CreateDisplayList()
{
        GLuint id1 = glGenLists(1);
        glNewList(id1, GL_COMPILE);
            glPushMatrix();
                ////glColor3f(0.0, 0.0, 1.0);
                glTranslatef(4.5,7.0,4.0);
                glRotatef(-30.0, 0.0,0.0,1.0);
                //mySolidCylinder(0.5, 0.7,10);
                glRotatef(90.0, 1.0,0.0,0.0);
                glTranslatef(0.0,0.0,-141.0);
                glutSolidCone(51.3,141.0,10,10);
                //glutSolidCone(51.3,141,20,10);
            glPopMatrix();
        glEndList();

        GLuint id2 = glGenLists(1);
        glNewList(id2, GL_COMPILE);
        glPushMatrix();
        glTranslatef(51.5,51.5,51.5);
        glutSolidCube(103.0);
        glPopMatrix();
        glEndList();

        primitives.push_back(new OpenCSG::DisplayListPrimitive(id1, OpenCSG::Intersection, 1));
        primitives.push_back(new OpenCSG::DisplayListPrimitive(id2, OpenCSG::Subtraction, 1));

}


//===========================================================================
// 光源の設定を行う関数
//===========================================================================
void SetLight( GLenum light )
{
    GLfloat light_diffuse[]  = { 1.0, 1.0, 1.0, 1.0 };    // 拡散反射光
    GLfloat light_specular[] = { 1.0, 1.0, 1.0, 1.0 };    // 鏡面反射光
    GLfloat light_ambient[]  = { 0.3, 0.3, 0.3, 0.1 };    // 環境光
    GLfloat light_position[] = { 50.0, 50.0, 10.0, 1.0 };    // 位置と種類

    // 光源の設定
    glLightfv( light, GL_DIFFUSE,  light_diffuse );     // 拡散反射光の設定
    glLightfv( light, GL_SPECULAR, light_specular ); // 鏡面反射光の設定
    glLightfv( light, GL_AMBIENT,  light_ambient );     // 環境光の設定
    glLightfv( light, GL_POSITION, light_position ); // 位置と種類の設定

    //glEnable(light);    // 光源の有効化
}

//===========================================================================
// 材質特性の設定を行う関数
//===========================================================================
void SetMaterial( GLfloat R, GLfloat G, GLfloat B , GLfloat Alpha)
{
    GLfloat mat_diffuse[]  = { R, G, B, Alpha };    // 拡散反射
    GLfloat mat_specular[] = { R, G, B, Alpha };    // 鏡面反射
    GLfloat mat_ambient[]  = { R, G, B, Alpha };    // 環境光反射
    GLfloat shininess[]       = { 50.0 };            // 鏡面反射の鋭さ

    glMaterialfv( GL_FRONT, GL_DIFFUSE,   mat_diffuse );    // 拡散反射の設定
    glMaterialfv( GL_FRONT, GL_SPECULAR,  mat_specular );    // 鏡面反射の設定
    glMaterialfv( GL_FRONT, GL_AMBIENT,   mat_ambient );    // 環境光反射の設定
    glMaterialfv( GL_FRONT, GL_SHININESS, shininess );        // 鏡面反射の鋭さの設定
}

void display(void)
{
    int i,j,k,m;
    double matrix_shita[4][4];
    double matrix_trans[4][4];
    double matrix_alpha[4][4];
    double matrix_temp[4][4];
    double matrix_sh_right[us_point_right][4][4];
    double inv_mat_sh_right[4][4];
    double gl_matrix[16];
    //double inv_gl_matrix[us_point][16];
    double sh_gl_left_matrix[us_point_left][16];
    double sh_gl_right_matrix[us_point_right][16];
    double us_gl_left_matrix[us_point_left][4][4];
    double us_gl_right_matrix[us_point_right][4][4];
    double inv_us_gl_left_matrix[us_point_left][4][4];
    double inv_us_gl_right_matrix[us_point_right][4][4];
    //double l_for_r_mat[us_point_left][4][4];
    //double r_for_l_mat[us_point_left][4][4];
    double l_for_r_mat[4][4];
    double r_for_l_mat[4][4];
    double temp_point[4];
    double inv_mat_us_left[4][4];
    double matrix_temp_left[us_point_left][4][4];
    double matrix_temp_right[us_point_right][4][4];
    US_POS temp;
    static int count=0;
    int count_us_l_r=0;
    int count_us_r_l=0;
    int left_in[us_point_right][us_point_left];//={{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}};
    int right_in[us_point_right][us_point_left];//={{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}};
    double test[2];

    for(i=0;i<us_point_right;i++){
        for(j=0;j<us_point_left;j++){
            left_in[i][j]=0;
            right_in[i][j]=0;
        }
    }
   
    glClear (GL_COLOR_BUFFER_BIT);

    glPushMatrix ();
    //glTranslatef( 0.0, 0.0, -5.0 );
    if(camera_flag == 0) polarview(dis, tws, ele, azi);
    else gluLookAt( lx, ly, lz, dx, dy, dz, sx, sy, sz);
    //gluLookAt( lx, ly, lz, dx, dy, dz, 0.0, 0.0, 1.0 );
    //gluLookAt( -10.0, 51.3, 60.0, 103.0, 103.0, 0.0, 0.0, 0.0, 1.0 );
    //glRotatef (theta, 0.0, 1.0, 0.0);
   
   
    //全体をずらす
    glPushMatrix();
    glTranslatef(-50,-50,0.0);

    if(roop_flag==0){
    //軸
        glPushMatrix();
            glTranslatef(0.0, 0.0, 1.0); //少し上にずらす
            glBegin(GL_LINES);
                glColor3f(1.0, 0.0, 0.0);
                glVertex3f(0,0,0);
                glVertex3f(10,0,0);
                glColor3f(0.0, 1.0, 0.0);
                glVertex3f(0,0,0);
                glVertex3f(0,10,0);
                glColor3f(0.0, 0.0, 1.0);
                glVertex3f(0,0,0);
                glVertex3f(0,0,10);
            glEnd();
        glPopMatrix();

        glClear( GL_DEPTH_BUFFER_BIT );            // Zバッファの初期化
        glEnable( GL_DEPTH_TEST );                // 隠面処理の適用
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
   

    //if(mot_flag == 0){
   
            //左足
        //if(roop_flag==0){
            glColor3f(1.0, 1.0, 0.0);
            //SetMaterial(1.0,1.0,0.0,1.0);
            glPushMatrix();
            //glTranslatef(0.0, 0.0, left_sh_height);
            glTranslatef(left_sh_pos.x, left_sh_pos.y, left_sh_pos.z);
            glRotatef(left_sh_pos.shita, 0.0, 0.0, 1.0);
            glRotatef(left_sh_pos.alpha, 1.0, 0.0, 0.0);
            //glRotatef(left_sh_angle, 1.0, 0.0, 0.0);
            glBegin(GL_POINTS);
                for(i=0;i<shoe_point;i++) glVertex3f(  sh_left[i].x, sh_left[i].y, sh_left[i].z );
            glEnd();
            glPopMatrix();

                //右足
            glPushMatrix();
            //SetMaterial(1.0,1.0,0.0,1.0);
            glTranslatef(right_sh_pos.x, right_sh_pos.y, right_sh_pos.z);
            glRotatef(right_sh_pos.shita, 0.0, 0.0, 1.0);
            glRotatef(right_sh_pos.alpha, 1.0, 0.0, 0.0);
            glBegin(GL_POINTS);
                for(i=0;i<shoe_point;i++) glVertex3f(  sh_right[i].x, sh_right[i].y, sh_right[i].z );
            glEnd();
            glPopMatrix();
            //printf("%f\r",right_sh_pos.shita);
        
        //else {
            //glPushMatrix();
            //glTranslatef(right_sh_pos.x, right_sh_pos.y, right_sh_pos.z);
            //glutSolidCube(5.0);
            //glPopMatrix();
        //}
        
            glPushMatrix();
                SetLight(GL_LIGHT0);        // 光源の設定（視点に対して固定）
                glEnable(GL_LIGHTING);
            glPopMatrix();

            //床
            SetMaterial(1.0,1.0,1.0,1.0);
            //glColor3f(1.0, 1.0, 1.0);
            glPushMatrix();
                glBegin(GL_POLYGON);
                    glVertex3f(-6.0, 0.0, 0.0);
                    glVertex3f(112.0 ,0.0, 0.0);
                    glVertex3f(112.0, 130.0, 0.0);
                    glVertex3f(-6.0, 130.0, 0.0);
                glEnd();
                SetMaterial(0.0,1.0,1.0,1.0);
                glTranslatef(0.0, 0.0 ,0.5);
                glBegin(GL_LINES);   
                    glVertex3f(-6.0, 100.0, 0.0);
                    glVertex3f(112.0, 100.0, 0.0);
                    glVertex3f(100.0, 0.0, 0.0);
                    glVertex3f(100.0, 130.0, 0.0);
                glEnd();
            glPopMatrix();
        }

        //右足センサ座標計算
        //para2matrix(0,0,0,right_sh_pos.shita,matrix_shita,2);
        //para2matrix(0,0,0,right_sh_pos.alpha,matrix_alpha,0);
        //para2matrix(right_sh_pos.x, right_sh_pos.y, right_sh_pos.z,0,matrix_trans,3);
        //matrixmultiply(matrix_trans, matrix_shita,  matrix_temp);
        //matrixmultiply(matrix_temp,  matrix_alpha, matrix_sh_right);
        //m=0;
        //for(j=0;j<N;j++){
        //    for(k=0;k<N;k++){
        //        gl_matrix[m]= matrix_sh_right[k][j];
        //        m++;
        //    }
        //}
        //inverseMatrix(matrix_sh_right,  inv_mat_sh_right);
        //
        //for(i=0;i<us_point;i++){
        //    //inverseMatrix(left_us_matrix[i],  inv_mat_us_left);
        //    matrixmultiply(left_us_matrix[i], inv_mat_sh_right,matrix_temp_temp[i]);
        //    //matrixmultiply(inv_mat_sh_right,left_us_matrix[i],matrix_temp_temp[i]);

        //    transmatrix(matrix_temp_temp[i], us_right[i], left_for_right[i]);
        //    //transmatrix(inv_mat_us_left, us_right[i], temp_point);
        //    //temp.x=temp_point[0];
        //    //temp.x=temp_point[1];
        //    //temp.x=temp_point[2];
        //    //transmatrix(matrix_sh_right,temp, left_for_right[i]);
        //}
        //for(i=0;i<us_point;i++){
        //    m=0;
        //    for(j=0;j<N;j++){
        //        for(k=0;k<N;k++){
        //            inv_gl_matrix[i][m]= matrix_temp_temp[i][k][j];
        //            m++;
        //        }
        //    }
        //}
        //左足センサ
        k=0;
        glPushMatrix();
        //glTranslatef(0.0, 0.0, left_sh_height);
        //glRotatef(left_sh_angle, 1.0, 0.0, 0.0);
        glTranslatef(left_sh_pos.x, left_sh_pos.y, left_sh_pos.z);
        glRotatef(left_sh_pos.shita, 0.0, 0.0, 1.0);
        glRotatef(left_sh_pos.alpha, 1.0, 0.0, 0.0);
        //for(i=p_us_left;i<=p_us_left+3;i=i+3){
        //for(i=p_us_left;i<=p_us_left+1;i++){
        for(i=0;i<us_point_left;i++){
            glPushMatrix();
                ////glColor3f(0.0, 0.0, 1.0);
                glTranslatef(us_left[i].x,us_left[i].y,us_left[i].z);
                glRotatef(us_left[i].shita, 0.0,0.0,1.0);
                glRotatef(us_left[i].alpha, 1.0,0.0,0.0);
                //glMultMatrixd(left_gl_matrix[i]);
                //if(count==0){
                //    printf("m1[0]:% 7.5f m[4]:% 7.5f m[8] :% 7.5f m[12]:% 7.5f\n", left_gl_matrix[i][0], left_gl_matrix[i][4], left_gl_matrix[i][8],  left_gl_matrix[i][12]);
                //    printf("m1[1]:% 7.5f m[5]:% 7.5f m[9] :% 7.5f m[13]:% 7.5f\n", left_gl_matrix[i][1], left_gl_matrix[i][5], left_gl_matrix[i][9],  left_gl_matrix[i][13]);
                //    printf("m1[2]:% 7.5f m[6]:% 7.5f m[10]:% 7.5f m[14]:% 7.5f\n", left_gl_matrix[i][2], left_gl_matrix[i][6], left_gl_matrix[i][10], left_gl_matrix[i][14]);
                //    printf("m1[3]:% 7.5f m[7]:% 7.5f m[11]:% 7.5f m[16]:% 7.5f\n", left_gl_matrix[i][3], left_gl_matrix[i][7], left_gl_matrix[i][11], left_gl_matrix[i][15]);
                //    printf("\n");
                //    printf("m2[0]:% 7.5f m[4]:% 7.5f m[8] :% 7.5f m[12]:% 7.5f\n", sh_gl_left_matrix[i][0], sh_gl_left_matrix[i][4], sh_gl_left_matrix[i][8],  sh_gl_left_matrix[i][12]);
                //    printf("m2[1]:% 7.5f m[5]:% 7.5f m[9] :% 7.5f m[13]:% 7.5f\n", sh_gl_left_matrix[i][1], sh_gl_left_matrix[i][5], sh_gl_left_matrix[i][9],  sh_gl_left_matrix[i][13]);
                //    printf("m2[2]:% 7.5f m[6]:% 7.5f m[10]:% 7.5f m[14]:% 7.5f\n", sh_gl_left_matrix[i][2], sh_gl_left_matrix[i][6], sh_gl_left_matrix[i][10], sh_gl_left_matrix[i][14]);
                //    printf("m2[3]:% 7.5f m[7]:% 7.5f m[11]:% 7.5f m[16]:% 7.5f\n", sh_gl_left_matrix[i][3], sh_gl_left_matrix[i][7], sh_gl_left_matrix[i][11], sh_gl_left_matrix[i][15]);
                //    //count++;
                //}
                //glMultMatrixd(inv_gl_matrix[i]);
                if(roop_flag==0) {
                    SetMaterial(0.0,0.0,1.0,1.0); mySolidCylinder(0.5, 0.7,10);
                }
                //glPushMatrix();
                //    glTranslatef(left_for_right[i][0],left_for_right[i][1],left_for_right[i][2]);
                //    SetMaterial(0.0,1.0,1.0,1.0);
                //    glutSolidSphere(10, 5, 5);
                //glPopMatrix();
                glRotatef(90.0, 1.0,0.0,0.0);
                glGetDoublev(GL_MODELVIEW_MATRIX, sh_gl_left_matrix[i]);
                if(us_flag_left){
                    //if(k==1) SetMaterial(1.0,1.0,0.0,0.5);
                    //else
                    SetMaterial(0.0,1.0,0.0,0.5);
                    //glutSolidCone(51.3,141.0,10,10);
                    glPushMatrix();
                    if(solid_flag){
                        glTranslatef(0.0,0.0,-141.0);
                        glutSolidCone(51.3,141,20,1);
                    }
                    else{
                        glBegin(GL_POINTS);
                        for(j=0;j<18772;j++) glVertex3f(cone_point[j].x,cone_point[j].y,cone_point[j].z);
                        glEnd();
                        //S2=141*141*(5*5+5*5)-51.3*51.3*(-10)*(-10);
                        //if((S1*S2)>0) printf("IN\r");
                        //else printf("OUT\r");
                    }
                    //for(j=0;j<us_point;j++){
                    //S2=141*141*(5*5+5*5)-51.3*51.3*(-10)*(-10);

                    //}
                    glPopMatrix();
                }
                //k++;
            glPopMatrix();
        }
        glPopMatrix();

        //glDisable(GL_LIGHTING);

        //集合演算をしたいです・・・（エラー発生中）
        //glPushMatrix();
        //SetMaterial(0.0,1.0,0.0,1.0);
        //OpenCSG::render(primitives, OpenCSG::Automatic, OpenCSG::NoDepthComplexitySampling);
     //       glDepthFunc(GL_EQUAL);
     //       for (std::vector<OpenCSG::Primitive*>::const_iterator Iter = primitives.begin(); Iter != primitives.end(); ++Iter) {
     //               (*Iter)->render();
     //       }
     //       glDepthFunc(GL_LESS);
        //glPopMatrix();

        //右足センサ
        //glEnable(GL_LIGHTING);
        glPushMatrix();
        glTranslatef(right_sh_pos.x, right_sh_pos.y, right_sh_pos.z);
        glRotatef(right_sh_pos.shita, 0.0, 0.0, 1.0);
        glRotatef(right_sh_pos.alpha, 1.0, 0.0, 0.0);
        //m=0;
        //for(j=0;j<N;j++){
        //    for(k=0;k<N;k++){
        //        gl2_matrix[m]= matrix_alpha[k][j];
        //        m++;
        //    }
        //}

        //glMultMatrixd(gl_matrix);
        for(i=0;i<us_point_right;i++){
            glPushMatrix();
                ////glColor3f(0.0, 0.0, 1.0);
                glTranslatef(us_right[i].x,us_right[i].y,us_right[i].z);
                glRotatef(us_right[i].shita, 0.0,0.0,1.0);
                glRotatef(us_right[i].alpha, 1.0,0.0,0.0);
                //printf("%d\n",p_us_right);
                //glMultMatrixd(right_gl_matrix[i]);
               
                //if(count==0){
                    //printf("\n");
                    //printf("m3[0]:% 7.5f m[4]:% 7.5f m[8] :% 7.5f m[12]:% 7.5f\n", right_gl_matrix[i][0], right_gl_matrix[i][4], right_gl_matrix[i][8],  right_gl_matrix[i][12]);
                    //printf("m3[1]:% 7.5f m[5]:% 7.5f m[9] :% 7.5f m[13]:% 7.5f\n", right_gl_matrix[i][1], right_gl_matrix[i][5], right_gl_matrix[i][9],  right_gl_matrix[i][13]);
                    //printf("m3[2]:% 7.5f m[6]:% 7.5f m[10]:% 7.5f m[14]:% 7.5f\n", right_gl_matrix[i][2], right_gl_matrix[i][6], right_gl_matrix[i][10], right_gl_matrix[i][14]);
                    //printf("m3[3]:% 7.5f m[7]:% 7.5f m[11]:% 7.5f m[16]:% 7.5f\n", right_gl_matrix[i][3], right_gl_matrix[i][7], right_gl_matrix[i][11], right_gl_matrix[i][15]);
                    //printf("\n");
                    //printf("m4[0]:% 7.5f m[4]:% 7.5f m[8] :% 7.5f m[12]:% 7.5f\n", sh_gl_right_matrix[i][0], sh_gl_right_matrix[i][4], sh_gl_right_matrix[i][8],  sh_gl_right_matrix[i][12]);
                    //printf("m4[1]:% 7.5f m[5]:% 7.5f m[9] :% 7.5f m[13]:% 7.5f\n", sh_gl_right_matrix[i][1], sh_gl_right_matrix[i][5], sh_gl_right_matrix[i][9],  sh_gl_right_matrix[i][13]);
                    //printf("m4[2]:% 7.5f m[6]:% 7.5f m[10]:% 7.5f m[14]:% 7.5f\n", sh_gl_right_matrix[i][2], sh_gl_right_matrix[i][6], sh_gl_right_matrix[i][10], sh_gl_right_matrix[i][14]);
                    //printf("m4[3]:% 7.5f m[7]:% 7.5f m[11]:% 7.5f m[16]:% 7.5f\n", sh_gl_right_matrix[i][3], sh_gl_right_matrix[i][7], sh_gl_right_matrix[i][11], sh_gl_right_matrix[i][15]);
                    //count++;
                //}
                if(roop_flag==0){
                    SetMaterial(0.0,0.0,1.0,1.0); mySolidCylinder(0.5, 0.7,10);
                }
                glRotatef(90.0, 1.0,0.0,0.0);
                glGetDoublev(GL_MODELVIEW_MATRIX, sh_gl_right_matrix[i]);
                if(us_flag_right){
                    //if(i==p_us_right) SetMaterial(1.0,0.0,1.0,0.5);
                    //else
                    SetMaterial(1.0,0.0,0.0,0.5);
                    //glutSolidCone(51.3,141.0,10,10);
                    glPushMatrix();
                    if(solid_flag){
                        glTranslatef(0.0,0.0,-141.0);
                        glutSolidCone(51.3,141,20,1);
                    }
                    else{
                        glBegin(GL_POINTS);
                        for(j=0;j<18772;j++) glVertex3f(cone_point[j].x,cone_point[j].y,cone_point[j].z);
                        glEnd();
                    }
                    glPopMatrix();
                }
                //else{
                //    glBegin(GL_POINTS);
                //    for(i=0;i<18772;i++) glVertex3f(cone_point[i].x,cone_point[i].y,cone_point[i].z);
                //    glEnd();   
                //    }

            glPopMatrix();
        }
        glPopMatrix();
    //}
    //else{
    //    Motion();
    //}

    glPopMatrix();

    if(roop_flag==1){
            //for(i=0;i<2;i++){
            for(i=0;i<us_point_left;i++){
                glPushMatrix();
                glLoadIdentity();
                glMultMatrixd(sh_gl_left_matrix[i]);
                //glutSolidSphere(5.0,10.0,10.0);
                glPopMatrix();
                m=0;
                for(j=0;j<N;j++){
                    for(k=0;k<N;k++){
                        us_gl_left_matrix[i][k][j] = sh_gl_left_matrix[i][m];
                        m++;
                    }
                }
            }
            for(i=0;i<us_point_right;i++){
                glPushMatrix();
                glLoadIdentity();
                glMultMatrixd(sh_gl_right_matrix[i]);
                //glutSolidSphere(5.0,10.0,10.0);
                glPopMatrix();
                m=0;
                for(j=0;j<N;j++){
                    for(k=0;k<N;k++){
                        us_gl_right_matrix[i][k][j] = sh_gl_right_matrix[i][m];
                        m++;
                    }
                }
            }

            memcpy(matrix_temp_left,us_gl_left_matrix,sizeof(us_gl_left_matrix));
            memcpy(matrix_temp_right,us_gl_right_matrix,sizeof(us_gl_right_matrix));

            //for(i=0;i<2;i++)  inverseMatrix(matrix_temp_left[i], inv_us_gl_left_matrix[i]);
            for(i=0;i<us_point_left;i++)  inverseMatrix(matrix_temp_left[i], inv_us_gl_left_matrix[i]);
            
            for(i=0;i<us_point_right;i++) inverseMatrix(matrix_temp_right[i], inv_us_gl_right_matrix[i]);
            
            //for(j=0;j<2;j++){
            for(j=0;j<us_point_left;j++){
                glPushMatrix();
                glLoadIdentity();
                glMultMatrixd(sh_gl_left_matrix[j]);
                //glutSolidSphere(5.0,10.0,10.0);
                for(i=0;i<us_point_right;i++){
                        //inverseMatrix(us_gl_right_matrix[i], inv_us_gl_right_matrix[i]);            
                        matrixmultiply(inv_us_gl_left_matrix[j],us_gl_right_matrix[i],l_for_r_mat);

                        //glPushMatrix();
                        //    glTranslatef(l_for_r_mat[j][0][3],l_for_r_mat[j][1][3],l_for_r_mat[j][2][3]);
                        //    glutSolidCube(5.0);
                        //glPopMatrix();
                        //if(count<us_point){
                        //    printf("%d x=%3.3f y=%3.3f z=%3.3f\n",i,l_for_r_mat[i][0][3], l_for_r_mat[i][1][3],l_for_r_mat[i][2][3]);
                        //    count++;
                        //}
                        //glPushMatrix();
                        //glTranslatef(l_for_r_mat[i][0][3], l_for_r_mat[i][1][3],l_for_r_mat[i][2][3]);
                        //glutSolidSphere(2.0,10.0,10.0);
                        //glPopMatrix();
                        
                        S2=180*180*(l_for_r_mat[0][3]*l_for_r_mat[0][3] + l_for_r_mat[1][3]*l_for_r_mat[1][3])-103.9*103.9*l_for_r_mat[2][3]*l_for_r_mat[2][3];
                        //S2=180*180*(l_for_r_mat[0][3]*l_for_r_mat[0][3] + l_for_r_mat[1][3]*l_for_r_mat[1][3])-65.5*65.5*l_for_r_mat[2][3]*l_for_r_mat[2][3];
                        if(S1*S2>0&&l_for_r_mat[2][3]<0) left_in[i][j]=1;//fprintf(input_left, "%3.2f,%3.2f,%3.2f,%3.2f,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,j,i); //printf("left%d for right%d IN\n",j,i);
                        //if(S1*S2>0) left_in[j]=1;
                    }
                glPopMatrix();
            }
            
            for(j=0;j<us_point_right;j++){
                glPushMatrix();
                glLoadIdentity();
                glMultMatrixd(sh_gl_right_matrix[j]);
                //glutSolidSphere(5.0,10.0,10.0);
                for(i=0;i<us_point_left;i++){
                        //inverseMatrix(us_gl_right_matrix[i], inv_us_gl_right_matrix[i]);            
                        matrixmultiply(inv_us_gl_right_matrix[j],us_gl_left_matrix[i],r_for_l_mat);
                        //glPushMatrix();
                        //    glTranslatef(r_for_l_mat[i][0][3],r_for_l_mat[i][1][3],r_for_l_mat[i][2][3]);
                        //    glutSolidCube(5.0);
                        //glPopMatrix();
                        //if(count<us_point){
                        //    printf("%d x=%3.3f y=%3.3f z=%3.3f\n",i,l_for_r_mat[i][0][3], l_for_r_mat[i][1][3],l_for_r_mat[i][2][3]);
                        //    count++;
                        //}
                        //glPushMatrix();
                        //glTranslatef(r_for_l_mat[i][0][3], r_for_l_mat[i][1][3],r_for_l_mat[i][2][3]);
                        //glutSolidSphere(2.0,10.0,10.0);
                        //glPopMatrix();
                        S2=180*180*(r_for_l_mat[0][3]*r_for_l_mat[0][3] + r_for_l_mat[1][3]*r_for_l_mat[1][3])-103.9*103.9*r_for_l_mat[2][3]*r_for_l_mat[2][3];
                        //S2=180*180*(r_for_l_mat[0][3]*r_for_l_mat[0][3] + r_for_l_mat[1][3]*r_for_l_mat[1][3])-65.5*65.5*r_for_l_mat[2][3]*r_for_l_mat[2][3];
                        if(S1*S2>0&&r_for_l_mat[2][3]<0) right_in[j][i]=1; //fprintf(input_right, "%3.2f,%3.2f,%3.2f,%3.2f,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,j,i);//printf("right%d for left%d IN\n",j,i);
                        //if(j==2&&i==30) printf("%f %f %f\r",inv_us_gl_right_matrix[2][0][3],inv_us_gl_right_matrix[2][1][3],inv_us_gl_right_matrix[2][2][3]);
                        //if(S1*S2>0) right_in[i]=1;
                }

glPopMatrix();
                //printf("%.2f,%.2f\r",test[0],test[1]);
            }
            count_us_r_l=0;
            for(i=0;i<us_point_right;i++){
                //count_us_r_l=0;
                for(j=0;j<us_point_left;j++){
                    if(right_in[i][j]==1 && left_in[i][j]==1) count_us_r_l++;
                }
                //printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r",left_in[2][0]*right_in[2][0], left_in[2][1]*right_in[2][1], left_in[2][2]*right_in[2][2], left_in[2][3]*right_in[2][3], left_in[2][4]*right_in[2][4], left_in[2][5]*right_in[2][5], left_in[2][6]*right_in[2][6],left_in[2][7]*right_in[2][7],
                                            //left_in[2][8]*right_in[2][8], left_in[2][9]*right_in[2][9], left_in[2][10]*right_in[2][10], left_in[2][11]*right_in[2][11], left_in[2][12]*right_in[2][12], left_in[2][13]*right_in[2][13], left_in[2][14]*right_in[2][14], left_in[2][15]*right_in[2][15],
                                            //left_in[2][16]*right_in[2][16], left_in[2][17]*right_in[2][17], left_in[2][18]*right_in[2][18], left_in[2][19]*right_in[2][19], left_in[2][20]*right_in[2][20], left_in[2][21]*right_in[2][21], left_in[2][22]*right_in[2][22], left_in[2][23]*right_in[2][23],
                                            //left_in[2][24]*right_in[2][24], left_in[2][25]*right_in[2][25], left_in[2][26]*right_in[2][26], left_in[2][27]*right_in[2][27], left_in[2][28]*right_in[2][28], left_in[2][29]*right_in[2][29], left_in[2][30]*right_in[2][30], left_in[2][31]*right_in[2][31], left_in[2][32]*right_in[2][32]);
                //
                //if(count_us_r_l>=2) printf("%3.2f OK\r",right_sh_pos.shita);
                ////else printf("%3.2f NO\r",right_sh_pos.shita);

                /*if(count_us_r_l>=1) fprintf(input_0, "%3.2f,%3.2f,%3.2f,%3.2f,%d,%3.0f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,i,us_left[0].shita,
                                            left_in[i][0]*right_in[i][0], left_in[i][1]*right_in[i][1], left_in[i][2]*right_in[i][2], left_in[i][3]*right_in[i][3], left_in[i][4]*right_in[i][4], left_in[i][5]*right_in[i][5], left_in[i][6]*right_in[i][6],left_in[i][7]*right_in[i][7],
                                            left_in[i][8]*right_in[i][8], left_in[i][9]*right_in[i][9], left_in[i][10]*right_in[i][10], left_in[i][11]*right_in[i][11], left_in[i][12]*right_in[i][12], left_in[i][13]*right_in[i][13], left_in[i][14]*right_in[i][14], left_in[i][15]*right_in[i][15],
                                            left_in[i][16]*right_in[i][16], left_in[i][17]*right_in[i][17], left_in[i][18]*right_in[i][18], left_in[i][19]*right_in[i][19], left_in[i][20]*right_in[i][20], left_in[i][21]*right_in[i][21], left_in[i][22]*right_in[i][22], left_in[i][23]*right_in[i][23],
                                            left_in[i][24]*right_in[i][24], left_in[i][25]*right_in[i][25], left_in[i][26]*right_in[i][26], left_in[i][27]*right_in[i][27], left_in[i][28]*right_in[i][28], left_in[i][29]*right_in[i][29], left_in[i][30]*right_in[i][30], left_in[i][31]*right_in[i][31], left_in[i][32]*right_in[i][32]);
                */
                //if(count_us_r_l>=1) fprintf(input_0, "%3.2f,%3.2f,%3.2f,%3.2f,%d,%3.0f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,i,us_left[0].shita,left_in[i][0]*right_in[i][0], left_in[i][1]*right_in[i][1], left_in[i][2]*right_in[i][2], left_in[i][3]*right_in[i][3], left_in[i][4]*right_in[i][4], left_in[i][5]*right_in[i][5], left_in[i][6]*right_in[i][6],left_in[i][7]*right_in[i][7],left_in[i][8]*right_in[i][8], left_in[i][9]*right_in[i][9], left_in[i][10]*right_in[i][10], left_in[i][11]*right_in[i][11]); //left_in[i][12]*right_in[i][12], left_in[i][13]*right_in[i][13], left_in[i][14]*right_in[i][14]);
                //if(count_us_r_l>=1) fprintf(input_0, "%3.2f,%3.2f,%3.2f,%3.2f,%d,%3.0f\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,i,us_left[0].shita);
            
            }
            if(count_us_r_l>=2) fprintf(input_0, "%3.2f,%3.2f,%3.2f,%3.2f,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,i,count_us_r_l);
            count_us_r_l=0;
            //if(left_in[5][10]==1&&left_in[5][11]==1&&right_in[5][10]==1&&right_in[5][11]==1) printf("OK %f\r",right_sh_pos.alpha);
            //else printf("NO%f\r",right_sh_pos.alpha);

            //for(i=0;i<us_point_right;i++){
            //    if(left_in[i][0]==1&&left_in[i][1]==1&&right_in[i][0]==1&&right_in[i][1]==1){
            //                    fprintf(input_0, "OK,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%d,%d,%d,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,
            //                        us_left[p_us_left].x,us_left[p_us_left].y,us_left[p_us_left].z,us_left[p_us_left].shita,
            //                        us_left[p_us_left+3].x,us_left[p_us_left+3].y,us_left[p_us_left+3].z,us_left[p_us_left+3].shita,
            //                        us_right[p_us_right].x,us_right[p_us_right].y,us_right[p_us_right].z,
            //                        p_us_left,left_in[i][0]*right_in[i][0],p_us_left+3,left_in[i][1]*right_in[i][1],i);
            //    }
            //    else if((left_in[i][0]==1&&right_in[i][0]==1)||(left_in[i][1]==1&&right_in[i][1]==1)){
            //                    fprintf(input_0, "NO,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%d,%d,%d,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,
            //                        us_left[p_us_left].x,us_left[p_us_left].y,us_left[p_us_left].z,us_left[p_us_left].shita,
            //                        us_left[p_us_left+3].x,us_left[p_us_left+3].y,us_left[p_us_left+3].z,us_left[p_us_left+3].shita,
            //                        us_right[p_us_right].x,us_right[p_us_right].y,us_right[p_us_right].z,
            //                        p_us_left,left_in[i][0]*right_in[i][0],p_us_left+3,left_in[i][1]*right_in[i][1],i);
            //    }
            //}
            //}
            //if(left_in[p_us_right][0]==1&&left_in[p_us_right][1]==1&&right_in[p_us_right][0]==1&&right_in[p_us_right][1]==1){
            //        printf("%d,OK,%3.2f,%3.2f,%3.2f,\r",p_us_right,right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita);
            //}
            //else printf("%d,NO,%3.2f,%3.2f,%3.2f,\r",p_us_right,right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita);
                //    switch(p_us_right){
                //        case 0:
                //            fprintf(input_0, "%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%d,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,
                //                    us_left[p_us_left].x,us_left[p_us_left].y,us_left[p_us_left].z,us_left[p_us_left].shita,
                //                    us_left[p_us_left+3].x,us_left[p_us_left+3].y,us_left[p_us_left+3].z,us_left[p_us_left+3].shita,
                //                    us_right[p_us_right].x,us_right[p_us_right].y,us_right[p_us_right].z,
                //                    p_us_left,p_us_left+3,p_us_right);
                //            break;
                //        case 1:
                //            fprintf(input_1, "%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%d,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,
                //                    us_left[p_us_left].x,us_left[p_us_left].y,us_left[p_us_left].z,us_left[p_us_left].shita,
                //                    us_left[p_us_left+3].x,us_left[p_us_left+3].y,us_left[p_us_left+3].z,us_left[p_us_left+3].shita,
                //                    us_right[p_us_right].x,us_right[p_us_right].y,us_right[p_us_right].z,
                //                    p_us_left,p_us_left+3,p_us_right);
                //            break;
                //        case 2:
                //            fprintf(input_2, "%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%d,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,
                //                    us_left[p_us_left].x,us_left[p_us_left].y,us_left[p_us_left].z,us_left[p_us_left].shita,
                //                    us_left[p_us_left+3].x,us_left[p_us_left+3].y,us_left[p_us_left+3].z,us_left[p_us_left+3].shita,
                //                    us_right[p_us_right].x,us_right[p_us_right].y,us_right[p_us_right].z,
                //                    p_us_left,p_us_left+3,p_us_right);
                //            break;
                //        case 3:
                //            fprintf(input_3, "%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%d,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,
                //                    us_left[p_us_left].x,us_left[p_us_left].y,us_left[p_us_left].z,us_left[p_us_left].shita,
                //                    us_left[p_us_left+3].x,us_left[p_us_left+3].y,us_left[p_us_left+3].z,us_left[p_us_left+3].shita,
                //                    us_right[p_us_right].x,us_right[p_us_right].y,us_right[p_us_right].z,
                //                    p_us_left,p_us_left+3,p_us_right);
                //            break;
                //        case 4:
                //            fprintf(input_4, "%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%d,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,
                //                    us_left[p_us_left].x,us_left[p_us_left].y,us_left[p_us_left].z,us_left[p_us_left].shita,
                //                    us_left[p_us_left+3].x,us_left[p_us_left+3].y,us_left[p_us_left+3].z,us_left[p_us_left+3].shita,
                //                    us_right[p_us_right].x,us_right[p_us_right].y,us_right[p_us_right].z,
                //                    p_us_left,p_us_left+3,p_us_right);
                //            break;
                //        case 5:
                //            fprintf(input_5, "%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%d,%d,%d\n",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita,right_sh_pos.alpha,
                //                    us_left[p_us_left].x,us_left[p_us_left].y,us_left[p_us_left].z,us_left[p_us_left].shita,
                //                    us_left[p_us_left+3].x,us_left[p_us_left+3].y,us_left[p_us_left+3].z,us_left[p_us_left+3].shita,
                //                    us_right[p_us_right].x,us_right[p_us_right].y,us_right[p_us_right].z,
                //                    p_us_left,p_us_left+3,p_us_right);
                //            break;
                //    }
                //                                            //printf("OK\n");
                //                                            //printf("OK,%3.2f,%3.2f,%3.2f,\r",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita);
                //}
                //else printf("NO,%3.2f,%3.2f,%3.2f,\r",right_sh_pos.x,right_sh_pos.y,right_sh_pos.shita);
    }
    //if(p_us_right>0) printf("Yes\n");
    //glDisable(GL_LIGHTING);
   
    //glPushMatrix();
    ////sprintf(ss,"%3.2f",right_sh_pos.shita);
    ////DrawString(ss,  -0.8,  -0.8 );
    //printf("left:%3.2f %3.2f right:%3.2f %3.2f %3.2f %3.2f %3.2f\r",left_sh_height,left_sh_angle,right_sh_pos.x,right_sh_pos.y,right_sh_pos.z,right_sh_pos.shita,right_sh_pos.alpha);
    //glPopMatrix();

    glDisable(GL_LIGHTING);
    glDisable(GL_BLEND);
    glDisable( GL_DEPTH_TEST );
    glPopMatrix();

    glutSwapBuffers();
}


void idle(void)
{
    int i;
    //theta = fmod( theta + 0.5, 360.0 );
    if(roop_flag){
        right_sh_pos.shita = right_sh_pos.shita + 10.0;
        if(right_sh_pos.shita > 30.0||(right_sh_pos.y<30&&right_sh_pos.x<21.0&&right_sh_pos.shita>0)){
            right_sh_pos.shita=-90.0;
            right_sh_pos.x=right_sh_pos.x+2.0;
            if(right_sh_pos.x>100){
                right_sh_pos.y=right_sh_pos.y+2.0;
                if(right_sh_pos.y>30.0)    right_sh_pos.x=0.0;
                else right_sh_pos.x=10.0;
                if(right_sh_pos.y>100){
                    right_sh_pos.alpha=right_sh_pos.alpha+10.0;
                    left_sh_pos.alpha=left_sh_pos.alpha-10.0;
                    left_sh_pos.z=28.0*sin(-PI*left_sh_pos.alpha/180.0);
                    right_sh_pos.shita=-90.0;
                    right_sh_pos.x=10.0;
                    right_sh_pos.y=0.0;
                    if(right_sh_pos.alpha>20){
                        roop_flag=0;
                        shoe_pos_reset();
                        printf("シミュレーション終了");
                        //for(i=0;i<us_point_left;i++) us_left[i].shita = us_left[i].shita - 10.0;
                        //if(us_left[0].shita<-90){   
                        //right_sh_pos.alpha=0.0;
                        //    roop_flag=0;
                        //    printf("シミュレーション終了");
                        //}
                           
                            //for(i=0;i<us_point_left;i++){
                            //    us_left[i].shita=us_left[i].shita - 10.0;
                            //}
                            //us_left[p_us_left].shita=us_left[p_us_left].shita - 10.0;
                            //us_left[p_us_left+3].shita=us_left[p_us_left+3].shita - 10.0;
                            //if(us_left[p_us_left].x<0 && us_left[p_us_left].shita>31){
                            //    p_us_left++;
                            //    us_left[p_us_left].shita=0.0;
                            //    us_left[p_us_left+3].shita=0.0;
                            //}
                            //else if(((us_left[p_us_left].x>0 && us_left[p_us_left].x<3.0)||(us_left[p_us_left+3].x>0 && us_left[p_us_left+3].x<3.0))&&(us_left[p_us_left].shita<-50.0)){
                            //        p_us_left++;
                            //    if((us_left[p_us_left].x>0 && us_left[p_us_left].x<3.0)&&(us_left[p_us_left+3].x>0 && us_left[p_us_left+3].x<3.0)){
                            //        us_left[p_us_left].shita=0.0;
                            //        us_left[p_us_left+3].shita=0.0;
                            //    }
                            //    else if(us_left[p_us_left].x>3.0||us_left[p_us_left+3].x>3.0){
                            //        us_left[p_us_left].shita=-20.0;
                            //        us_left[p_us_left+3].shita=-20.0;
                            //    }
                            //}
                            //else if((us_left[p_us_left].x>3.0||us_left[p_us_left+3].x>3.0)&&us_left[p_us_left].shita<-90.0){
                            //        p_us_left++;
                            //        if(p_us_left+3>32){
                            //            shoe_pos_reset();
                            //            for(i=0;i<us_point_left;i++) us_left[i].shita=in_us_left[i].shita;
                            //            //p_us_right++;
                            //            //printf("%d\r",p_us_right);
                            //            p_us_left=0;
                            //            //if(p_us_right>5){
                            //                roop_flag=0;
                            //                //p_us_right=0;
                            //                printf("シミュレーション終了");
                            //            //}
                            //        }
                            //        else{
                            //            us_left[p_us_left].shita=-20.0;
                            //            us_left[p_us_left+3].shita=-20.0;
                            //        }
                            //}
                    }

                            //printf("終了\n");
                }
            }
        }
    }

    glutPostRedisplay();
}


void myKbd( unsigned char key, int x, int y )
{

    int i;

    if ( key == KEY_ESC ) exit( 0 );

    //カメラ位置
    if ( key == 'a' ){
        if(camera_flag == 0) dis = dis + 0.5;
        else lx = lx + 0.5;
    }

    if ( key == 'z' ){
        if(camera_flag == 0) dis = dis - 0.5;
        else lx = lx - 0.5;
    }

    if ( key == 's' ){
        if(camera_flag == 0) tws = tws + 0.5;
        else ly = ly + 0.5;
    }

    if ( key == 'x' ){
        if(camera_flag == 0) tws = tws - 0.5;
        else ly = ly - 0.5;
    }

    if ( key == 'd' ){
        if(camera_flag == 0) ele = ele + 0.5;
        else lz = lz + 0.5;
    }

    if ( key == 'c' ){
        if(camera_flag == 0) ele = ele - 0.5;
        else lz = lz - 0.5;
    }

    if ( key == 'f' ) azi = azi + 0.5;

    if ( key == 'v' ) azi = azi - 0.5;

    if ( key == '0' ){
        camera_flag = 0;
        camera_pos_reset();
    }

    if ( key == '1' ){
        camera_flag = 1;
        lx = 0.0; ly=-170.0; lz=0.0;
        dx=0.0; dy=0.0, dz=0.0;
        sx=0.0; sy=0.0; sz=1.0;
    }

    if ( key == '2' ){
        camera_flag = 2;
        lx = -170.0; ly=0.0; lz=0.0;
        dx=0.0; dy=0.0, dz=0.0;
        sx=0.0; sy=0.0; sz=1.0;
    }

    if ( key == '3' ){
        camera_flag = 3;
        lx = 0.0; ly=170.0; lz=0.0;
        dx=0.0; dy=0.0, dz=0.0;
        sx=0.0; sy=0.0; sz=1.0;
    }

    if ( key == '4' ){
        camera_flag = 4;
        lx = 170.0; ly=0.0; lz=0.0;
        dx=0.0; dy=0.0, dz=0.0;
        sx=0.0; sy=0.0; sz=1.0;
    }

    if ( key == '5' ){
        camera_flag = 5;
        lx = 120.0; ly=0.0; lz=120.0;
        dx=0.0; dy=0.0, dz=0.0;
        sx=0.0; sy=0.0; sz=1.0;
    }

    if ( key == '6' ){
        camera_flag = 5;
        lx = -120.0; ly=0.0; lz=120.0;
        dx=0.0; dy=0.0, dz=0.0;
        sx=0.0; sy=0.0; sz=1.0;
    }

    if ( key == '7' ){
        camera_flag = 5;
        lx = 0.0; ly=-120.0; lz=120.0;
        dx=0.0; dy=0.0, dz=0.0;
        sx=0.0; sy=0.0; sz=1.0;
    }
    if ( key == '8' ){
        camera_flag = 5;
        lx = 0.0; ly=120.0; lz=120.0;
        dx=0.0; dy=0.0, dz=0.0;
        sx=0.0; sy=0.0; sz=1.0;
    }

    //超音波センサ表示切り替え
    if ( key == 'p' ) us_flag_right = 1 - us_flag_right;

    if ( key == 'o' ) us_flag_left = 1 - us_flag_left;

    //右足位置姿勢
    if ( key == 't' ) right_sh_pos.x = right_sh_pos.x + 0.5;

    if ( key == 'g' ) right_sh_pos.x = right_sh_pos.x - 0.5;

    if ( key == 'y' ) right_sh_pos.y = right_sh_pos.y + 0.5;

    if ( key == 'h' ) right_sh_pos.y = right_sh_pos.y - 0.5;

    if ( key == 'u' ) right_sh_pos.z = right_sh_pos.z + 0.5;

    if ( key == 'j' ) right_sh_pos.z = right_sh_pos.z - 0.5;

    if ( key == 'i' ) right_sh_pos.shita = right_sh_pos.shita + 0.5;

    if ( key == 'k' ) right_sh_pos.shita = right_sh_pos.shita - 0.5;

    if ( key == 'n' ) right_sh_pos.alpha = right_sh_pos.alpha + 0.5;

    if ( key == 'm' ) right_sh_pos.alpha = right_sh_pos.alpha - 0.5;

    if ( key == '9' ) shoe_pos_reset();

    //左足姿勢
    if ( key == 'q' ) left_sh_pos.z = left_sh_pos.z + 0.5;

    if ( key == 'w' ) left_sh_pos.z = left_sh_pos.z - 0.5;

    if ( key == 'e' ) left_sh_pos.alpha = left_sh_pos.alpha + 0.5;

    if ( key == 'r' ) left_sh_pos.alpha = left_sh_pos.alpha - 0.5;

    if ( key == ' ' ){
        mot_flag = 1 - mot_flag;
        if(mot_flag==0) stop_mot=0;
    }

    if ( key == ';' ) {
        if(mot_flag) stop_mot = 1 - stop_mot;
    }

    if ( key == ':' ){
        roop_flag = 1 - roop_flag;
        if(roop_flag==1){
            right_sh_pos.x=0.0;
            right_sh_pos.y=0.0;
            right_sh_pos.shita = - 90.0;
            us_flag_left=0;
            us_flag_right=0;
        }
    }

    if ( key == '@' ) roop_flag = 1 - roop_flag;
    if ( key == '-' ) solid_flag = 1 - solid_flag;

    if ( key == ',' ) {
        p_us_left++;
        if(p_us_left+3>6) us_left[p_us_left].shita=-20.0;
        if(p_us_left+3>32){
            p_us_left = 0;
            for(i=0;i<us_point_left;i++) us_left[i]=in_us_left[i];
        }
        //if(p_us_left>12) p_us_left=0;
    }
    if ( key == '.' ) {
        p_us_right++;
        if(p_us_right>5) p_us_right = 0;
    }
}


void myInit (char *progname)
{
    int width = 800, height = 800;
    float aspect = (float) width / (float) height;

    glutInitWindowPosition(0, 0);
    glutInitWindowSize( width, height);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
    glutCreateWindow(progname);
    glClearColor (0.0, 0.0, 0.0, 1.0);
    glutKeyboardFunc( myKbd );
    //glutMouseFunc(Mouse);            // マウス入力時の処理関数の設定
    //glutMotionFunc(Mouse_Motion);            // マウスドラッグ時の処理関数の設定
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45.0, aspect, 0.5, 500.0);
    glMatrixMode(GL_MODELVIEW);
}


int main(int argc, char** argv)
{

    int i,j,k,m;
    double matrix_alpha[4][4];
    double matrix_shita[4][4];
    double matrix_trans[4][4];
    double matrix_temp[4][4];
    FILE *fp_left4cm;
    FILE *fp_right4cm;
    FILE *fp_test;
    double left_length = 0;
    double right_length = 0;
    double length=0;
    double n=0.01;
    double temp_x;
    double temp_y;
    double temp;
    int flag=0;

    fp_left=fopen(file_left,"r");
    fp_right=fopen(file_right,"r");
    fp_us_left = fopen("us_position_left.csv","r");
    fp_us_right = fopen("us_position_right.csv","r");
    input_0 = fopen("simu1.csv","w");
    //input_1 = fopen("simu2.csv","w");
    //input_2 = fopen("simu3.csv","w");
    //input_3 = fopen("simu4.csv","w");
    //input_4 = fopen("simu5.csv","w");
    //input_5 = fopen("simu6.csv","w");
    //input_right = fopen("simu_right.csv","w");
    fp_left4cm = fopen("left_z4cm.txt","r");
    //fp_right4cm = fopen("right_z4cm.txt","r");
    fp_test = fopen("test_left.csv","w");
   
    printf("読み込み中\n");
    for(i=0;i<shoe_point;i++){
        fscanf(fp_left,"%f %f %f",&sh_left[i].x, &sh_left[i].y, &sh_left[i].z);
    }
    for(i=0;i<shoe_point;i++){
        fscanf(fp_right,"%f %f %f",&sh_right[i].x, &sh_right[i].y, &sh_right[i].z);
    }