JunkBox_Win_Lib: WinNiLib/NiJTextTool.cpp Source File

00001 
00002 #include  "tools++.h"
00003 #include  "NiJTextTool.h"
00004 
00005 
00006 using namespace jbxl;
00007 using namespace jbxwl;
00008 
00009 
00011 // CNiJTextTool Class
00012 
00013 CNiJTextTool::CNiJTextTool(void)
00014 {
00015     posVect = (Vector<double>*)malloc(sizeof(Vector<double>)*NI_TOTAL_JOINT_NUM);
00016     rotQuat = (Quaternion<double>*)malloc(sizeof(Quaternion<double>)*NI_TOTAL_JOINT_NUM);
00017     jntAngl = NULL;
00018 }
00019 
00020 
00021 
00022 CNiJTextTool::~CNiJTextTool(void)
00023 {
00024     DEBUG_INFO("DESTRUCTOR: CNiJTextTool\n");
00025 
00026     free_data();
00027 }
00028 
00029 
00030 
00031 void  CNiJTextTool::free_data(void)
00032 {
00033     CBaseFrameTool::free_data();
00034 
00035     ::freeNull(jntAngl);
00036 }
00037 
00038 
00039 
00040 void  CNiJTextTool::clear_data(void)
00041 {
00042     free_data();
00043 }
00044 
00045 
00046 
00048 //
00049 
00050 BOOL  CNiJTextTool::readFile(FILE* fp)
00051 {
00052     if (fp==NULL) return FALSE;
00053 
00054     clear_data();
00055 
00056     Buffer buf = make_Buffer(LDATA);    // Line 読み込み用バッファ
00057 
00058     // ファイルヘッダのチェック
00059     fgets_Buffer(&buf, fp);
00060     if (strncmp(NI_JTXT_FILE_ID, (char*)buf.buf, strlen(NI_JTXT_FILE_ID))) {
00061         free_Buffer(&buf);
00062         return FALSE;
00063     }
00064 
00065     // フレーム数を数える
00066     int frames = 0;
00067     fgets_Buffer(&buf, fp);
00068     while(!feof(fp)) {
00069         if (buf.buf[0]>='0' && buf.buf[0]<='9') frames++; // 行頭が数字の場合はフレーム時間
00070         fgets_Buffer(&buf, fp);
00071     }
00072     if (frames==0) {
00073         free_Buffer(&buf);
00074         return FALSE;
00075     }
00076     frames_num = frames;
00077 
00078     // 各フレームの開始時間を調べる
00079     int* frame_msec = (int*)malloc(frames_num*sizeof(int));
00080     memset(frame_msec, 0, frames_num*sizeof(int));
00081 
00082     fseek(fp, 0, SEEK_SET);
00083     fgets_Buffer(&buf, fp);
00084     // 最初のフレーム時間行まで移動
00085     while (buf.buf[0]<'0' || buf.buf[0]>'9') fgets_Buffer(&buf, fp);
00086 
00087     int joints = 0;
00088     for (unsigned int i=0; i<frames_num; i++) {
00089         frame_msec[i] = atoi((char*)buf.buf);
00090         //
00091         int jnum = 0;
00092         fgets_Buffer(&buf, fp);
00093         while (!feof(fp) && (buf.buf[0]<'0' || buf.buf[0]>'9')) {
00094             if (buf.buf[0]==' ') jnum++;
00095             fgets_Buffer(&buf, fp);
00096         }
00097         joints = Max(joints, jnum);
00098     }
00099     joints_num = joints;
00100 
00101     //
00102     // データ領域の確保
00103     //
00104     jointsData = (NiJointData*)malloc(sizeof(NiJointData)*joints_num);
00105     if (jointsData==NULL) {
00106         clear_data();
00107         return FALSE;
00108     }
00109     clearJointsData(joints_num);
00110 
00111     // Frame Data の格納先を作る
00112     framesData = makeFramesData(frames_num, joints_num, frame_msec);
00113     ::free(frame_msec);
00114     if (framesData==NULL) {
00115         free_Buffer(&buf);
00116         clear_data();
00117         return FALSE;
00118     }
00119 
00120     if (posVect==NULL) posVect = (Vector<double>*)malloc(sizeof(Vector<double>)*NI_TOTAL_JOINT_NUM);
00121     if (rotQuat==NULL) rotQuat = (Quaternion<double>*)malloc(sizeof(Quaternion<double>)*NI_TOTAL_JOINT_NUM);
00122     if (posVect==NULL || rotQuat==NULL) {
00123         free_Buffer(&buf);
00124         clear_data();
00125         return FALSE;
00126     }
00127     clearVectorData(NI_TOTAL_JOINT_NUM);
00128 
00129     //
00130     // データの読み込み
00131     //
00132     BOOL tmline = FALSE;
00133     int  prvms = 0;
00134     fseek(fp, 0, SEEK_SET);
00135 
00136     char jname[L_ID];
00137     memset(jname, 0, L_ID);
00138 
00139     //
00140     fgets_Buffer(&buf, fp);
00141 
00142     for (unsigned int i=0; i<frames_num; i++) {
00143         // 次のフレーム時間行まで移動
00144         while (buf.buf[0]<'0' || buf.buf[0]>'9') fgets_Buffer(&buf, fp);
00145 
00146         int nowms = framesData[i].msec;
00147         if (i==0) prvms = nowms;
00148         int msec = nowms - prvms;
00149         if (msec<0) msec += 60000;  // + 60sec
00150         prvms = nowms;
00151 
00152         if (i==0) framesData[0].msec = 0;
00153         else      framesData[i].msec = framesData[i-1].msec + msec;
00154         framesData[i].frmn = framesData[i].msec;
00155         
00156         //
00157         double px, py, pz;
00158         double qx, qy, qz, qs, th;
00159 
00160         NiJointData* jdat = framesData[i].jdat;
00161 
00162         //
00163         for (int j=0; j<joints_num; j++) {
00164             fgets_Buffer(&buf, fp);
00165             while (buf.buf[0]!=' ') {
00166                 if (buf.buf[0]>='0' || buf.buf[0]<='9') {   // ジョイントデータが途中で終わった?!
00167                     tmline = TRUE;          
00168                     break;
00169                 }
00170                 fgets_Buffer(&buf, fp);                     // コメント行の読み飛ばし
00171             }
00172             if (tmline) {
00173                 tmline = FALSE;
00174                 break;
00175             }
00176 
00177             //
00178             jname[0] = '\0';
00179             px = py = pz = 0.0;
00180             qx = qy = qz = 0.0;
00181             qs = 1.0;
00182             sscanf((char*)buf.buf, "%s %lf %lf %lf %lf %lf %lf %lf %lf", jname, &px, &py, &pz, &qx, &qy, &qz, &qs, &th);
00183 
00184             jdat[j].vect.set(px, py, pz);
00185             jdat[j].quat.set(qs, qx, qy, qz);
00186             jdat[j].angl  = th;
00187             jdat[j].joint = NiJointNum(jname);;
00188             jdat[j].index = i;
00189         }
00190     }
00191 
00192     //
00193     start_time = framesData[0].msec;
00194     stop_time  = framesData[frames_num-1].msec;
00195     exec_time  = stop_time - start_time;
00196 
00197     free_Buffer(&buf);
00198 
00199     return TRUE;
00200 }
00201 
00202 
00203 
00204 void  CNiJTextTool::writeCurrentData(FILE* fp, unsigned short msec)
00205 {
00206     if (fp==NULL) return;
00207     if (posVect==NULL || rotQuat==NULL) return;
00208 
00209     fprintf(fp, "%d\n", msec);
00210 
00211     for (int j=0; j<NI_TOTAL_JOINT_NUM; j++) {
00212         if (posVect[j].c>0 || rotQuat[j].c>0) {
00213             std::string jn = NiJointName(j);
00214             fprintf(fp, " %-10s %11.6f %11.6f %11.6f  ",    jn.c_str(),   posVect[j].x, posVect[j].y, posVect[j].z);
00215             fprintf(fp, "    %11.8f %11.8f %11.8f %11.8f",  rotQuat[j].x, rotQuat[j].y, rotQuat[j].z, rotQuat[j].s);
00216             if (jntAngl!=NULL) fprintf(fp, "    %11.6f",    jntAngl[j]*RAD2DEGREE);
00217             fprintf(fp, "\n");
00218         }
00219     }
00220 
00221     return;
00222 }
00223 
00224 
00225 
00226 void  CNiJTextTool::setPosVect(Vector<double>* pos, NiSDK_Lib lib, BOOL mirror)
00227 {
00228     if (posVect==NULL) posVect = (Vector<double>*)malloc(sizeof(Vector<double>)*NI_TOTAL_JOINT_NUM);
00229     if (posVect==NULL) return;
00230 
00231     //
00232     for (int j=0; j<NI_TOTAL_JOINT_NUM; j++) {
00233         int n = Ni2SDKJointNum(j, lib);
00234         if (mirror && n>=0) n = NiSDKMirrorJointNum(n, lib);
00235 
00236         if (n>=0) {
00237             posVect[j] = pos[n];
00238             if (mirror) posVect[j].y = -posVect[j].y;
00239             //posVect[j].c = 1.0;
00240         }
00241         else {
00242             posVect[j].init(-1.0);
00243         }
00244     }
00245 
00246     return;
00247 }
00248 
00249 
00250 
00251 void  CNiJTextTool::setRotQuat(Quaternion<double>* rot, NiSDK_Lib lib, BOOL mirror)
00252 {
00253     if (rotQuat==NULL) rotQuat = (Quaternion<double>*)malloc(sizeof(Quaternion<double>)*NI_TOTAL_JOINT_NUM);
00254     if (rotQuat==NULL) return;
00255 
00256     //
00257     for (int j=0; j<NI_TOTAL_JOINT_NUM; j++) {
00258         int n = Ni2SDKJointNum(j, lib);
00259         if (mirror && n>=0) n = NiSDKMirrorJointNum(n, lib);
00260 
00261         if (n>=0) {
00262             rotQuat[j] = rot[n];
00263             if (mirror) {
00264                 rotQuat[j].x = -rotQuat[j].x;
00265                 rotQuat[j].z = -rotQuat[j].z;
00266             }
00267             //rotQuat[j].c = 1.0;
00268         }
00269         else {
00270             rotQuat[j].init(-1.0);
00271         }
00272     }
00273 
00274     return;
00275 }
00276 
00277 
00278 
00279 void  CNiJTextTool::setJntAngl(double* agl, NiSDK_Lib lib, BOOL mirror)
00280 {
00281     if (agl==NULL) {
00282         ::freeNull(jntAngl);
00283         return;
00284     }
00285 
00286     if (jntAngl==NULL) jntAngl = (double*)malloc(sizeof(double)*NI_TOTAL_JOINT_NUM);
00287     if (jntAngl==NULL) return;
00288 
00289     //
00290     for (int j=0; j<NI_TOTAL_JOINT_NUM; j++) {
00291         int n = Ni2SDKJointNum(j, lib);
00292         if (mirror && n>=0) n = NiSDKMirrorJointNum(n, lib);
00293 
00294         if (n>=0) {
00295             jntAngl[j] = agl[n];
00296         }
00297         else {
00298             jntAngl[j] = 0.0;
00299         }
00300     }
00301 
00302     return;
00303 }
00304 
00305 
00306 
00307 NiJointData*  CNiJTextTool::getJointsData(int frmnum, int fps) 
00308 { 
00309     if (frmnum<0) return NULL;
00310 
00311     if (fps<=0) fps = 30;
00312     int msec = (int)(1000./fps*frmnum);
00313     if (msec>exec_time) return NULL;
00314 
00315     unsigned int f;
00316     double t = 0.0;
00317 
00318     for (f=1; f<frames_num; f++) {
00319         if (framesData[f].msec >= msec + start_time) {
00320             t = (msec - framesData[f-1].msec)/(double)(framesData[f].msec - framesData[f-1].msec);
00321             break;
00322         }
00323     }
00324     if (f>=frames_num) return NULL;
00325 
00326     //
00327     clearJointsData(joints_num);
00328 
00329     NiJointData* jdat1 = framesData[f-1].jdat;
00330     NiJointData* jdat2 = framesData[f].jdat;
00331 
00332     for (int j=0; j<joints_num; j++) {
00333         if (jdat1[j].joint>=0) {
00334             for (int k=0; k<joints_num; k++) {
00335                 if (jdat1[j].joint==jdat2[k].joint) {
00336                     jointsData[j].joint = jdat1[j].joint;
00337                     jointsData[j].index = frmnum;
00338                     jointsData[j].vect  = BSplineInterp4 (jdat1[j].vect, jdat2[k].vect, t);
00339                     jointsData[j].quat  = SlerpQuaternion(jdat1[j].quat, jdat2[k].quat, t);
00340                 }
00341             }
00342         }
00343     }
00344     
00345     return jointsData;
00346 }
00347