FreeWRL / FreeX3D 4.3.0
BVHreader.c
1
2
3// license: MIT or similar permissive
4/*
5BVH includes some skeleton information X3D HAnim doesn't need / can't use:
6x length of bones aka OFFSET
7* HAnim supplies its own skeleton
8x parenting heirarchy to help accumulate global transform for a limb
9* HAnim - we do transforms at each joint, so we need only local joint angles
10
11What X3D needs is HAnim2MotionData.
12
13*/
14
15
16#define ASSERT
17
18#include <config.h>
19#include <string.h>
20#include <malloc.h>
21#include <stdlib.h>
22#include "LinearAlgebra.h"
23#include <stdio.h>
24#define TRUE 1
25#define FALSE 0
26#define NULL ((void *)0)
27
28#ifdef _MSC_VER
29#define strcasecmp stricmp
30#endif //_MSC_VER
31#define RADIANS_PER_DEGREE (double)0.0174532925199432957692
32#define DEGREES_PER_RADIAN (double)57.2957795130823208768
33
34
35enum {
36CHAN_RX = 1,
37CHAN_RY = 2,
38CHAN_RZ = 3,
39CHAN_TX = 4,
40CHAN_TY = 5,
41CHAN_TZ = 6,
42CHAN_NONE = 0,
43};
44static struct chan_name {
45int iname;
46char *cname;
47} chan_names [] = {
48{CHAN_RX, "Xrotation"},
49{CHAN_RY, "Yrotation"},
50{CHAN_RZ, "Zrotation"},
51{CHAN_TX, "Xposition"},
52{CHAN_TY, "Yposition"},
53{CHAN_TZ, "Zposition"},
54{CHAN_NONE,NULL},
55};
56static int chan_lookup(char *cname){
57 int i, iname;
58 struct chan_name *cn;
59 i = 0;
60 iname = 0;
61 do{
62 cn = &chan_names[i];
63 if(!strcmp(cn->cname,cname)){
64 iname = cn->iname;
65 break;
66 }
67 i++;
68 }while(cn->cname != NULL);
69 return iname;
70
71}
72
73char * getline2(char *line, int maxlen, char **position){
74 char *cur = *position;
75 char *end = strstr(cur,"\n");
76 if(end == NULL) return NULL;
77 int len = (end-cur) < (maxlen-1) ? (end-cur) : (maxlen-1);
78 memcpy(line,cur,len);
79 line[len] = '\0';
80 *position = &cur[len+1];
81 return *position;
82}
83
85 char *jname;
86 char *mocap_name;
87 int nchan;
88 int ichan[6];
89 int level;
90 float *values;
91};
92char* get_jname(char* mocap_name);
93// T-pose vs HAnim H-pose: designers assume different axes for arms, forarms, hands, fingers etc
94// when we rotate by -90 for left shoulder, and +90 for right shoulder,
95// we can swap x and y axis and change sign on one of them
96// I think the joint axis-swap list needs to include finger joints for LOA3
97// I think the joint axis-swap list needs to include finger joints for LOA3
98static char* swaplistleft[] = { "l_shoulder" ,"l_elbow", "l_wrist", NULL, };
99static char* swaplistright[] = { "r_shoulder", "r_elbow", "r_wrist", NULL, };
100static char* rootnodelist[] = { "" };
101static int instringlist(char* name, char** list) {
102 int have = FALSE;
103 int i = 0;
104 while (list[i]) {
105 if (!strcmp(name, list[i])) {
106 have = TRUE; break;
107 }
108 i++;
109 }
110 return have;
111}
112
113void read_bvh_blob(char* blob, int ignorePosition, int yUp, int teePose,
114 int flipZ, float armAngle, float legAngle, float scale,
115 struct joint_frame_motion** chan, int* njoint, int* channel_count, float** values,
116 float* bvh_frame_time, int* bvh_frame_count);
117void read_bvh_blob(char* blob, int ignorePosition, int yUp, int teePose,
118 int flipZ, float armAngle, float legAngle, float scale,
119 struct joint_frame_motion** chan, int* njoint, int* channel_count, float** values,
120 float* bvh_frame_time, int* bvh_frame_count)
121{
122 // File loading stuff
123 // Open the file for importing
124
125 // Seperate into a list of lists, each line a list of words.
126 char *rv;
127 char line [4096], *pos;
128 char *token, *delims;
129 float global_scale = 1.0f;
130
131 pos = blob;
132 rv = getline2(line,2048,&pos);
133
134
135 // Split by whitespace.
136 delims = " ,\r\n\t\"";
137 token = strtok(line,delims);
138 // Create hierarchy as empties
139 if( strcasecmp(token,"hierarchy")){
140 printf("not a BVH file \n");
141 return;
142 }
143
144 *bvh_frame_count = 0;
145 *bvh_frame_time = 0.0;
146
147 int channelIndex = -1;
148 *channel_count = 0;
149 struct joint_frame_motion *cjoint, *cj, ccjoints[100];
150 cjoint = ccjoints; //malloc(100 * sizeof(struct joint_frame_motion));
151 memset(cjoint,0,100*sizeof(struct joint_frame_motion));
152 int mjoint = 0;
153 int level = 0;
154 while( getline2(line,2048,&pos)){
155 //...
156 token = strtok(line,delims);
157 //printf("token %s\n",token);
158 if(!strcasecmp(token,"root") || !strcasecmp(token,"joint")){
159 // JOINT name, start new joint
160 char *nametokens[4];
161 char name[100];
162 int len=0;
163 memset(nametokens,0,4*sizeof(void*));
164 while(nametokens[len] = strtok(NULL,delims)) len++;
165 //printf("len %d\n",len);
166 // Join spaces into 1 word with underscores joining it.
167 strcpy(name,nametokens[0]);
168 printf("%d name=%s ",mjoint, name);
169 // Make sure the names are unique - Object names will match joint names exactly and both will be unique.
170 for(int i=1;i<len-1;i++) {
171 strcat(name,"_");
172 strcat(name,nametokens[i]);
173 }
174 cj = &cjoint[mjoint];
175 mjoint++;
176 cj->mocap_name = strdup(name);
177 }
178 if(!strcasecmp(token,"OFFSET")){
179 if(0){
180 //get offset numbers
181 float offset[3];
182 for(int i=0;i<3;i++){
183 token = strtok(NULL,delims);
184 sscanf(token,"%f",&offset[i]);
185 offset[i] *= scale;
186 }
187 }
188 }
189 if(!strcasecmp(token,"CHANNELS")){
190 int channels;
191 token = strtok(NULL,delims); //CHANNELS
192 sscanf(token,"%d",&channels);
193 *channel_count += channels;
194 printf(" channels %d totalchannels %d\n",channels,*channel_count );
195 cj->nchan = channels;
196 cj->level = level;
197
198 //for channel in file_lines[lineIdx][2:]:
199 for(int i=0;i<channels;i++){
200 char *channel = strtok(NULL,delims); //Zrotation
201 int ichan = chan_lookup(channel);
202 cj->ichan[i] = ichan;
203
204 }
205 }
206 if(!strcasecmp(token,"end")){
207 }
208 if(!strcmp(token,"}")){
209 level--;
210 }
211 if(!strcmp(token,"{")){
212 level++;
213 }
214 if(!strcasecmp(token,"motion") ){ //MOTION
215 // End of joint hierarchy.
216 // MOTION
217 break; //get out of hierarchy loop
218 }
219 } //end while lines
220
221 *njoint = mjoint;
222 // start of motion channel float values, starting with:
223 // Frames: n
224 // Frame Time: dt
225 getline2(line,2048,&pos); //Frames: 2752
226 token = strtok(line,delims); //frames:
227 if(!strcasecmp(token,"frames:")){
228 token = strtok(NULL,delims); //2752
229 sscanf(token,"%d",bvh_frame_count);
230 }
231
232 getline2(line,2048,&pos); //Frame Time: 0.00833333
233 token = strtok(line,delims); //frame
234 if(!strcasecmp(token,"frame")){
235 token = strtok(NULL,delims); //time
236 if(!strcasecmp(token,"time:")){
237 token = strtok(NULL,delims); //0.00833333
238 sscanf(token,"%f",bvh_frame_time);
239 }
240 }
241
242 printf("njoint %d \n",*njoint);
243 printf("nchannel %d\n",*channel_count);
244 struct joint_frame_motion *cchan = malloc(*njoint *sizeof(struct joint_frame_motion));
245 *chan = cchan;
246 memcpy(cchan,cjoint,mjoint * sizeof(struct joint_frame_motion));
247
248 float * fvalues = malloc( (*channel_count) * (*bvh_frame_count) * sizeof(float));
249 *values = fvalues;
250 int k = 0;
251 //char *delims2 = " ,\t\r\n";
252 char *str = pos;
253 //FILE * fout = fopen("single_row.bvh","w+");
254 for(int iframe=0;iframe<*bvh_frame_count;iframe++){
255 for(int i=0;i<*channel_count;i++){
256 token = strtok(str,delims);
257 str = NULL; //so next strtok(NULL,...)
258 sscanf(token,"%f",&fvalues[k]);
259 //fprintf(fout,"%f ",fvalues[k]);
260 k++;
261 }
262 //fprintf(fout,"\n");
263 }
264 //fclose(fout);
265 float* fv0 = &fvalues[0];
266
267 //convert degrees to radians
268 for(int iframe=0;iframe< *bvh_frame_count;iframe++){
269 float *fv = &fvalues[iframe * (*channel_count)];
270 int kchan = 0;
271 for(int j=0;j<mjoint;j++){
272 //printf("%s %d \n",vector_get(char*,jnames,j),chan[j].nchan);
273 int axis_swap_left, axis_swap_right;
274 char* jname = get_jname(cchan[j].mocap_name);
275 axis_swap_left = axis_swap_right = 0;
276 if(1) if (teePose) {
277 axis_swap_left = instringlist(jname, swaplistleft);
278 axis_swap_right = instringlist(jname, swaplistright);
279 if (axis_swap_left || axis_swap_right) {
280 int ix, iy;
281 for (int k = 0; k < cchan[j].nchan; k++) {
282 if (cchan[j].ichan[k] == 1) ix = k;
283 if (cchan[j].ichan[k] == 2) iy = k;
284 }
285 if (axis_swap_left) {
286 float tmp = fv[kchan + ix];
287 fv[kchan + ix] = fv[kchan + iy];
288 fv[kchan + iy] = -tmp;
289 }
290 if (axis_swap_right) {
291 float tmp = fv[kchan + ix];
292 fv[kchan + ix] = -fv[kchan + iy];
293 fv[kchan + iy] = tmp;
294 }
295 }
296 }
297
298 for(int k=0;k<cchan[j].nchan;k++){
299 int ifore = 3; // yUp ? 3 : 2;
300 if (cchan[j].ichan[k] < 4) {
301 if (flipZ) {
302 if (cchan[j].ichan[k] == ifore)
303 fv[kchan] *= -1;
304 }
305 if (legAngle != 0.0f) {
306 if (cchan[j].ichan[k] == ifore && !strcmp(jname, "l_hip"))
307 fv[kchan] += legAngle;
308 else if (cchan[j].ichan[k] == ifore && !strcmp(jname, "r_hip"))
309 fv[kchan] -= legAngle;
310 }
311 if(armAngle != 0.0f){
312 if (cchan[j].ichan[k] == ifore && !strcmp(jname, "l_shoulder"))
313 fv[kchan] += armAngle;
314 else if (cchan[j].ichan[k] == ifore && !strcmp(jname, "r_shoulder"))
315 fv[kchan] -= armAngle;
316 }
317 fv[kchan] *= RADIANS_PER_DEGREE; //PI / 180.0; //
318 }
319 if (cchan[j].ichan[k] > 3) {
320 fv[kchan] *= scale;
321 if (cchan[j].ichan[k] == 6 && flipZ)
322 fv[kchan] = -fv[kchan];
323 if (ignorePosition)
324 fv[kchan] = 0.0f;
325 }
326 //if(!strcmp(get_jname(cchan[j].mocap_name),"l_shoulder"))
327 // printf("%d %5.2f ",cchan[j].ichan[k],cchan[j].ichan[k] < 4 ? fv[kchan]*DEGREES_PER_RADIAN : fv[kchan]);
328 kchan++;
329 }
330 //printf("\n");
331 }
332 }
333
334}
335
336
337// Mapping no LOA-1 HAnim joints (18 joints) motion-capture joints example (18 different joints)
338struct name_map {
339int no;
340char *jname;
341char *mocap_name[6];
342} loa1_mapping [] = {
343{1,"humanoid_root",{"Hips","hip","joint_root",0,0}},
344{2,"sacroiliac",{"Spine",0,0,0,0,0}},
345{3,"l_hip",{"LeftHip","lThigh","UpperLeg_L","LeftUpLeg",0,0}},
346{4,"l_knee",{"LeftKnee","lShin","LowerLeg_L","LeftLeg",0,0}},
347{5,"l_talocrural",{"LeftAnkle","lFoot","Foot_L","LeftFoot",0,0}},
348{6,"l_metatarsophalangeal",{"Toes_L","LeftToeBase",0,0,0,0}},
349{7,"r_hip",{"RightHip","rThigh","UpperLeg_R","RightUpLeg",0,0}},
350{8,"r_knee",{"RightKnee","rShin","LowerLeg_R","RightLeg",0,0}},
351{9,"r_talocrural",{"RightAnkle","rFoot","Foot_R","RightFoot",0,0}},
352{10,"r_metatarsophalangeal",{"Toes_R","RightToeBase",0,0,0,0}},
353{11,"vl5",{"Chest","abdomen","Spine1",0,0,0}},
354{12,"skullbase",{"Neck","Head",0,0,0,0}},
355//{13,"l_shoulder",{"LeftCollar","lCollar",0,0,0,0}},
356{13,"l_shoulder",{"LeftShoulder","lShldr","UpperArm_L","LeftArm",0,0}},
357{14,"l_elbow",{"LeftElbow","lForeArm","LowerArm_L","LeftForeArm",0,0}},
358{15,"l_radiocarpal",{"LeftWrist","lHand","Hand_L","LeftHand",0,0}},
359//{16,"r_shoulder",{"RightCollar","rCollar",0,0,0,0}},
360{16,"r_shoulder",{"RightShoulder","rShldr","UpperArm_R","RightArm",0,0}},
361{17,"r_elbow",{"RightElbow","rForeArm","LowerArm_R","RightForeArm",0,0}},
362{18,"r_radiocarpal",{"RightWrist","rHand","Hand_R","RightHand",0,0}},
363{0,NULL,{0,0,0,0,0,0}},
364};
365static char *ignore = "IGNORE";
366char * jname_lookup(char *mocap_name){
367 int i, iname;
368 char *jname = ignore;
369 struct name_map *nm;
370 i = 0;
371 iname = -1;
372 do{
373 nm = &loa1_mapping[i];
374 int j=0;
375 char *nm_mocap_name;
376 while(nm_mocap_name = nm->mocap_name[j]){
377 if(!strcasecmp(nm_mocap_name,mocap_name)){
378 //great built-in mapping!
379 iname = i;
380 break;
381 }
382 j++;
383 }
384 if(iname > -1) break;
385 i++;
386 }while(nm->no > 0);
387 if(iname > -1){
388 jname = loa1_mapping[iname].jname;
389 }
390 return jname;
391}
392
393//every bvh publisher and bvh seems to have different naming convention
394//if you worked out a mapping from bvh joint name to LOA1 joint name
395// set it here and we'll use it
396static char** mapp = NULL;
397static int nmap = 0;
398void bvh_set_mapping(char** mapping, int n) {
399 mapp = mapping;
400 nmap = n;
401}
402char* jname_mapping(char* mocap_name) {
403 char* jname = ignore;
404 for (int i = 0; i < nmap; i++) {
405 if (!strcasecmp(mapp[i * 2 + 1], mocap_name)) {
406 jname = mapp[i * 2];
407 }
408 }
409 return jname;
410}
411char* get_jname(char* mocap_name) {
412 char* jname = NULL;
413 if (mapp) {
414 jname = jname_mapping(mocap_name);
415 }
416 else {
417 jname = jname_lookup(mocap_name);
418 }
419 return jname;
420}
421void map_mocap_to_hanim_loa( struct joint_frame_motion *chan, int mjoint, int loa){
422
423 //map mocap joint names to HAnim2 loa joint names - see section 4.4.4 Joint mapping example
424 printf("=====BEFORE MAPPING====\n");
425 for(int i=0;i<mjoint;i++){
426 //printf("%d %s\n",i,cjoint[i].mocap_name);
427 printf("%d %s\n",i,chan[i].mocap_name);
428 }
429 printf("=========\n");
430
431 if(loa == 1 || loa == -1){
432 for(int i=0;i<mjoint;i++){
433 chan[i].jname = get_jname(chan[i].mocap_name);
434 //if (mapp) {
435 // chan[i].jname = jname_mapping(chan[i].mocap_name);
436 //}
437 //else {
438 // chan[i].jname = jname_lookup(chan[i].mocap_name);
439 //}
440 }
441 }
442
443 printf("====AFTER MAPPING=====\n");
444 for(int i=0;i<mjoint;i++){
445 //printf("%d %s\n",i,cjoint[i].mocap_name);
446 printf("%d %s\n",i,chan[i].jname);
447 }
448 printf("=========\n");
449}
450