prj/tech/libsrc/mp/multiped.c

   1 #include "multiped.h"
   2 #include "motstruc.h"
   3 #include <stdio.h>
   4 #include <math.h>
   5 #include <lg.h>
   6 #include <schedstr.h>
   7
   8 //#include <appbiped.h>
   9 #include <motion.h>
  10
  11 #define QSCALE  0.5
  12
  13 //
  14
  15 extern char * Jnt[];
  16
  17 //
  18
  19 mpm_motion *mpd_motion_table;
  20 int mpd_max_mtab = 0;
  21
  22 void mp_old_init(int max_mtab)
  23 {
  24         int i;
  25
  26         mpd_motion_table=(mpm_motion *)Malloc(max_mtab*sizeof(mpm_motion));
  27
  28         //DbgSetLogFile(MP_SRC, "mpout.txt");
  29
  30         for (i = 0; i < max_mtab; i++)
  31         {
  32                 mpd_motion_table[i].handle=MP_MOT_INVALID;
  33         }
  34
  35         mpd_max_mtab = max_mtab;
  36 }
  37
  38 void mp_old_close()
  39 {
  40    Free(mpd_motion_table);
  41    mpd_max_mtab = 0;
  42 }
  43
  44 // this is clearly imbecilic, but we don't really know what motion loading should
  45 // do yet.
  46 int mp_old_load_motion(Id motID)
  47 {
  48         int result;
  49         int i = 0;
  50
  51         while ((i < mpd_max_mtab) && (mpd_motion_table[i].handle != MP_MOT_INVALID))
  52         {
  53                 i++;
  54         }
  55
  56         if (i == mpd_max_mtab)
  57         {
  58                 result = MP_MOT_INVALID;
  59         }
  60         else
  61         {
  62                 if (ResExtract(motID, (void *) &mpd_motion_table[i]))
  63                 {
  64                         mpd_motion_table[i].handle = i;
  65                         result = i;
  66                 }
  67         }
  68
  69         return result;
  70 }
  71
  72 //
  73
  74 float MotionWeight = 1.0;
  75
  76 //
  77 extern int DBG_flag;
  78 //
  79
  80 #if 0
  81 //
  82 // THIS IS THE UPDATE FUNCTION FOR LG-BIPED DATA.
  83 //
  84 void mp_old_update(multiped * mp, float dt, mxs_vector * pos, mxs_matrix * orient)
  85 {
  86         int i, res_id, res_index, norm_id, bend, frame;
  87         mpm_cmot_handle * cm;
  88         unsigned char * motion_data;
  89         effec_data * ed;
  90         effec_capture * ec;
  91         limb_data * ld;
  92         limb_capture * lc;
  93         trunk_data * td;
  94         trunk_capture * tc;
  95         hand_capture * hc;
  96         mxs_vector * dst;
  97         mxs_vector v1, v2;
  98         mxs_matrix m1, m2, uppertorso_offset, lowerab_direc;
  99         mxs_real r1, r2, r3, r4;
 100         int num_torsos = 0;
 101         int num_limbs = 0;
 102         int num_effectors = 0;
 103         mps_motion_node * node;
 104
 105         int torsos[10];
 106         int limbs[10];
 107         int effectors[10];
 108
 109         int num_frames;
 110         float frame_inc;
 111
 112         mpm_motion * m;
 113
 114         if (mp_list_count(&mp->main_motion))
 115         {
 116                 //m = &mpd_motion_table[mp->motions[0].handle];
 117                 node = (mps_motion_node *) mp_list_traverse(&mp->main_motion, NULL);
 118                 m = &mpd_motion_table[node->handle];
 119                 num_frames = m->info.num_frames;
 120
 121                 frame_inc = ((float) m->info.frequency) * dt / 1000.0;
 122                 node->frame += frame_inc;
 123                 frame = (int) floor(node->frame + 0.5);
 124
 125                 if (frame < m->info.num_frames)
 126                 {
 127                         res_id = REFID(m->component->ref);
 128
 129                         Spew(MP_SRC, ("\nnew frame\n\n"));
 130
 131                 // Loop through component motions, finding torsos, limbs, and effectors.
 132
 133                         cm = m->component;
 134                         for (i = 0; i < MAX_COMPONENTS; i++, cm++)
 135                         {
 136                                 if ((cm->ctype > MPCT_NULL) && (cm->ctype <= MPCT_HAND))
 137                                 {
 138                                         res_index = REFINDEX(cm->ref);
 139
 140                                         motion_data = (unsigned char *) RefLock(cm->ref);
 141
 142                                         switch (cm->ctype)
 143                                         {
 144                                                 case MPCT_TORSO:
 145                                                         torsos[num_torsos++] = i;
 146                                                         break;
 147
 148                                                 case MPCT_LIMB:
 149
 150                                                         limbs[num_limbs++] = i;
 151                                                         break;
 152
 153                                                 case MPCT_EFFECTOR:
 154                                                 case MPCT_HAND:
 155
 156                                                         effectors[num_effectors++] = i;
 157                                                         break;
 158                                         }
 159
 160
 161                                         RefUnlock(cm->ref);
 162                                 }
 163                         }
 164
 165                         for (i = 0; i < num_torsos; i++)
 166                         {
 167                                 cm = m->component + torsos[i];
 168
 169                                 motion_data = (unsigned char *) RefLock(cm->ref);
 170
 171                                 //Spew(MP_SRC, ("torso\n"));
 172
 173                                 td = (trunk_data *) motion_data;
 174                                 tc = (trunk_capture *) (motion_data + sizeof(trunk_data));
 175                                 tc += frame;
 176
 177                         // pelvis and torso orientations.
 178                                 mx_mul_mat(&m1, orient, &tc->base_orient);
 179                                 mx_copy_mat(&lowerab_direc, &m1);
 180                                 mx_copy_vec(&mp->norm[NORM_PELV], &m1.vec[1]);
 181
 182                                 mx_copy_mat(&uppertorso_offset, &tc->ab_orient);
 183                                 mx_mul_mat(&m2, &m1, &tc->ab_orient);
 184                                 mx_copy_vec(&mp->norm[NORM_UPTORS], &m2.vec[1]);
 185
 186                         // butt position.
 187                                 mx_mat_mul_vec(&v1, orient, &tc->base);
 188
 189                                 //mx_add_vec(&mp->joints[BUTT], pos, &v1);
 190                                 mp->joints[BUTT].x = node->base_pos.x + v1.x;
 191                                 mp->joints[BUTT].y = node->base_pos.y + v1.y;
 192                                 mp->joints[BUTT].z = v1.z;
 193
 194                                 mx_copy_vec(pos, &mp->joints[BUTT]);
 195
 196                         // abdomen position.
 197                                 mx_unit_vec(&v1, 2);
 198                                 mx_scaleeq_vec(&v1, td->ab_per * td->spine_len);
 199                                 mx_mat_muleq_vec(&m1, &v1);
 200                                 mx_add_vec(&mp->joints[ABDOMEN], &mp->joints[BUTT], &v1);
 201
 202                         // neck.
 203
 204                                 mx_unit_vec(&v1, 2);
 205                                 mx_scaleeq_vec(&v1, (1 - td->ab_per) * td->spine_len);
 206                                 mx_mat_muleq_vec(&m2, &v1);
 207                                 mx_add_vec(&mp->joints[NECK], &mp->joints[ABDOMEN], &v1);
 208
 209                         // shoulders.
 210
 211                                 mx_copy_vec(&v1, &tc->lshldr);
 212                                 mx_mat_muleq_vec(&m2, &v1);
 213                                 mx_add_vec(&mp->joints[LSHLDR], &mp->joints[ABDOMEN], &v1);
 214                                 mx_copy_vec(&v1, &tc->rshldr);
 215                                 mx_mat_muleq_vec(&m2, &v1);
 216                                 mx_add_vec(&mp->joints[RSHLDR], &mp->joints[ABDOMEN], &v1);
 217
 218                         // hips.
 219
 220                                 mx_unit_vec(&v1, 1);
 221                                 mx_scaleeq_vec(&v1, td->hip_width / 2);
 222                                 mx_mat_muleq_vec(&m1, &v1);
 223                                 mx_scale_vec(&v2, &v1, -1);
 224                                 mx_add_vec(&mp->joints[LHIP], &mp->joints[BUTT], &v1);
 225                                 mx_add_vec(&mp->joints[RHIP], &mp->joints[BUTT], &v2);
 226
 227                                 RefUnlock(cm->ref);
 228                         }
 229
 230                         for (i = 0; i < num_limbs; i++)
 231                         {
 232                                 cm = m->component + limbs[i];
 233                                 motion_data = (unsigned char *) RefLock(cm->ref);
 234
 235                                 ld = (limb_data *) motion_data;
 236                                 lc = (limb_capture *) (motion_data + sizeof(limb_data));
 237                                 lc += frame;
 238
 239                                 //Spew(MP_SRC, ("limb: %s -> %s\n", Jnt[ld->base_joint_id], Jnt[ld->end_joint_id]));
 240
 241                                 switch (ld->base_joint_id)
 242                                 {
 243                                         case LSHLDR:
 244                                                 dst = mp->joints + LELBOW;
 245                                                 mx_mul_mat(&m1, &lowerab_direc, &uppertorso_offset);
 246                                                 norm_id = NORM_LARM;
 247                                                 bend = 0;
 248                                                 break;
 249
 250                                         case RSHLDR:
 251                                                 dst = mp->joints + RELBOW;
 252                                                 mx_mul_mat(&m1, &lowerab_direc, &uppertorso_offset);
 253                                                 norm_id = NORM_RARM;
 254                                                 bend = 0;
 255                                                 break;
 256
 257                                         case LHIP:
 258                                                 dst = mp->joints + LKNEE;
 259                                                 mx_copy_mat(&m1, &lowerab_direc);
 260                                                 norm_id = NORM_LLEG;
 261                                                 bend = 1;
 262                                                 break;
 263
 264                                         case RHIP:
 265                                                 dst = mp->joints + RKNEE;
 266                                                 mx_copy_mat(&m1, &lowerab_direc);
 267                                                 norm_id = NORM_RLEG;
 268                                                 bend = 1;
 269                                                 break;
 270                                 }
 271
 272                                 mx_mat_mul_vec(&v1, &m1, &lc->e_data);
 273                                 mx_add_vec(&mp->joints[ld->end_joint_id], &mp->joints[ld->base_joint_id], &v1);
 274
 275                                 mx_mat_mul_vec(&mp->norm[norm_id], &m1, &lc->n_data);
 276
 277                         // solve for joint position.
 278
 279                                 mx_sub_vec(&v1, &mp->joints[ld->end_joint_id], &mp->joints[ld->base_joint_id]);
 280                                 r1 = mx_dot_vec(&v1, &v1);
 281                                 r2 = sqrt(r1);
 282
 283                                 r3 = ld->len1 * ld->len1 - ld->len2 * ld->len2;
 284                                 r4 = ((r3/r1) + 1.0) / 2.0;
 285
 286                                 dst->x = mp->joints[ld->base_joint_id].x + v1.x * r4;
 287                                 dst->y = mp->joints[ld->base_joint_id].y + v1.y * r4;
 288                                 dst->z = mp->joints[ld->base_joint_id].z + v1.z * r4;
 289
 290                                 mx_cross_vec(&v2, &mp->norm[norm_id], &v1);
 291                                 r4 = (r3/r2 + r2) / 2.0;
 292                                 r4 = sqrt((ld->len1 * ld->len1) - (r4 * r4)) / r2;
 293                                 if (bend)
 294                                 {
 295                                         r4 = -r4;
 296                                 }
 297
 298                                 dst->x += v2.x * r4;
 299                                 dst->y += v2.y * r4;
 300                                 dst->z += v2.z * r4;
 301
 302                                 RefUnlock(cm->ref);
 303                         }
 304
 305                         for (i = 0; i < num_effectors; i++)
 306                         {
 307                                 cm = m->component + effectors[i];
 308                                 motion_data = (unsigned char *) RefLock(cm->ref);
 309
 310                                 ed = (effec_data *) motion_data;
 311
 312                                 if (cm->ctype == MPCT_HAND)
 313                                 {
 314                                         hc = (hand_capture *) (motion_data + sizeof(effec_data));
 315                                         hc += frame;
 316                                         ec = &hc->effec_table;
 317                                 }
 318                                 else
 319                                 {
 320                                         ec = (effec_capture *) (motion_data + sizeof(effec_data));
 321                                         ec += frame;
 322                                 }
 323
 324                                 //Spew(MP_SRC, ("effector: %s -> %s\n", Jnt[ed->base_joint_id], Jnt[ed->eff_pos_id]));
 325
 326                                 dst = mp->joints + ed->eff_pos_id;
 327
 328                                 switch (ed->eff_pos_id)
 329                                 {
 330                                         case HEAD:
 331                                                 norm_id = NORM_HEAD;
 332                                                 break;
 333
 334                                         case LFINGER:
 335                                                 norm_id = NORM_LHAND;
 336                                                 break;
 337
 338                                         case RFINGER:
 339                                                 norm_id = NORM_RHAND;
 340                                                 break;
 341
 342                                         case LTOE:
 343                                                 norm_id = NORM_LLEG;
 344                                                 break;
 345
 346                                         case RTOE:
 347                                                 norm_id = NORM_RLEG;
 348                                                 break;
 349
 350                                         default:
 351                                                 Spew(MP_SRC, ("neglected effector\n"));
 352                                                 break;
 353                                 }
 354                                 mx_copy_mat(&m1, &ec->e_data);
 355                                 mx_mul_mat(&m2, &uppertorso_offset, &m1);
 356                                 mx_mul_mat(&m1, &lowerab_direc, &m2);
 357                                 mx_copy_vec(&v1, &m1.vec[0]);
 358                                 mx_scaleeq_vec(&v1, ed->len1);
 359                                 mx_add_vec(&mp->joints[ed->eff_pos_id], &mp->joints[ed->base_joint_id], &v1);
 360                                 mx_copy_vec(&mp->norm[norm_id], &m1.vec[1]);
 361
 362                                 if (norm_id == NORM_HEAD)
 363                                 {
 364                                         //Spew(MP_SRC, ("head normal: %f, %f, %f\n", fix_from_float(m1.vec[1].x), fix_from_float(m1.vec[1].y), fix_from_float(m1.vec[1].z)));
 365                                 }
 366
 367                                 RefUnlock(cm->ref);
 368                         }
 369                 /*
 370                         mp->motions[0].frame++;
 371                         if (mp->motions[0].frame >= num_frames)
 372                         {
 373                                 //mp->motion = -1;
 374                                 mp->num_active_motions = 0;
 375                         }
 376                 */
 377                 }
 378         }
 379 }
 380 #endif
 381
 382 //
 383 /*
 384 void mp_start_motion(multiped * mp, unsigned int motion_num, mxs_vector * start_pos, mxs_matrix * start_orient)
 385 {
 386         mpm_motion * m = &mpd_qmotion_table[motion_num];
 387
 388         //mx_mat2ang(start_orient, &m->info.start_orient);
 389 //mx_copy_mat(start_orient, &m->info.start_orient);
 390         //mx_identity_mat(start_orient);
 391
 392         mp->motion = motion_num;
 393         mp->frame = 0;
 394         mx_copy_vec(&mp->base_pos, start_pos);
 395
 396
 397 }
 398
 399 //
 400
 401 void mp_stop_all_motions(multiped * mp)
 402 {
 403         mp->motion = -1;
 404 }
 405 */
 406 //
 407
 408