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

   1 /*
   2  * Copyright (C) 2003-2010 Neverball authors
   3  *
   4  * NEVERBALL is  free software; you can redistribute  it and/or modify
   5  * it under the  terms of the GNU General  Public License as published
   6  * by the Free  Software Foundation; either version 2  of the License,
   7  * or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT  ANY  WARRANTY;  without   even  the  implied  warranty  of
  11  * MERCHANTABILITY or  FITNESS FOR A PARTICULAR PURPOSE.   See the GNU
  12  * General Public License for more details.
  13  */
  14
  15
  16 /* Random code used in more than one place. */
  17
  18 #include "solid_all.h"
  19 #include "solid_cmd.h"
  20 #include "solid_vary.h"
  21
  22 #include "common.h"
  23 #include "vec3.h"
  24 #include "geom.h"
  25
  26 /*---------------------------------------------------------------------------*/
  27
  28 static float erp(float t)
  29 {
  30     return 3.0f * t * t - 2.0f * t * t * t;
  31 }
  32
  33 #if UNUSED
  34 static float derp(float t)
  35 {
  36     return 6.0f * t     - 6.0f * t * t;
  37 }
  38 #endif
  39
  40 void sol_body_p(float p[3], const struct s_vary *vary, int pi, float t)
  41 {
  42     float v[3];
  43
  44     if (pi >= 0)
  45     {
  46         const struct b_path *pp = vary->base->pv + pi;
  47         const struct b_path *pq = vary->base->pv + pp->pi;
  48
  49         float s = MIN(t / pp->t, 1.0f);
  50
  51         v_sub(v, pq->p, pp->p);
  52         v_mad(p, pp->p, v, pp->s ? erp(s) : s);
  53     }
  54     else
  55     {
  56         p[0] = 0.0f;
  57         p[1] = 0.0f;
  58         p[2] = 0.0f;
  59     }
  60 }
  61
  62 void sol_body_v(float v[3],
  63                 const struct s_vary *vary,
  64                 int pi, float t, float dt)
  65 {
  66     if (pi >= 0 && vary->pv[pi].f)
  67     {
  68         float p[3], q[3];
  69
  70         sol_body_p(p, vary, pi, t);
  71         sol_body_p(q, vary, pi, t + dt);
  72
  73         v_sub(v, q, p);
  74
  75         v[0] /= dt;
  76         v[1] /= dt;
  77         v[2] /= dt;
  78     }
  79     else
  80     {
  81         v[0] = 0.0f;
  82         v[1] = 0.0f;
  83         v[2] = 0.0f;
  84     }
  85 }
  86
  87 void sol_body_e(float e[4],
  88                 const struct s_vary *vary,
  89                 const struct v_body *bp,
  90                 float dt)
  91 {
  92     struct b_path *pp = vary->base->pv + bp->pi;
  93
  94     if (bp->pi >= 0)
  95     {
  96         struct b_path *pq = vary->base->pv + pp->pi;
  97
  98         if (pp->fl & P_ORIENTED || pq->fl & P_ORIENTED)
  99         {
 100             if (!vary->pv[bp->pi].f)
 101                 dt = 0;
 102
 103             q_slerp(e, pp->e, pq->e, (bp->t + dt) / pp->t);
 104             return;
 105         }
 106     }
 107
 108     e[0] = 1.0f;
 109     e[1] = 0.0f;
 110     e[2] = 0.0f;
 111     e[3] = 0.0f;
 112 }
 113
 114 void sol_body_w(float w[3],
 115                 const struct s_vary *vary,
 116                 const struct v_body *bp)
 117 {
 118     struct b_path *pp = vary->base->pv + bp->pi;
 119
 120     if (bp->pi >= 0 && vary->pv[bp->pi].f)
 121     {
 122         struct b_path *pq = vary->base->pv + pp->pi;
 123
 124         if (pp->fl & P_ORIENTED || pq->fl & P_ORIENTED)
 125         {
 126             float d[4], i[4], a;
 127
 128             /*
 129              * a * d = b
 130              * d = b / a
 131              * d = b * (1 / a)
 132              */
 133
 134             i[0] =  pp->e[0];
 135             i[1] = -pp->e[1];
 136             i[2] = -pp->e[2];
 137             i[3] = -pp->e[3];
 138
 139             q_mul(d, pq->e, i);
 140             q_nrm(d, d);
 141
 142             /* Match slerp by using the short path. */
 143
 144             if (d[0] < 0)
 145             {
 146                 d[0] = -d[0];
 147                 d[1] = -d[1];
 148                 d[2] = -d[2];
 149                 d[3] = -d[3];
 150             }
 151
 152             q_as_axisangle(d, w, &a);
 153             v_scl(w, w, a / pp->t);
 154             return;
 155         }
 156     }
 157
 158     w[0] = 0.0f;
 159     w[1] = 0.0f;
 160     w[2] = 0.0f;
 161 }
 162
 163 /*---------------------------------------------------------------------------*/
 164
 165 /*
 166  * Integrate the rotation of the given basis E under angular velocity W
 167  * through time DT.
 168  */
 169 void sol_rotate(float e[3][3], const float w[3], float dt)
 170 {
 171     if (v_len(w) > 0.0f)
 172     {
 173         float a[3], M[16], f[3][3];
 174
 175         /* Compute the rotation matrix. */
 176
 177         v_nrm(a, w);
 178         m_rot(M, a, v_len(w) * dt);
 179
 180         /* Apply it to the basis. */
 181
 182         m_vxfm(f[0], M, e[0]);
 183         m_vxfm(f[1], M, e[1]);
 184         m_vxfm(f[2], M, e[2]);
 185
 186         /* Re-orthonormalize the basis. */
 187
 188         v_crs(e[2], f[0], f[1]);
 189         v_crs(e[1], f[2], f[0]);
 190         v_crs(e[0], f[1], f[2]);
 191
 192         v_nrm(e[0], e[0]);
 193         v_nrm(e[1], e[1]);
 194         v_nrm(e[2], e[2]);
 195     }
 196 }
 197
 198 /*---------------------------------------------------------------------------*/
 199
 200 /*
 201  * Compute the new angular velocity and orientation of a ball pendulum.
 202  * A gives the accelleration of the ball.  G gives the gravity vector.
 203  */
 204 void sol_pendulum(struct v_ball *up,
 205                   const float a[3],
 206                   const float g[3], float dt)
 207 {
 208     float v[3], A[3], F[3], r[3], Y[3], T[3] = { 0.0f, 0.0f, 0.0f };
 209
 210     const float m  = 5.000f;
 211     const float ka = 0.500f;
 212     const float kd = 0.995f;
 213
 214     /* Find the total force over DT. */
 215
 216     v_scl(A, a,     ka);
 217     v_mad(A, A, g, -dt);
 218
 219     /* Find the force. */
 220
 221     v_scl(F, A, m / dt);
 222
 223     /* Find the position of the pendulum. */
 224
 225     v_scl(r, up->E[1], -up->r);
 226
 227     /* Find the torque on the pendulum. */
 228
 229     if (fabsf(v_dot(r, F)) > 0.0f)
 230         v_crs(T, F, r);
 231
 232     /* Apply the torque and dampen the angular velocity. */
 233
 234     v_mad(up->W, up->W, T, dt);
 235     v_scl(up->W, up->W,    kd);
 236
 237     /* Apply the angular velocity to the pendulum basis. */
 238
 239     sol_rotate(up->E, up->W, dt);
 240
 241     /* Apply a torque turning the pendulum toward the ball velocity. */
 242
 243     v_mad(v, up->v, up->E[1], v_dot(up->v, up->E[1]));
 244     v_crs(Y, v, up->E[2]);
 245     v_scl(Y, up->E[1], 2 * v_dot(Y, up->E[1]));
 246
 247     sol_rotate(up->E, Y, dt);
 248 }
 249
 250 /*---------------------------------------------------------------------------*/
 251
 252 static void sol_path_flag(struct s_vary *vary, int pi, int f)
 253 {
 254     union cmd cmd;
 255
 256     if (vary->pv[pi].f == f)
 257         return;
 258
 259     vary->pv[pi].f = f;
 260
 261     cmd.type = CMD_PATH_FLAG;
 262     cmd.pathflag.pi = pi;
 263     cmd.pathflag.f = vary->pv[pi].f;
 264     sol_cmd_enq(&cmd);
 265 }
 266
 267 static void sol_path_loop(struct s_vary *vary, int p0, int f)
 268 {
 269     int pi = p0;
 270     int pj = p0;
 271     int pk;
 272
 273     do  /* Tortoise and hare cycle traverser. */
 274     {
 275         sol_path_flag(vary, pi, f);
 276
 277         pi = vary->base->pv[pi].pi;
 278         pj = vary->base->pv[pj].pi;
 279         pj = vary->base->pv[pj].pi;
 280     }
 281     while (pi != pj);
 282
 283     /*
 284      * At this point, the indices point to a node in the loop, but we
 285      * still need to walk any remaining nodes in that loop. This is
 286      * essentially the second part of the tortoise and hare algorithm
 287      * which finds the start of the loop, although we only care about
 288      * walking the remaining nodes.
 289      */
 290
 291     pj = p0;
 292     pk = pi;
 293
 294     do
 295     {
 296         sol_path_flag(vary, pi, f);
 297
 298         pi = vary->base->pv[pi].pi;
 299         pj = vary->base->pv[pj].pi;
 300     }
 301     while (pi != pj && pi != pk);
 302 }
 303
 304 /*---------------------------------------------------------------------------*/
 305
 306 /*
 307  * Compute the states of all switches after DT seconds have passed.
 308  */
 309 void sol_swch_step(struct s_vary *vary, float dt, int ms)
 310 {
 311     int xi;
 312
 313     union cmd cmd;
 314
 315     for (xi = 0; xi < vary->xc; xi++)
 316     {
 317         struct v_swch *xp = vary->xv + xi;
 318
 319         if (xp->tm < xp->base->tm)
 320         {
 321             xp->t += dt;
 322             xp->tm += ms;
 323
 324             if (xp->tm >= xp->base->tm)
 325             {
 326                 sol_path_loop(vary, xp->base->pi, xp->base->f);
 327
 328                 xp->f = xp->base->f;
 329
 330                 cmd.type = CMD_SWCH_TOGGLE;
 331                 cmd.swchtoggle.xi = xi;
 332                 sol_cmd_enq(&cmd);
 333             }
 334         }
 335     }
 336 }
 337
 338 /*
 339  * Compute the positions of all bodies after DT seconds have passed.
 340  */
 341 void sol_body_step(struct s_vary *vary, float dt, int ms)
 342 {
 343     int i;
 344
 345     union cmd cmd;
 346
 347     for (i = 0; i < vary->bc; i++)
 348     {
 349         struct v_body *bp = vary->bv + i;
 350         struct v_path *pp = vary->pv + bp->pi;
 351
 352         if (bp->pi >= 0 && pp->f)
 353         {
 354             bp->t += dt;
 355             bp->tm += ms;
 356
 357             if (bp->tm >= pp->base->tm)
 358             {
 359                 bp->t  = 0;
 360                 bp->tm = 0;
 361                 bp->pi = pp->base->pi;
 362
 363                 cmd.type        = CMD_BODY_TIME;
 364                 cmd.bodytime.bi = i;
 365                 cmd.bodytime.t  = bp->t;
 366                 sol_cmd_enq(&cmd);
 367
 368                 cmd.type        = CMD_BODY_PATH;
 369                 cmd.bodypath.bi = i;
 370                 cmd.bodypath.pi = bp->pi;
 371                 sol_cmd_enq(&cmd);
 372             }
 373         }
 374     }
 375 }
 376
 377 /*
 378  * Compute the positions of all balls after DT seconds have passed.
 379  */
 380 void sol_ball_step(struct s_vary *vary, float dt)
 381 {
 382     int i;
 383
 384     for (i = 0; i < vary->uc; i++)
 385     {
 386         struct v_ball *up = vary->uv + i;
 387
 388         v_mad(up->p, up->p, up->v, dt);
 389
 390         sol_rotate(up->e, up->w, dt);
 391     }
 392 }
 393
 394 /*---------------------------------------------------------------------------*/
 395
 396 int sol_item_test(struct s_vary *vary, float *p, float item_r)
 397 {
 398     const float *ball_p = vary->uv->p;
 399     const float  ball_r = vary->uv->r;
 400     int hi;
 401
 402     for (hi = 0; hi < vary->hc; hi++)
 403     {
 404         struct v_item *hp = vary->hv + hi;
 405         float r[3];
 406
 407         v_sub(r, ball_p, hp->p);
 408
 409         if (hp->t != ITEM_NONE && v_len(r) < ball_r + item_r)
 410         {
 411             p[0] = hp->p[0];
 412             p[1] = hp->p[1];
 413             p[2] = hp->p[2];
 414
 415             return hi;
 416         }
 417     }
 418     return -1;
 419 }
 420
 421 struct b_goal *sol_goal_test(struct s_vary *vary, float *p, int ui)
 422 {
 423     const float *ball_p = vary->uv[ui].p;
 424     const float  ball_r = vary->uv[ui].r;
 425     int zi;
 426
 427     for (zi = 0; zi < vary->base->zc; zi++)
 428     {
 429         struct b_goal *zp = vary->base->zv + zi;
 430         float r[3];
 431
 432         r[0] = ball_p[0] - zp->p[0];
 433         r[1] = ball_p[2] - zp->p[2];
 434         r[2] = 0;
 435
 436         if (v_len(r) + ball_r < zp->r &&
 437             ball_p[1] > zp->p[1] &&
 438             ball_p[1] < zp->p[1] + GOAL_HEIGHT / 2)
 439         {
 440             p[0] = zp->p[0];
 441             p[1] = zp->p[1];
 442             p[2] = zp->p[2];
 443
 444             return zp;
 445         }
 446     }
 447     return NULL;
 448 }
 449
 450 /*
 451  * Test for a ball entering a teleporter.
 452  */
 453 int sol_jump_test(struct s_vary *vary, float *p, int ui)
 454 {
 455     const float *ball_p = vary->uv[ui].p;
 456     const float  ball_r = vary->uv[ui].r;
 457     int ji, touch = 0;
 458
 459     for (ji = 0; ji < vary->base->jc; ji++)
 460     {
 461         struct b_jump *jp = vary->base->jv + ji;
 462         float d, r[3];
 463
 464         r[0] = ball_p[0] - jp->p[0];
 465         r[1] = ball_p[2] - jp->p[2];
 466         r[2] = 0;
 467
 468         /* Distance of the far side from the edge of the halo. */
 469
 470         d = v_len(r) + ball_r - jp->r;
 471
 472         /*
 473          * The "touch"  distance, which  must be cleared  before being
 474          * able to trigger a  teleporter, is the ball's radius.  (This
 475          * is different from switches.)
 476          */
 477
 478         if (d <= ball_r &&
 479             ball_p[1] > jp->p[1] &&
 480             ball_p[1] < jp->p[1] + JUMP_HEIGHT / 2)
 481         {
 482             touch = 1;
 483
 484             if (d <= 0.0f)
 485             {
 486                 p[0] = jp->q[0] + (ball_p[0] - jp->p[0]);
 487                 p[1] = jp->q[1] + (ball_p[1] - jp->p[1]);
 488                 p[2] = jp->q[2] + (ball_p[2] - jp->p[2]);
 489
 490                 return JUMP_INSIDE;
 491             }
 492         }
 493     }
 494     return touch ? JUMP_TOUCH : JUMP_OUTSIDE;
 495 }
 496
 497 /*
 498  * Test for a ball entering a switch.
 499  */
 500 int sol_swch_test(struct s_vary *vary, int ui)
 501 {
 502     const float *ball_p = vary->uv[ui].p;
 503     const float  ball_r = vary->uv[ui].r;
 504
 505     union cmd cmd;
 506
 507     int xi, rc = SWCH_OUTSIDE;
 508
 509     for (xi = 0; xi < vary->xc; xi++)
 510     {
 511         struct v_swch *xp = vary->xv + xi;
 512
 513         /* FIXME enter/exit events don't work for timed switches */
 514
 515         if (xp->base->t == 0 || xp->f == xp->base->f)
 516         {
 517             float d, r[3];
 518
 519             r[0] = ball_p[0] - xp->base->p[0];
 520             r[1] = ball_p[2] - xp->base->p[2];
 521             r[2] = 0;
 522
 523             /* Distance of the far side from the edge of the halo. */
 524
 525             d = v_len(r) + ball_r - xp->base->r;
 526
 527             /*
 528              * The  "touch"  distance, which  must  be cleared  before
 529              * being able to trigger a switch, is the ball's diameter.
 530              * (This is different from teleporters.)
 531              */
 532
 533             if (d <= ball_r * 2 &&
 534                 ball_p[1] > xp->base->p[1] &&
 535                 ball_p[1] < xp->base->p[1] + SWCH_HEIGHT / 2)
 536             {
 537                 if (!xp->e && d <= 0.0f)
 538                 {
 539                     /* The ball enters. */
 540
 541                     if (xp->base->tm == 0)
 542                     {
 543                         xp->e = 1;
 544
 545                         cmd.type = CMD_SWCH_ENTER;
 546                         cmd.swchenter.xi = xi;
 547                         sol_cmd_enq(&cmd);
 548                     }
 549
 550                     /* Toggle the state, update the path. */
 551
 552                     xp->f = xp->f ? 0 : 1;
 553
 554                     cmd.type = CMD_SWCH_TOGGLE;
 555                     cmd.swchtoggle.xi = xi;
 556                     sol_cmd_enq(&cmd);
 557
 558                     sol_path_loop(vary, xp->base->pi, xp->f);
 559
 560                     /* It toggled to non-default state, start the timer. */
 561
 562                     if (xp->f != xp->base->f)
 563                     {
 564                         xp->t = 0.0f;
 565                         xp->tm = 0;
 566                     }
 567
 568                     /* If visible, set the result. */
 569
 570                     if (!xp->base->i)
 571                         rc = SWCH_INSIDE;
 572                 }
 573             }
 574
 575             /* The ball exits. */
 576
 577             else if (xp->e)
 578             {
 579                 xp->e = 0;
 580
 581                 cmd.type = CMD_SWCH_EXIT;
 582                 cmd.swchexit.xi = xi;
 583                 sol_cmd_enq(&cmd);
 584             }
 585         }
 586     }
 587     return rc;
 588 }
 589
 590 /*---------------------------------------------------------------------------*/