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