22#include "LinearAlgebra.h"
26#define NULL ((void *)0)
29#define strcasecmp stricmp
31#define RADIANS_PER_DEGREE (double)0.0174532925199432957692
32#define DEGREES_PER_RADIAN (double)57.2957795130823208768
44static struct chan_name {
48{CHAN_RX,
"Xrotation"},
49{CHAN_RY,
"Yrotation"},
50{CHAN_RZ,
"Zrotation"},
51{CHAN_TX,
"Xposition"},
52{CHAN_TY,
"Yposition"},
53{CHAN_TZ,
"Zposition"},
56static int chan_lookup(
char *cname){
63 if(!strcmp(cn->cname,cname)){
68 }
while(cn->cname != NULL);
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);
80 *position = &cur[len+1];
92char* get_jname(
char* mocap_name);
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) {
105 if (!strcmp(name, list[i])) {
113void read_bvh_blob(
char* blob,
int ignorePosition,
int yUp,
int teePose,
114 int flipZ,
float armAngle,
float legAngle,
float scale,
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,
120 float* bvh_frame_time,
int* bvh_frame_count)
127 char line [4096], *pos;
128 char *token, *delims;
129 float global_scale = 1.0f;
132 rv = getline2(line,2048,&pos);
136 delims =
" ,\r\n\t\"";
137 token = strtok(line,delims);
139 if( strcasecmp(token,
"hierarchy")){
140 printf(
"not a BVH file \n");
144 *bvh_frame_count = 0;
145 *bvh_frame_time = 0.0;
147 int channelIndex = -1;
154 while( getline2(line,2048,&pos)){
156 token = strtok(line,delims);
158 if(!strcasecmp(token,
"root") || !strcasecmp(token,
"joint")){
163 memset(nametokens,0,4*
sizeof(
void*));
164 while(nametokens[len] = strtok(NULL,delims)) len++;
167 strcpy(name,nametokens[0]);
168 printf(
"%d name=%s ",mjoint, name);
170 for(
int i=1;i<len-1;i++) {
172 strcat(name,nametokens[i]);
174 cj = &cjoint[mjoint];
176 cj->mocap_name = strdup(name);
178 if(!strcasecmp(token,
"OFFSET")){
182 for(
int i=0;i<3;i++){
183 token = strtok(NULL,delims);
184 sscanf(token,
"%f",&offset[i]);
189 if(!strcasecmp(token,
"CHANNELS")){
191 token = strtok(NULL,delims);
192 sscanf(token,
"%d",&channels);
193 *channel_count += channels;
194 printf(
" channels %d totalchannels %d\n",channels,*channel_count );
195 cj->nchan = channels;
199 for(
int i=0;i<channels;i++){
200 char *channel = strtok(NULL,delims);
201 int ichan = chan_lookup(channel);
202 cj->ichan[i] = ichan;
206 if(!strcasecmp(token,
"end")){
208 if(!strcmp(token,
"}")){
211 if(!strcmp(token,
"{")){
214 if(!strcasecmp(token,
"motion") ){
225 getline2(line,2048,&pos);
226 token = strtok(line,delims);
227 if(!strcasecmp(token,
"frames:")){
228 token = strtok(NULL,delims);
229 sscanf(token,
"%d",bvh_frame_count);
232 getline2(line,2048,&pos);
233 token = strtok(line,delims);
234 if(!strcasecmp(token,
"frame")){
235 token = strtok(NULL,delims);
236 if(!strcasecmp(token,
"time:")){
237 token = strtok(NULL,delims);
238 sscanf(token,
"%f",bvh_frame_time);
242 printf(
"njoint %d \n",*njoint);
243 printf(
"nchannel %d\n",*channel_count);
248 float * fvalues = malloc( (*channel_count) * (*bvh_frame_count) *
sizeof(
float));
254 for(
int iframe=0;iframe<*bvh_frame_count;iframe++){
255 for(
int i=0;i<*channel_count;i++){
256 token = strtok(str,delims);
258 sscanf(token,
"%f",&fvalues[k]);
265 float* fv0 = &fvalues[0];
268 for(
int iframe=0;iframe< *bvh_frame_count;iframe++){
269 float *fv = &fvalues[iframe * (*channel_count)];
271 for(
int j=0;j<mjoint;j++){
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;
277 axis_swap_left = instringlist(jname, swaplistleft);
278 axis_swap_right = instringlist(jname, swaplistright);
279 if (axis_swap_left || axis_swap_right) {
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;
285 if (axis_swap_left) {
286 float tmp = fv[kchan + ix];
287 fv[kchan + ix] = fv[kchan + iy];
288 fv[kchan + iy] = -tmp;
290 if (axis_swap_right) {
291 float tmp = fv[kchan + ix];
292 fv[kchan + ix] = -fv[kchan + iy];
293 fv[kchan + iy] = tmp;
298 for(
int k=0;k<cchan[j].nchan;k++){
300 if (cchan[j].ichan[k] < 4) {
302 if (cchan[j].ichan[k] == ifore)
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;
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;
317 fv[kchan] *= RADIANS_PER_DEGREE;
319 if (cchan[j].ichan[k] > 3) {
321 if (cchan[j].ichan[k] == 6 && flipZ)
322 fv[kchan] = -fv[kchan];
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}},
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}},
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}},
365static char *ignore =
"IGNORE";
366char * jname_lookup(
char *mocap_name){
368 char *jname = ignore;
373 nm = &loa1_mapping[i];
376 while(nm_mocap_name = nm->mocap_name[j]){
377 if(!strcasecmp(nm_mocap_name,mocap_name)){
384 if(iname > -1)
break;
388 jname = loa1_mapping[iname].jname;
396static char** mapp = NULL;
398void bvh_set_mapping(
char** mapping,
int n) {
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)) {
411char* get_jname(
char* mocap_name) {
414 jname = jname_mapping(mocap_name);
417 jname = jname_lookup(mocap_name);
424 printf(
"=====BEFORE MAPPING====\n");
425 for(
int i=0;i<mjoint;i++){
427 printf(
"%d %s\n",i,chan[i].mocap_name);
429 printf(
"=========\n");
431 if(loa == 1 || loa == -1){
432 for(
int i=0;i<mjoint;i++){
433 chan[i].jname = get_jname(chan[i].mocap_name);
443 printf(
"====AFTER MAPPING=====\n");
444 for(
int i=0;i<mjoint;i++){
446 printf(
"%d %s\n",i,chan[i].jname);
448 printf(
"=========\n");