JunkBox_Lib++ (for Windows): ExtendLib/BVHTool.cpp Source File

00001 
00009 #include  "BVHTool.h"
00010 
00011 
00012 using namespace jbxl;
00013 
00014 
00015 
00017 
00018 CBVHTool::~CBVHTool(void)
00019 { 
00020     DEBUG_INFO("DESTRUCTOR: CBVHTool");
00021 
00022     free_data();
00023 }
00024 
00025 
00026 
00027 void  CBVHTool::init()
00028 {
00029     // BVHData
00030     joint_num   = 0;    // 関節の数（End Site を含まない）
00031     frame_num   = 0;    // フレームの数
00032     frame_time  = 0.0;  // フレーム間の時間 s
00033     framepsec   = 0;    // FPS
00034     channels    = 0;    // 総チャンネル数．チャンネル数の合計．モーションデータの数．
00035     state       = 0;    // データの状態
00036     
00037     hierarchy   = NULL; // 関節部のツリーデータへのポインタ．上記参照．
00038     motion      = NULL; // MOTION データへのポインタ joint_num*frame_num
00039     offset      = NULL; // 関節のOFFSET
00040     //
00041     channel_num = NULL;
00042     channel_idx = NULL;
00043     joint_name  = NULL;
00044 
00045     flex_joint  = init_Buffer();
00046 }
00047 
00048 
00049 
00050 void  CBVHTool::free_data(void)
00051 {
00052     state       = 0;
00053     joint_num   = 0;
00054     channels    = 0;
00055     frame_num   = 0;
00056     frame_time  = 0.0;
00057     framepsec   = 0;
00058 
00059     if (hierarchy!=NULL) {
00060         del_tTree(&hierarchy);
00061         hierarchy = NULL;
00062     }
00063     if (motion!=NULL) {
00064         free(motion);
00065         motion = NULL;
00066     }
00067     if (offset!=NULL) {
00068         free(offset);
00069         offset = NULL;
00070     }
00071 
00072     clear_data();
00073 }
00074 
00075 
00076 
00077 void  CBVHTool::clear_data(void)
00078 {
00079     if (joint_name!=NULL) {
00080         for (int j=0; j<joint_num; j++) {
00081             free_Buffer(&joint_name[j]);
00082         }
00083         free(joint_name);
00084     }
00085     joint_name = NULL;
00086 
00087     if (channel_num!=NULL) {
00088         free(channel_num);
00089         channel_num = NULL;
00090     }
00091 
00092     if (channel_idx!=NULL) {
00093         free(channel_idx);
00094         channel_idx = NULL;
00095     }
00096 
00097     free_Buffer(&flex_joint);
00098 }
00099 
00100 
00101 
00102 void  CBVHTool::copy_bvh_data(BVHData* bvh)
00103 {
00104     if (bvh==NULL) return;
00105 
00106     joint_num   = bvh->joint_num;
00107     frame_num   = bvh->frame_num;
00108     frame_time  = bvh->frame_time;
00109     framepsec   = bvh->framepsec;
00110     channels    = bvh->channels;
00111     state       = bvh->state;
00112     
00113     hierarchy   = bvh->hierarchy;
00114     motion      = bvh->motion;
00115     offset      = bvh->offset;
00116 }
00117 
00118 
00119 
00120 BVHData  CBVHTool::setback_bvh_data(void)
00121 {
00122     BVHData bvh;
00123 
00124     set_offset();
00125 
00126     bvh.joint_num   = joint_num;
00127     bvh.frame_num   = frame_num;
00128     bvh.frame_time  = frame_time;
00129     bvh.framepsec   = framepsec;
00130     bvh.channels    = channels;
00131     bvh.state       = state;
00132     
00133     bvh.hierarchy   = hierarchy;
00134     bvh.motion      = motion;
00135     bvh.offset      = offset;
00136 
00137     return bvh;
00138 }
00139 
00140 
00141 
00142 
00144 //
00145 
00146 BOOL  CBVHTool::readFile(char* fname)
00147 {
00148     state = BVH_ERR_INVLD_ARGS;
00149     if (fname==NULL || fname[0]=='\0') return FALSE;
00150 
00151     unsigned long int sz = file_size(fname);
00152     if (sz==0) return FALSE;
00153 
00154     state = BVH_ERR_FAIL_FOPEN;
00155     FILE* fp = fopen(fname, "r");
00156     if (fp==NULL) return FALSE;
00157 
00158     BOOL ret = readBVH(fp);
00159 
00160     return ret;
00161 }
00162     
00163 
00164     
00165 BOOL  CBVHTool::readBVH(FILE* fp)
00166 {
00167     state = BVH_ERR_INVLD_ARGS;
00168     if (fp==NULL) return FALSE;
00169 
00170     state = BVH_ERR_FAIL_OP;
00171     BVHData* ptr = bvh_read_data(fp);
00172     if (ptr==NULL) return FALSE;
00173     if (ptr->state!=0) {
00174         del_BVHData(&ptr);
00175         return FALSE;
00176     }
00177     //
00178     copy_bvh_data(ptr);
00179     get_bvh_params();
00180 
00181     return TRUE;
00182 }
00183 
00184 
00185 
00186 BOOL  CBVHTool::writeFile(char* fname)
00187 {
00188     state = BVH_ERR_INVLD_ARGS;
00189     if (fname==NULL || fname[0]=='\0') return FALSE;
00190 
00191     state = 0;
00192     BVHData bvh_data = setback_bvh_data();
00193     state = bvh_write_file(fname, &bvh_data);
00194     if (state!=0) return FALSE;
00195 
00196     return TRUE;
00197 }
00198 
00199 
00200 
00212 BOOL  CBVHTool::writeMultiFile(char* fname, int sec, int space)
00213 {
00214     state = BVH_ERR_INVLD_ARGS;
00215     if (fname==NULL || fname[0]=='\0') return FALSE;
00216 
00217     int echfrm = (int)(sec/frame_time);         // 1ファイルのフレーム数
00218     int num = (frame_num + echfrm - 1)/echfrm;  // 出力ファイルの数
00219     if (num==0 || num>99) return FALSE;
00220 
00221     //
00222     char* fn = dup_str(fname);
00223     char* et = get_file_extension(fn);
00224     if (et!=NULL) et[-1] = '\0';
00225 
00226     Buffer format = make_Buffer_bystr(fn);
00227     cat_s2Buffer("_%02d", &format);
00228     if (et!=NULL) {
00229         cat_s2Buffer(".", &format);
00230         cat_s2Buffer(et,  &format);
00231     }
00232     ::free(fn);
00233 
00234     //
00235     state = 0;
00236     BVHData bvh_data = setback_bvh_data();
00237 
00238     for (int i=0; i<num; i++) {
00239         //
00240         int outfrm = i*echfrm;                          // 今まで出力した有効フレーム数
00241 
00242         if (i==num-1) {
00243             bvh_data.frame_num = frame_num - outfrm;    // 残りのフレーム数．のりしろは使用しない．
00244         }
00245         else if (i==num-2) {
00246             if ((num-1)*echfrm + space > frame_num) {   // のりしろが大きすぎる
00247                 space = frame_num - (num-1)*echfrm;
00248             }
00249             bvh_data.frame_num = echfrm + space;
00250         }
00251         else {
00252             bvh_data.frame_num = echfrm + space;
00253         }
00254 
00255         if (i!=0 && space>0) {
00256             bvh_data.frame_num++;                               //
00257             bvh_data.motion = &(motion[(outfrm-1)*channels]);   // -1: のりしろ
00258             if (channel_num[0]==6) {                            // ROOT のチャンネル数
00259                 bvh_data.motion[0] = motion[0];
00260                 bvh_data.motion[1] = motion[1];
00261                 bvh_data.motion[2] = motion[2];
00262                 bvh_data.motion[3] = 0.0;
00263                 bvh_data.motion[4] = 0.0;
00264                 bvh_data.motion[5] = 0.0;
00265             }
00266         }
00267         else {
00268             bvh_data.motion = &(motion[outfrm*channels]);
00269         }
00270         
00271         //
00272         char* fn = numbering_name((char*)format.buf, i);
00273         state = bvh_write_file(fn, &bvh_data);
00274         free(fn);
00275         if (state!=0) break;
00276     }
00277     
00278     free_Buffer(&format);
00279     
00280     if (state!=0) return FALSE;
00281     return TRUE;
00282 }
00283 
00284 
00285 
00286 BOOL  CBVHTool::printBVH(FILE* fp)
00287 {
00288     state = BVH_ERR_INVLD_ARGS;
00289     if (fp==NULL) return FALSE;
00290 
00291     state = 0;
00292     BVHData bvh_data = setback_bvh_data();
00293     state = bvh_print_data(fp, &bvh_data);
00294     if (state!=0) return FALSE;
00295 
00296     return TRUE;
00297 }
00298 
00299 
00300 
00301 void  CBVHTool::setHierarchy(tTree* hrchy)
00302 {
00303     free_data();
00304 
00305     hierarchy = hrchy;
00306     joint_num = bvh_count_joints(hierarchy);
00307     channels  = bvh_count_channels(hierarchy);
00308 
00309     get_bvh_params();
00310 }
00311 
00312 
00313 
00314 void  CBVHTool::set_offset(void)
00315 {
00316     if (offset==NULL) return;
00317 
00318     int jnum = 0;
00319     _set_offset(hierarchy, &jnum);
00320 }
00321 
00322 
00323 
00324 void  CBVHTool::_set_offset(tTree* tree, int* jnum)
00325 {
00326    if (tree!=NULL) {
00327         while(tree->esis!=NULL) tree = tree->esis;
00328   
00329         do {
00330             tList_data* ld = &(tree->ldat);
00331 
00333             if (ld->ptr==NULL) {
00334                 ld->sz  = sizeof(vector);
00335                 ld->ptr = (void*)malloc(ld->sz);
00336             }
00337             *((vector*)ld->ptr) = offset[*jnum];
00338             (*jnum)++;
00340 
00341             if (tree->next!=NULL) _set_offset(tree->next, jnum);
00342             tree = tree->ysis;
00343             //
00344         } while(tree!=NULL);
00345     }
00346 
00347     return;
00348 
00349 
00350 }
00351 
00352 
00353 
00354 
00356 //
00357 
00364 void  CBVHTool::get_bvh_params()
00365 {
00366     size_t  len = sizeof(int)*joint_num;
00367     channel_num = (int*)malloc(len);
00368     channel_idx = (int*)malloc(len);
00369     //
00370     if (channel_num==NULL || channel_idx==NULL) {
00371         if (channel_num!=NULL) free(channel_num);
00372         if (channel_idx!=NULL) free(channel_idx);
00373         channel_num = NULL;
00374         channel_idx = NULL;
00375         return;
00376     }
00377     memset(channel_num, 0, sizeof((int)len));
00378     memset(channel_idx, 0, sizeof((int)len));
00379 
00380     //
00381     flex_joint = make_Buffer(joint_num*6);
00382     joint_name = (Buffer*)malloc(sizeof(Buffer)*joint_num);
00383 
00384     int* ch = channel_num;
00385     Buffer* jn = joint_name;
00386 
00387     _get_bvh_params(hierarchy, &ch, &jn);
00388 
00389     //
00390     channel_idx[0] = 0;
00391     for (int j=1; j<joint_num; j++) {
00392         channel_idx[j] = channel_idx[j-1] + channel_num[j-1];   
00393     }
00394 
00395 }
00396 
00397 
00398 
00399 void  CBVHTool::_get_bvh_params(tTree* tree, int** ch, Buffer** jn)
00400 {
00401     if (tree!=NULL) {
00402         while(tree->esis!=NULL) tree = tree->esis;
00403   
00404         do {
00405             tList_data ld = tree->ldat;
00406 
00408             if ((*ch)!=NULL) {
00409                 **ch = ld.lv;
00410                 (*ch)++;
00411             }
00412             cat_Buffer(&ld.val, &flex_joint);
00413 
00414             **jn = dup_Buffer(ld.key);
00415             (*jn)++;
00417 
00418 
00419             if (tree->next!=NULL) {
00420                 _get_bvh_params(tree->next, ch, jn);
00421             }
00422             tree = tree->ysis;
00423             //
00424         } while(tree!=NULL);
00425     }
00426 
00427     return;
00428 }
00429 
00430 
00431 
00432 
00434 //
00435 
00436 Vector<double>*  CBVHTool::getPosData(int frame)
00437 {
00438     if (frame<0 || frame>=frame_num) return NULL;
00439 
00440     size_t len = sizeof(Vector<double>)*joint_num;
00441     Vector<double>* vect = (Vector<double>*)malloc(len);
00442     if (vect==NULL) return NULL;
00443     memset(vect, 0, len);
00444 
00445     int frm = frame*channels;
00446 
00447     for (int j=0; j<joint_num; j++) {
00448         //
00449         BOOL  exstf = FALSE;
00450         int   chnum = channel_num[j];
00451         int   index = channel_idx[j];
00452         char* tpchr = (char*)&flex_joint.buf[index*2];
00453 
00454         for (int i=0; i<chnum; i++) {
00455             if (tpchr[i*2]=='P') {
00456                 exstf = TRUE;
00457                 index = frm + index + i;
00458                 tpchr+= i*2;
00459                 break;
00460             }
00461         }
00462 
00463         //
00464         vect[j].set(0.0, 0.0, 0.0);
00465 
00466         if (exstf) {
00467             for (int k=0; k<3; k++) {
00468                 int k2 = k*2;
00469                 if (tpchr[k2]=='P') {
00470                     if      (tpchr[k2+1]=='X') vect[j].x = motion[index+k];
00471                     else if (tpchr[k2+1]=='Y') vect[j].y = motion[index+k];
00472                     else if (tpchr[k2+1]=='Z') vect[j].z = motion[index+k];
00473                 }
00474             }
00475         }
00476     }
00477 
00478     return vect;
00479 }
00480 
00481 
00482 
00483 Quaternion<double>*  CBVHTool::getQuaternion(int frame)
00484 {
00485     if (frame<0 || frame>=frame_num) return NULL;
00486 
00487     size_t len = sizeof(Quaternion<double>)*joint_num;
00488     Quaternion<double>* quat = (Quaternion<double>*)malloc(len);
00489     if (quat==NULL) return NULL;
00490 
00491     Vector<double> ex(1.0, 0.0, 0.0, 1.0);  
00492     Vector<double> ey(0.0, 1.0, 0.0, 1.0);  
00493     Vector<double> ez(0.0, 0.0, 1.0, 1.0);
00494     
00495     double d2r = PI/180.0;
00496     int frm = frame*channels;
00497 
00498     //
00499     for (int j=0; j<joint_num; j++) {
00500         //
00501         BOOL  exstf = FALSE;
00502         int   chnum = channel_num[j];
00503         int   index = channel_idx[j];
00504         char* tpchr = (char*)&flex_joint.buf[index*2];
00505 
00506         quat[j].set(1.0, 0.0, 0.0, 0.0, 1.0);
00507         //
00508         int chfwd = 0;
00509         for (int i=0; i<chnum; i++) {
00510             if (tpchr[i*2]=='R') {
00511                 exstf = TRUE;               // 回転用データの存在
00512                 index = frm + index + i;    // MOTION データの位置
00513                 tpchr+= i*2;                // チャンネルインデックスの位置
00514                 chfwd = i;                  // 処理済チャンネル数
00515                 break;
00516             }
00517         }
00518 
00519         //
00520         if (exstf) {
00521             Quaternion<double> q1(1.0, 0.0, 0.0, 0.0, 1.0);
00522             Quaternion<double> q2(1.0, 0.0, 0.0, 0.0, 1.0);
00523             Quaternion<double> q3(1.0, 0.0, 0.0, 0.0, 1.0);
00524             
00525             // X->Y, Y->Z, Z->X
00526             double th = motion[index]*d2r;
00527             if      (tpchr[1]=='X') q3.setRotation(th, ey); 
00528             else if (tpchr[1]=='Y') q3.setRotation(th, ez);
00529             else if (tpchr[1]=='Z') q3.setRotation(th, ex);
00530 
00531             chnum = chnum - chfwd - 1;  // 残りの回転用チャンネル数
00532             if (chnum>0 && tpchr[2]=='R') {
00533                 th = motion[index+1]*d2r;
00534                 if      (tpchr[3]=='X') q2.setRotation(th, ey);
00535                 else if (tpchr[3]=='Y') q2.setRotation(th, ez);
00536                 else if (tpchr[3]=='Z') q2.setRotation(th, ex);
00537 
00538                 chnum--;
00539                 if (chnum>0 && tpchr[4]=='R') {
00540                     th = motion[index+2]*d2r;
00541                     if      (tpchr[5]=='X') q1.setRotation(th, ey);
00542                     else if (tpchr[5]=='Y') q1.setRotation(th, ez);
00543                     else if (tpchr[5]=='Z') q1.setRotation(th, ex);
00544                 }
00545             }
00546 
00547             quat[j] = q3*q2*q1;
00548         }
00549 
00550     }
00551     return quat;
00552 }
00553 
00554 
00555 
00556 Vector<double>*  CBVHTool::getPosOffset(void)
00557 {
00558     size_t len = sizeof(Vector<double>)*joint_num;
00559     Vector<double>* vect = (Vector<double>*)malloc(len);
00560 
00561     if (vect==NULL) return NULL;
00562     memset(vect, 0, len);
00563 
00564     for (int j=0; j<joint_num; j++) {
00565         vector vt = offset[j];
00566         vect[j].x = vt.x;
00567         vect[j].y = vt.y;
00568         vect[j].z = vt.z;
00569         vect[j].n = vt.n;
00570     }
00571 
00572     return vect;
00573 }
00574 
00575 
00576 
00577 
00578 
00580 // Pending
00581 
00582 /*
00583 Matrix<double>*  CBVHTool::getRotMatrix(int frame)
00584 {
00585     if (frame<0 || frame>=frame_num) return NULL;
00586 
00587     size_t len = sizeof(Matrix<double>)*joint_num;
00588     Matrix<double>* mtrx = (Matrix<double>*)malloc(len);
00589     if (mtrx==NULL) return NULL;
00590 
00591     double d2r = PI/180.0;
00592     int frm = frame*bvh_data->channels;
00593 
00594     for (int j=0; j<joint_num; j++) {
00595         //
00596         BOOL  exstf = FALSE;
00597         int   chnum = channel_num[j];
00598         int   index = channel_idx[j];
00599         char* tpchr = (char*)&flex_joint.buf[index*2];
00600 
00601         for (int i=0; i<chnum; i++) {
00602             if (tpchr[i*2]=='R') {
00603                 exstf = TRUE;
00604                 index = frm + index + i;
00605                 tpchr+= i*2;
00606                 break;
00607             }
00608         }
00609         if (exstf) {
00610             if (tpchr[2]!='R' || tpchr[4]!='R') {
00611                 exstf = FALSE;  // not supported
00612             }
00613         }
00614 
00615         //
00616         if (exstf) {
00617             //
00618             Vector<double> e;
00619 
00620             if (tpchr[1]=='X' && tpchr[3]=='Y' && tpchr[5]=='Z') {
00621                 // ZYX -> XZY
00622                 PRINT_MESG("WARNING: CBVHTool::getRotMatrix: Not Implemeted Function is called. XYZ\n");
00623                 mtrx[j].init(2, 3, 3);
00624             }
00625 
00626             else if (tpchr[1]=='X' && tpchr[3]=='Z' && tpchr[5]=='Y') {
00627                 // YZX -> ZXY
00628                 PRINT_MESG("WARNING: CBVHTool::getRotMatrix: Not Implemeted Function is called. XZY\n");
00629                 mtrx[j].init(2, 3, 3);
00630             }
00631 
00632             else if (tpchr[1]=='Y' && tpchr[3]=='X' && tpchr[5]=='Z') {
00633                 // ZXY -> XYZ
00634                 e.z = bvh_data->motion[index]  *d2r;
00635                 e.y = bvh_data->motion[index+1]*d2r;
00636                 e.x = bvh_data->motion[index+2]*d2r;
00637                 mtrx[j] = EulerXYZ2RotMatrix(e);
00638             }
00639 
00640             else if (tpchr[1]=='Y' && tpchr[3]=='Z' && tpchr[5]=='X') {
00641                 // XZY -> YXZ 
00642                 PRINT_MESG("WARNING: CBVHTool::getRotMatrix: Not Implemeted Function is called. YZX\n");
00643                 mtrx[j].init(2, 3, 3);
00644             }
00645 
00646             else if (tpchr[1]=='Z' && tpchr[3]=='X' && tpchr[5]=='Y') {
00647                 // YXZ -> ZYX 
00648                 e.x = bvh_data->motion[index]  *d2r;
00649                 e.y = bvh_data->motion[index+1]*d2r;
00650                 e.z = bvh_data->motion[index+2]*d2r;
00651                 mtrx[j] = EulerZYX2RotMatrix(e);
00652             }
00653 
00654             else if (tpchr[1]=='Z' && tpchr[3]=='Y' && tpchr[5]=='X') {
00655                 // XYZ -> YZX
00656                 PRINT_MESG("WARNING: CBVHTool::getRotMatrix: Not Implemeted Function is called. ZYX\n");
00657                 mtrx[j].init(2, 3, 3);
00658             }
00659 
00660             else {
00661                 mtrx[j].init(2, 3, 3);      // 3x3行列
00662             }
00663         }
00664         else {
00665             mtrx[j].init(2, 3, 3);          // 3x3行列
00666         }
00667     }
00668 
00669     return mtrx;
00670 }
00671 
00672 */
00673 
00674