vcs.maemo.org Git - neverball/blob - share/solid_all.c

   1
   2 /* Random code used in more than one place. */
   3
   4 #include "solid_all.h"
   5 #include "solid_cmd.h"
   6 #include "solid.h"
   7 #include "common.h"
   8 #include "vec3.h"
   9 #include "geom.h"
  10
  11 /*---------------------------------------------------------------------------*/
  12
  13 static float erp(float t)
  14 {
  15     return 3.0f * t * t - 2.0f * t * t * t;
  16 }
  17
  18 #if UNUSED
  19 static float derp(float t)
  20 {
  21     return 6.0f * t     - 6.0f * t * t;
  22 }
  23 #endif
  24
  25 void sol_body_p(float p[3], const struct s_file *fp, int pi, float t)
  26 {
  27     float v[3];
  28
  29     if (pi >= 0)
  30     {
  31         const struct s_path *pp = fp->pv + pi;
  32         const struct s_path *pq = fp->pv + pp->pi;
  33
  34         float s = MIN(t / pp->t, 1.0f);
  35
  36         v_sub(v, pq->p, pp->p);
  37         v_mad(p, pp->p, v, pp->s ? erp(s) : s);
  38     }
  39     else
  40     {
  41         p[0] = 0.0f;
  42         p[1] = 0.0f;
  43         p[2] = 0.0f;
  44     }
  45 }
  46
  47 void sol_body_v(float v[3],
  48                 const struct s_file *fp,
  49                 int pi, float t, float dt)
  50 {
  51     if (pi >= 0 && fp->pv[pi].f)
  52     {
  53         float p[3], q[3];
  54
  55         sol_body_p(p, fp, pi, t);
  56         sol_body_p(q, fp, pi, t + dt);
  57
  58         v_sub(v, q, p);
  59
  60         v[0] /= dt;
  61         v[1] /= dt;
  62         v[2] /= dt;
  63     }
  64     else
  65     {
  66         v[0] = 0.0f;
  67         v[1] = 0.0f;
  68         v[2] = 0.0f;
  69     }
  70 }
  71
  72 void sol_body_e(float e[4],
  73                 const struct s_file *fp,
  74                 const struct s_body *bp)
  75 {
  76     struct s_path *pp = fp->pv + bp->pi;
  77
  78     if (bp->pi >= 0)
  79     {
  80         struct s_path *pq = fp->pv + pp->pi;
  81
  82         if (pp->fl & P_ORIENTED || pq->fl & P_ORIENTED)
  83         {
  84             q_slerp(e, pp->e, pq->e, bp->t / pp->t);
  85             return;
  86         }
  87     }
  88
  89     e[0] = 1.0f;
  90     e[1] = 0.0f;
  91     e[2] = 0.0f;
  92     e[3] = 0.0f;
  93 }
  94
  95 void sol_body_w(float w[3],
  96                 const struct s_file *fp,
  97                 const struct s_body *bp)
  98 {
  99     struct s_path *pp = fp->pv + bp->pi;
 100
 101     if (bp->pi >= 0 && pp->f)
 102     {
 103         struct s_path *pq = fp->pv + pp->pi;
 104
 105         if (pp->fl & P_ORIENTED || pq->fl & P_ORIENTED)
 106         {
 107             float d[4], i[4], a;
 108
 109             /*
 110              * a * d = b
 111              * d = b / a
 112              * d = b * (1 / a)
 113              */
 114
 115             i[0] =  pp->e[0];
 116             i[1] = -pp->e[1];
 117             i[2] = -pp->e[2];
 118             i[3] = -pp->e[3];
 119
 120             q_mul(d, pq->e, i);
 121             q_nrm(d, d);
 122
 123             /* Match slerp by using the short path. */
 124
 125             if (d[0] < 0)
 126             {
 127                 d[0] = -d[0];
 128                 d[1] = -d[1];
 129                 d[2] = -d[2];
 130                 d[3] = -d[3];
 131             }
 132
 133             q_as_axisangle(d, w, &a);
 134             v_scl(w, w, a / pp->t);
 135             return;
 136         }
 137     }
 138
 139     w[0] = 0.0f;
 140     w[1] = 0.0f;
 141     w[2] = 0.0f;
 142 }
 143
 144 /*---------------------------------------------------------------------------*/
 145
 146 /*
 147  * Integrate the rotation of the given basis E under angular velocity W
 148  * through time DT.
 149  */
 150 void sol_rotate(float e[3][3], const float w[3], float dt)
 151 {
 152     if (v_len(w) > 0.0f)
 153     {
 154         float a[3], M[16], f[3][3];
 155
 156         /* Compute the rotation matrix. */
 157
 158         v_nrm(a, w);
 159         m_rot(M, a, v_len(w) * dt);
 160
 161         /* Apply it to the basis. */
 162
 163         m_vxfm(f[0], M, e[0]);
 164         m_vxfm(f[1], M, e[1]);
 165         m_vxfm(f[2], M, e[2]);
 166
 167         /* Re-orthonormalize the basis. */
 168
 169         v_crs(e[2], f[0], f[1]);
 170         v_crs(e[1], f[2], f[0]);
 171         v_crs(e[0], f[1], f[2]);
 172
 173         v_nrm(e[0], e[0]);
 174         v_nrm(e[1], e[1]);
 175         v_nrm(e[2], e[2]);
 176     }
 177 }
 178
 179 /*---------------------------------------------------------------------------*/
 180
 181 /*
 182  * Compute the new angular velocity and orientation of a ball pendulum.
 183  * A gives the accelleration of the ball.  G gives the gravity vector.
 184  */
 185 void sol_pendulum(struct s_ball *up,
 186                   const float a[3],
 187                   const float g[3], float dt)
 188 {
 189     float v[3], A[3], F[3], r[3], Y[3], T[3] = { 0.0f, 0.0f, 0.0f };
 190
 191     const float m  = 5.000f;
 192     const float ka = 0.500f;
 193     const float kd = 0.995f;
 194
 195     /* Find the total force over DT. */
 196
 197     v_scl(A, a,     ka);
 198     v_mad(A, A, g, -dt);
 199
 200     /* Find the force. */
 201
 202     v_scl(F, A, m / dt);
 203
 204     /* Find the position of the pendulum. */
 205
 206     v_scl(r, up->E[1], -up->r);
 207
 208     /* Find the torque on the pendulum. */
 209
 210     if (fabsf(v_dot(r, F)) > 0.0f)
 211         v_crs(T, F, r);
 212
 213     /* Apply the torque and dampen the angular velocity. */
 214
 215     v_mad(up->W, up->W, T, dt);
 216     v_scl(up->W, up->W,    kd);
 217
 218     /* Apply the angular velocity to the pendulum basis. */
 219
 220     sol_rotate(up->E, up->W, dt);
 221
 222     /* Apply a torque turning the pendulum toward the ball velocity. */
 223
 224     v_mad(v, up->v, up->E[1], v_dot(up->v, up->E[1]));
 225     v_crs(Y, v, up->E[2]);
 226     v_scl(Y, up->E[1], 2 * v_dot(Y, up->E[1]));
 227
 228     sol_rotate(up->E, Y, dt);
 229 }
 230
 231 /*---------------------------------------------------------------------------*/
 232
 233 /*
 234  * Compute the states of all switches after DT seconds have passed.
 235  */
 236 void sol_swch_step(struct s_file *fp, float dt)
 237 {
 238     int xi;
 239
 240     union cmd cmd;
 241
 242     for (xi = 0; xi < fp->xc; xi++)
 243     {
 244         struct s_swch *xp = fp->xv + xi;
 245
 246         volatile float t = xp->t;
 247
 248         if (t < xp->t0)
 249         {
 250             xp->t = (t += dt);
 251
 252             if (t >= xp->t0)
 253             {
 254                 int pi = xp->pi;
 255                 int pj = xp->pi;
 256
 257                 do  /* Tortoise and hare cycle traverser. */
 258                 {
 259                     fp->pv[pi].f = xp->f0;
 260                     fp->pv[pj].f = xp->f0;
 261
 262                     cmd.type        = CMD_PATH_FLAG;
 263                     cmd.pathflag.pi = pi;
 264                     cmd.pathflag.f  = fp->pv[pi].f;
 265                     sol_cmd_enq(&cmd);
 266
 267                     pi = fp->pv[pi].pi;
 268                     pj = fp->pv[pj].pi;
 269                     pj = fp->pv[pj].pi;
 270                 }
 271                 while (pi != pj);
 272
 273                 xp->f = xp->f0;
 274
 275                 cmd.type          = CMD_SWCH_TOGGLE;
 276                 cmd.swchtoggle.xi = xi;
 277                 sol_cmd_enq(&cmd);
 278             }
 279         }
 280     }
 281 }
 282
 283 /*
 284  * Compute the positions of all bodies after DT seconds have passed.
 285  */
 286 void sol_body_step(struct s_file *fp, float dt)
 287 {
 288     int i;
 289
 290     union cmd cmd;
 291
 292     for (i = 0; i < fp->bc; i++)
 293     {
 294         struct s_body *bp = fp->bv + i;
 295         struct s_path *pp = fp->pv + bp->pi;
 296
 297         volatile float t = bp->t;
 298
 299         if (bp->pi >= 0 && pp->f)
 300         {
 301             bp->t = (t += dt);
 302
 303             if (t >= pp->t)
 304             {
 305                 bp->t  = 0;
 306                 bp->pi = pp->pi;
 307
 308                 cmd.type        = CMD_BODY_TIME;
 309                 cmd.bodytime.bi = i;
 310                 cmd.bodytime.t  = bp->t;
 311                 sol_cmd_enq(&cmd);
 312
 313                 cmd.type        = CMD_BODY_PATH;
 314                 cmd.bodypath.bi = i;
 315                 cmd.bodypath.pi = bp->pi;
 316                 sol_cmd_enq(&cmd);
 317             }
 318         }
 319     }
 320 }
 321
 322 /*
 323  * Compute the positions of all balls after DT seconds have passed.
 324  */
 325 void sol_ball_step(struct s_file *fp, float dt)
 326 {
 327     int i;
 328
 329     for (i = 0; i < fp->uc; i++)
 330     {
 331         struct s_ball *up = fp->uv + i;
 332
 333         v_mad(up->p, up->p, up->v, dt);
 334
 335         sol_rotate(up->e, up->w, dt);
 336     }
 337 }
 338
 339 /*---------------------------------------------------------------------------*/
 340
 341 int sol_item_test(struct s_file *fp, float *p, float item_r)
 342 {
 343     const float *ball_p = fp->uv->p;
 344     const float  ball_r = fp->uv->r;
 345
 346     int hi;
 347
 348     for (hi = 0; hi < fp->hc; hi++)
 349     {
 350         float r[3];
 351
 352         r[0] = ball_p[0] - fp->hv[hi].p[0];
 353         r[1] = ball_p[1] - fp->hv[hi].p[1];
 354         r[2] = ball_p[2] - fp->hv[hi].p[2];
 355
 356         if (fp->hv[hi].t != ITEM_NONE && v_len(r) < ball_r + item_r)
 357         {
 358             p[0] = fp->hv[hi].p[0];
 359             p[1] = fp->hv[hi].p[1];
 360             p[2] = fp->hv[hi].p[2];
 361
 362             return hi;
 363         }
 364     }
 365     return -1;
 366 }
 367
 368 struct s_goal *sol_goal_test(struct s_file *fp, float *p, int ui)
 369 {
 370     const float *ball_p = fp->uv[ui].p;
 371     const float  ball_r = fp->uv[ui].r;
 372     int zi;
 373
 374     for (zi = 0; zi < fp->zc; zi++)
 375     {
 376         float r[3];
 377
 378         r[0] = ball_p[0] - fp->zv[zi].p[0];
 379         r[1] = ball_p[2] - fp->zv[zi].p[2];
 380         r[2] = 0;
 381
 382         if (v_len(r) < fp->zv[zi].r - ball_r &&
 383             ball_p[1] > fp->zv[zi].p[1] &&
 384             ball_p[1] < fp->zv[zi].p[1] + GOAL_HEIGHT / 2)
 385         {
 386             p[0] = fp->zv[zi].p[0];
 387             p[1] = fp->zv[zi].p[1];
 388             p[2] = fp->zv[zi].p[2];
 389
 390             return &fp->zv[zi];
 391         }
 392     }
 393     return NULL;
 394 }
 395
 396 /*
 397  * Test if the  ball UI is inside a  jump. Return 1 if yes  and fill P
 398  * with the destination position, return 0 if not, and return 2 if the
 399  * ball is on the border of a jump.
 400  */
 401 int sol_jump_test(struct s_file *fp, float *p, int ui)
 402 {
 403     const float *ball_p = fp->uv[ui].p;
 404     const float  ball_r = fp->uv[ui].r;
 405     int ji;
 406     float l;
 407     int res = 0;
 408
 409     for (ji = 0; ji < fp->jc; ji++)
 410     {
 411         float r[3];
 412
 413         r[0] = ball_p[0] - fp->jv[ji].p[0];
 414         r[1] = ball_p[2] - fp->jv[ji].p[2];
 415         r[2] = 0;
 416
 417         l = v_len(r) - fp->jv[ji].r;
 418         if (l < 0 &&
 419             ball_p[1] > fp->jv[ji].p[1] &&
 420             ball_p[1] < fp->jv[ji].p[1] + JUMP_HEIGHT / 2)
 421         {
 422             if (l < - ball_r )
 423             {
 424                 p[0] = fp->jv[ji].q[0] + (ball_p[0] - fp->jv[ji].p[0]);
 425                 p[1] = fp->jv[ji].q[1] + (ball_p[1] - fp->jv[ji].p[1]);
 426                 p[2] = fp->jv[ji].q[2] + (ball_p[2] - fp->jv[ji].p[2]);
 427
 428                 return 1;
 429             }
 430             else
 431                 res = 2;
 432         }
 433     }
 434     return res;
 435 }
 436
 437 /*
 438  * Test and process the event the ball UI enters a switch. Return 1 if
 439  * a visible  switch is  activated, return 0  otherwise (no  switch is
 440  * activated or only invisible switches).
 441  */
 442 int sol_swch_test(struct s_file *fp, int ui)
 443 {
 444     const float *ball_p = fp->uv[ui].p;
 445     const float  ball_r = fp->uv[ui].r;
 446     int xi;
 447     int res = 0;
 448
 449     union cmd cmd;
 450
 451     for (xi = 0; xi < fp->xc; xi++)
 452     {
 453         struct s_swch *xp = fp->xv + xi;
 454
 455         /* FIXME enter/exit events don't work for timed switches */
 456
 457         if (xp->t0 == 0 || xp->f == xp->f0)
 458         {
 459             float l;
 460             float r[3];
 461
 462             r[0] = ball_p[0] - xp->p[0];
 463             r[1] = ball_p[2] - xp->p[2];
 464             r[2] = 0;
 465
 466             l = v_len(r) - xp->r;
 467
 468             if (l < ball_r &&
 469                 ball_p[1] > xp->p[1] &&
 470                 ball_p[1] < xp->p[1] + SWCH_HEIGHT / 2)
 471             {
 472                 if (!xp->e && l < - ball_r)
 473                 {
 474                     int pi = xp->pi;
 475                     int pj = xp->pi;
 476
 477                     /* The ball enters. */
 478
 479                     if (xp->t0 == 0)
 480                     {
 481                         xp->e = 1;
 482
 483                         cmd.type         = CMD_SWCH_ENTER;
 484                         cmd.swchenter.xi = xi;
 485                         sol_cmd_enq(&cmd);
 486                     }
 487
 488                     /* Toggle the state, update the path. */
 489
 490                     xp->f = xp->f ? 0 : 1;
 491
 492                     cmd.type          = CMD_SWCH_TOGGLE;
 493                     cmd.swchtoggle.xi = xi;
 494                     sol_cmd_enq(&cmd);
 495
 496                     do  /* Tortoise and hare cycle traverser. */
 497                     {
 498                         fp->pv[pi].f = xp->f;
 499                         fp->pv[pj].f = xp->f;
 500
 501                         cmd.type        = CMD_PATH_FLAG;
 502                         cmd.pathflag.pi = pi;
 503                         cmd.pathflag.f  = fp->pv[pi].f;
 504                         sol_cmd_enq(&cmd);
 505
 506                         pi = fp->pv[pi].pi;
 507                         pj = fp->pv[pj].pi;
 508                         pj = fp->pv[pj].pi;
 509                     }
 510                     while (pi != pj);
 511
 512                     /* It toggled to non-default state, start the timer. */
 513
 514                     if (xp->f != xp->f0)
 515                         xp->t = 0.0f;
 516
 517                     /* If visible, set the result. */
 518
 519                     if (!xp->i)
 520                         res = 1;
 521                 }
 522             }
 523
 524             /* The ball exits. */
 525
 526             else if (xp->e)
 527             {
 528                 xp->e = 0;
 529
 530                 cmd.type        = CMD_SWCH_EXIT;
 531                 cmd.swchexit.xi = xi;
 532                 sol_cmd_enq(&cmd);
 533             }
 534         }
 535     }
 536     return res;
 537 }
 538
 539 /*---------------------------------------------------------------------------*/