function cos_azimuth(int label_A, int label_B, float rot2_p, float rot2_v) { float R = get_length(label_A, label_B); float rot_p, rot_v, rot_b; float XA, YA, ZA; float XB, YB, ZB; get_rot_abs(label_A, rot_p, rot_v, rot_b); get_pos(label_A, XA, YA, ZA); //座標を取得 get_pos(label_B, XB, YB, ZB); //座標を取得 int x = 0; // 配列添え字は0からスタート int y = 1; int z = 2; float vec1[3]; // 配列要素3個確保するなら、添え字は0〜2まで vec1[x]= XB-XA; vec1[y]= YB-YA; vec1[z]= ZB-ZA; float vec2[3]; vec2[x]= 0.0; vec2[y]= 0.0; vec2[z]= 1.0; // 列 行 float mat[5][3][3]; // 同じく添え字は0からスタートする // | 1 0 0 | //1 | 0 cosφp sinφp| // | 0 -sinφp cosφp| mat[0][0][0] = 1.0; mat[0][0][1] = 0.0; mat[0][0][2] = 0.0; mat[0][1][0] = 0.0; mat[0][1][1] = cos_deg(rot2_p); mat[0][1][2] = sin_deg(rot2_p); mat[0][2][0] = 0.0; mat[0][2][1] = -sin_deg(rot2_p); mat[0][2][2] = cos_deg(rot2_p); // | cosφv 0 sinφv| //2 | 0 1 0 | // | -sinφv 0 cosφv| mat[1][0][0] = cos_deg(rot2_v); mat[1][0][1] = 0.0; mat[1][0][2] = sin_deg(rot2_v); mat[1][1][0] = 0.0; mat[1][1][1] = 1.0; mat[1][1][2] = 0.0; mat[1][2][0] = -sin_deg(rot2_v); mat[1][2][1] = 0.0; mat[1][2][2] = cos_deg(rot2_v); // |cosψb -sinψb 0 | //3 |sinψb cosψb 0 | // | 0 0 1 | mat[2][0][0] = cos_deg(rot_b); mat[2][0][1] = -sin_deg(rot_b); mat[2][0][2] = 0.0; mat[2][1][0] = sin_deg(rot_b); mat[2][1][1] = cos_deg(rot_b); mat[2][1][2] = 0.0; mat[2][2][0] = 0.0; mat[2][2][1] = 0.0; mat[2][2][2] = 1.0; // | 1 0 0 | //4 | 0 cosψp sinψp| // | 0 -sinψp cosψp| mat[3][0][0] = 1.0; mat[3][0][1] = 0.0; mat[3][0][2] = 0.0; mat[3][1][0] = 0.0; mat[3][1][1] = cos_deg(rot_p); mat[3][1][2] = sin_deg(rot_p); mat[3][2][0] = 0.0; mat[3][2][1] = -sin_deg(rot_p); mat[3][2][2] = cos_deg(rot_p); // | cosψv 0 sinψv| //5 | 0 1 0 | // | -sinψv 0 cosψv| mat[4][0][0] = cos_deg(rot_v); mat[4][0][1] = 0.0; mat[4][0][2] = sin_deg(rot_v); mat[4][1][0] = 0.0; mat[4][1][1] = 1.0; mat[4][1][2] = 0.0; mat[4][2][0] = -sin_deg(rot_v); mat[1][2][1] = 0.0; mat[4][2][2] = cos_deg(rot_v); float vec3[3]; // for文の外で宣言する for(n=0;n<=4;n++){ vec3[x] = 0.0; // for文の中で明示的に0.0で初期化する vec3[y] = 0.0; vec3[z] = 0.0; // 行列とベクトルの積を計算 for(i=0;i<=2;i++){ for(j=0;j<=2;j++){ vec3[i] += mat[n][i][j]*vec2[j]; } } vec2[x] = vec3[x]; // 次の周回で使用するためにvec2に代入 vec2[y] = vec3[y]; vec2[z] = vec3[z]; } float cos_a; // RSEでは明示的に宣言しなくても大丈夫だが念のため cos_a = (vec1[x]*vec2[x]+vec1[y]*vec2[y]+vec1[z]*vec2[z]) / R; return cos_a; }