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

   1 /*
   2  * Copyright (C) 2003 Robert Kooima
   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 #include <stdio.h>
  16 #include <math.h>
  17
  18 #include "vec3.h"
  19
  20 #define A 10
  21 #define B 11
  22 #define C 12
  23 #define D 13
  24 #define E 14
  25 #define F 15
  26
  27 #define TINY 1e-5
  28
  29 /*---------------------------------------------------------------------------*/
  30
  31 void v_nrm(float *n, const float *v)
  32 {
  33     float d = v_len(v);
  34
  35     if (d == 0.0f)
  36     {
  37         n[0] = 0.0f;
  38         n[1] = 0.0f;
  39         n[2] = 0.0f;
  40     }
  41     else
  42     {
  43         n[0] = v[0] / d;
  44         n[1] = v[1] / d;
  45         n[2] = v[2] / d;
  46     }
  47 }
  48
  49 void v_crs(float *u, const float *v, const float *w)
  50 {
  51     u[0] = v[1] * w[2] - v[2] * w[1];
  52     u[1] = v[2] * w[0] - v[0] * w[2];
  53     u[2] = v[0] * w[1] - v[1] * w[0];
  54 }
  55
  56 /*---------------------------------------------------------------------------*/
  57
  58 void m_cpy(float *M, const float *N)
  59 {
  60     M[0] = N[0]; M[1] = N[1]; M[2] = N[2]; M[3] = N[3];
  61     M[4] = N[4]; M[5] = N[5]; M[6] = N[6]; M[7] = N[7];
  62     M[8] = N[8]; M[9] = N[9]; M[A] = N[A]; M[B] = N[B];
  63     M[C] = N[C]; M[D] = N[D]; M[E] = N[E]; M[F] = N[F];
  64 }
  65
  66 void m_xps(float *M, const float *N)
  67 {
  68     M[0] = N[0]; M[1] = N[4]; M[2] = N[8]; M[3] = N[C];
  69     M[4] = N[1]; M[5] = N[5]; M[6] = N[9]; M[7] = N[D];
  70     M[8] = N[2]; M[9] = N[6]; M[A] = N[A]; M[B] = N[E];
  71     M[C] = N[3]; M[D] = N[7]; M[E] = N[B]; M[F] = N[F];
  72 }
  73
  74 int  m_inv(float *I, const float *N)
  75 {
  76     double T[16], M[16];
  77     double d;
  78     int i;
  79
  80     for (i = 0; i < 16; i++)
  81         M[i] = N[i];
  82
  83     T[0] = +(M[5] * (M[A] * M[F] - M[B] * M[E]) -
  84              M[9] * (M[6] * M[F] - M[7] * M[E]) +
  85              M[D] * (M[6] * M[B] - M[7] * M[A]));
  86     T[1] = -(M[4] * (M[A] * M[F] - M[B] * M[E]) -
  87              M[8] * (M[6] * M[F] - M[7] * M[E]) +
  88              M[C] * (M[6] * M[B] - M[7] * M[A]));
  89     T[2] = +(M[4] * (M[9] * M[F] - M[B] * M[D]) -
  90              M[8] * (M[5] * M[F] - M[7] * M[D]) +
  91              M[C] * (M[5] * M[B] - M[7] * M[9]));
  92     T[3] = -(M[4] * (M[9] * M[E] - M[A] * M[D]) -
  93              M[8] * (M[5] * M[E] - M[6] * M[D]) +
  94              M[C] * (M[5] * M[A] - M[6] * M[9]));
  95
  96     T[4] = -(M[1] * (M[A] * M[F] - M[B] * M[E]) -
  97              M[9] * (M[2] * M[F] - M[3] * M[E]) +
  98              M[D] * (M[2] * M[B] - M[3] * M[A]));
  99     T[5] = +(M[0] * (M[A] * M[F] - M[B] * M[E]) -
 100              M[8] * (M[2] * M[F] - M[3] * M[E]) +
 101              M[C] * (M[2] * M[B] - M[3] * M[A]));
 102     T[6] = -(M[0] * (M[9] * M[F] - M[B] * M[D]) -
 103              M[8] * (M[1] * M[F] - M[3] * M[D]) +
 104              M[C] * (M[1] * M[B] - M[3] * M[9]));
 105     T[7] = +(M[0] * (M[9] * M[E] - M[A] * M[D]) -
 106              M[8] * (M[1] * M[E] - M[2] * M[D]) +
 107              M[C] * (M[1] * M[A] - M[2] * M[9]));
 108
 109     T[8] = +(M[1] * (M[6] * M[F] - M[7] * M[E]) -
 110              M[5] * (M[2] * M[F] - M[3] * M[E]) +
 111              M[D] * (M[2] * M[7] - M[3] * M[6]));
 112     T[9] = -(M[0] * (M[6] * M[F] - M[7] * M[E]) -
 113              M[4] * (M[2] * M[F] - M[3] * M[E]) +
 114              M[C] * (M[2] * M[7] - M[3] * M[6]));
 115     T[A] = +(M[0] * (M[5] * M[F] - M[7] * M[D]) -
 116              M[4] * (M[1] * M[F] - M[3] * M[D]) +
 117              M[C] * (M[1] * M[7] - M[3] * M[5]));
 118     T[B] = -(M[0] * (M[5] * M[E] - M[6] * M[D]) -
 119              M[4] * (M[1] * M[E] - M[2] * M[D]) +
 120              M[C] * (M[1] * M[6] - M[2] * M[5]));
 121
 122     T[C] = -(M[1] * (M[6] * M[B] - M[7] * M[A]) -
 123              M[5] * (M[2] * M[B] - M[3] * M[A]) +
 124              M[9] * (M[2] * M[7] - M[3] * M[6]));
 125     T[D] = +(M[0] * (M[6] * M[B] - M[7] * M[A]) -
 126              M[4] * (M[2] * M[B] - M[3] * M[A]) +
 127              M[8] * (M[2] * M[7] - M[3] * M[6]));
 128     T[E] = -(M[0] * (M[5] * M[B] - M[7] * M[9]) -
 129              M[4] * (M[1] * M[B] - M[3] * M[9]) +
 130              M[8] * (M[1] * M[7] - M[3] * M[5]));
 131     T[F] = +(M[0] * (M[5] * M[A] - M[6] * M[9]) -
 132              M[4] * (M[1] * M[A] - M[2] * M[9]) +
 133              M[8] * (M[1] * M[6] - M[2] * M[5]));
 134
 135     d = M[0] * T[0] + M[4] * T[4] + M[8] * T[8] + M[C] * T[C];
 136
 137     if (fabs(d) > TINY)
 138     {
 139         I[0] = T[0] / d;
 140         I[1] = T[4] / d;
 141         I[2] = T[8] / d;
 142         I[3] = T[C] / d;
 143         I[4] = T[1] / d;
 144         I[5] = T[5] / d;
 145         I[6] = T[9] / d;
 146         I[7] = T[D] / d;
 147         I[8] = T[2] / d;
 148         I[9] = T[6] / d;
 149         I[A] = T[A] / d;
 150         I[B] = T[E] / d;
 151         I[C] = T[3] / d;
 152         I[D] = T[7] / d;
 153         I[E] = T[B] / d;
 154         I[F] = T[F] / d;
 155
 156         return 1;
 157     }
 158     return 0;
 159 }
 160
 161 /*---------------------------------------------------------------------------*/
 162
 163 void m_ident(float *M)
 164 {
 165     M[0] = 1.f; M[4] = 0.f; M[8] = 0.f; M[C] = 0.f;
 166     M[1] = 0.f; M[5] = 1.f; M[9] = 0.f; M[D] = 0.f;
 167     M[2] = 0.f; M[6] = 0.f; M[A] = 1.f; M[E] = 0.f;
 168     M[3] = 0.f; M[7] = 0.f; M[B] = 0.f; M[F] = 1.f;
 169 }
 170
 171 void m_basis(float *M,
 172              const float e0[3],
 173              const float e1[3],
 174              const float e2[3])
 175 {
 176     M[0] = e0[0]; M[4] = e1[0]; M[8] = e2[0]; M[C] = 0.f;
 177     M[1] = e0[1]; M[5] = e1[1]; M[9] = e2[1]; M[D] = 0.f;
 178     M[2] = e0[2]; M[6] = e1[2]; M[A] = e2[2]; M[E] = 0.f;
 179     M[3] =   0.f; M[7] =   0.f; M[B] =   0.f; M[F] = 1.f;
 180 }
 181
 182 /*---------------------------------------------------------------------------*/
 183
 184 void m_xlt(float *M, const float *v)
 185 {
 186     M[0] = 1.f; M[4] = 0.f; M[8] = 0.f; M[C] = v[0];
 187     M[1] = 0.f; M[5] = 1.f; M[9] = 0.f; M[D] = v[1];
 188     M[2] = 0.f; M[6] = 0.f; M[A] = 1.f; M[E] = v[2];
 189     M[3] = 0.f; M[7] = 0.f; M[B] = 0.f; M[F] =  1.f;
 190 }
 191
 192 void m_scl(float *M, const float *v)
 193 {
 194     M[0] = v[0]; M[4] =  0.f; M[8] =  0.f; M[C] = 0.f;
 195     M[1] =  0.f; M[5] = v[1]; M[9] =  0.f; M[D] = 0.f;
 196     M[2] =  0.f; M[6] =  0.f; M[A] = v[2]; M[E] = 0.f;
 197     M[3] =  0.f; M[7] =  0.f; M[B] =  0.f; M[F] = 1.f;
 198 }
 199
 200 void m_rot(float *M, const float *v, float a)
 201 {
 202     float u[3];
 203     float U[16];
 204     float S[16];
 205
 206     const float s = fsinf(a);
 207     const float c = fcosf(a);
 208
 209     v_nrm(u, v);
 210
 211     U[0] = u[0] * u[0]; U[4] = u[0] * u[1]; U[8] = u[0] * u[2];
 212     U[1] = u[1] * u[0]; U[5] = u[1] * u[1]; U[9] = u[1] * u[2];
 213     U[2] = u[2] * u[0]; U[6] = u[2] * u[1]; U[A] = u[2] * u[2];
 214
 215     S[0] =   0.f; S[4] = -u[2]; S[8] =  u[1];
 216     S[1] =  u[2]; S[5] =   0.f; S[9] = -u[0];
 217     S[2] = -u[1]; S[6] =  u[0]; S[A] =   0.f;
 218
 219     M[0] = U[0] + c * (1.f - U[0]) + s * S[0];
 220     M[1] = U[1] + c * (0.f - U[1]) + s * S[1];
 221     M[2] = U[2] + c * (0.f - U[2]) + s * S[2];
 222     M[3] = 0.f;
 223     M[4] = U[4] + c * (0.f - U[4]) + s * S[4];
 224     M[5] = U[5] + c * (1.f - U[5]) + s * S[5];
 225     M[6] = U[6] + c * (0.f - U[6]) + s * S[6];
 226     M[7] = 0.f;
 227     M[8] = U[8] + c * (0.f - U[8]) + s * S[8];
 228     M[9] = U[9] + c * (0.f - U[9]) + s * S[9];
 229     M[A] = U[A] + c * (1.f - U[A]) + s * S[A];
 230     M[B] = 0.f;
 231     M[C] = 0.f;
 232     M[D] = 0.f;
 233     M[E] = 0.f;
 234     M[F] = 1.f;
 235 }
 236
 237 /*---------------------------------------------------------------------------*/
 238
 239 void m_mult(float *M, const float *N, const float *O)
 240 {
 241     M[0] = N[0] * O[0] + N[4] * O[1] + N[8] * O[2] + N[C] * O[3];
 242     M[1] = N[1] * O[0] + N[5] * O[1] + N[9] * O[2] + N[D] * O[3];
 243     M[2] = N[2] * O[0] + N[6] * O[1] + N[A] * O[2] + N[E] * O[3];
 244     M[3] = N[3] * O[0] + N[7] * O[1] + N[B] * O[2] + N[F] * O[3];
 245
 246     M[4] = N[0] * O[4] + N[4] * O[5] + N[8] * O[6] + N[C] * O[7];
 247     M[5] = N[1] * O[4] + N[5] * O[5] + N[9] * O[6] + N[D] * O[7];
 248     M[6] = N[2] * O[4] + N[6] * O[5] + N[A] * O[6] + N[E] * O[7];
 249     M[7] = N[3] * O[4] + N[7] * O[5] + N[B] * O[6] + N[F] * O[7];
 250
 251     M[8] = N[0] * O[8] + N[4] * O[9] + N[8] * O[A] + N[C] * O[B];
 252     M[9] = N[1] * O[8] + N[5] * O[9] + N[9] * O[A] + N[D] * O[B];
 253     M[A] = N[2] * O[8] + N[6] * O[9] + N[A] * O[A] + N[E] * O[B];
 254     M[B] = N[3] * O[8] + N[7] * O[9] + N[B] * O[A] + N[F] * O[B];
 255
 256     M[C] = N[0] * O[C] + N[4] * O[D] + N[8] * O[E] + N[C] * O[F];
 257     M[D] = N[1] * O[C] + N[5] * O[D] + N[9] * O[E] + N[D] * O[F];
 258     M[E] = N[2] * O[C] + N[6] * O[D] + N[A] * O[E] + N[E] * O[F];
 259     M[F] = N[3] * O[C] + N[7] * O[D] + N[B] * O[E] + N[F] * O[F];
 260 }
 261
 262 void m_pxfm(float *v, const float *M, const float *w)
 263 {
 264     float d = w[0] * M[3] + w[1] * M[7] + w[2] * M[B] + M[F];
 265
 266     v[0] = (w[0] * M[0] + w[1] * M[4] + w[2] * M[8] + M[C]) / d;
 267     v[1] = (w[0] * M[1] + w[1] * M[5] + w[2] * M[9] + M[D]) / d;
 268     v[2] = (w[0] * M[2] + w[1] * M[6] + w[2] * M[A] + M[E]) / d;
 269 }
 270
 271 void m_vxfm(float *v, const float *M, const float *w)
 272 {
 273     v[0] = (w[0] * M[0] + w[1] * M[4] + w[2] * M[8]);
 274     v[1] = (w[0] * M[1] + w[1] * M[5] + w[2] * M[9]);
 275     v[2] = (w[0] * M[2] + w[1] * M[6] + w[2] * M[A]);
 276 }
 277
 278 /*---------------------------------------------------------------------------*/
 279
 280 void m_view(float *M,
 281             const float *c,
 282             const float *p,
 283             const float *u)
 284 {
 285     float x[3];
 286     float y[3];
 287     float z[3];
 288
 289     v_sub(z, p, c);
 290     v_nrm(z, z);
 291     v_crs(x, u, z);
 292     v_nrm(x, x);
 293     v_crs(y, z, x);
 294
 295     m_basis(M, x, y, z);
 296 }
 297
 298 /*---------------------------------------------------------------------------*/
 299
 300 void q_as_axisangle(const float q[4], float u[3], float *a)
 301 {
 302     *a = 2.0f * facosf(q[0]);
 303     v_nrm(u, q + 1);
 304 }
 305
 306 void q_by_axisangle(float q[4], const float u[3], float a)
 307 {
 308     float c = fcosf(a * 0.5f);
 309     float s = fsinf(a * 0.5f);
 310     float n[3];
 311
 312     v_nrm(n, u);
 313
 314     q[0] =        c;
 315     q[1] = n[0] * s;
 316     q[2] = n[1] * s;
 317     q[3] = n[2] * s;
 318 }
 319
 320 void q_nrm(float q[4], const float r[4])
 321 {
 322     float d = q_len(r);
 323
 324     if (d == 0.0f)
 325     {
 326         q[0] = 1.0f;
 327         q[1] = 0.0f;
 328         q[2] = 0.0f;
 329         q[3] = 0.0f;
 330     }
 331     else
 332     {
 333         q[0] = r[0] / d;
 334         q[1] = r[1] / d;
 335         q[2] = r[2] / d;
 336         q[3] = r[3] / d;
 337     }
 338 }
 339
 340 void q_mul(float q[4], const float a[4], const float b[4])
 341 {
 342     q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
 343     q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
 344     q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
 345     q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
 346 }
 347
 348 void q_rot(float v[3], const float r[4], const float w[3])
 349 {
 350     float a[4], b[4], c[4];
 351
 352     a[0] = 0.0f;
 353     a[1] = w[0];
 354     a[2] = w[1];
 355     a[3] = w[2];
 356
 357     q_mul(b, r, a);
 358
 359     a[0] =  r[0];
 360     a[1] = -r[1];
 361     a[2] = -r[2];
 362     a[3] = -r[3];
 363
 364     q_mul(c, b, a);
 365
 366     v[0] = c[1];
 367     v[1] = c[2];
 368     v[2] = c[3];
 369 }
 370
 371 void q_euler(float v[3], const float q[4])
 372 {
 373     float m11 = (2 * q[0] * q[0]) + (2 * q[1] * q[1]) - 1;
 374     float m12 = (2 * q[1] * q[2]) + (2 * q[0] * q[3]);
 375     float m13 = (2 * q[1] * q[3]) - (2 * q[0] * q[2]);
 376     float m23 = (2 * q[2] * q[3]) + (2 * q[0] * q[1]);
 377     float m33 = (2 * q[0] * q[0]) + (2 * q[3] * q[3]) - 1;
 378
 379     v[0] = fatan2f(m12, m11);
 380     v[1] = fasinf(-m13);
 381     v[2] = fatan2f(m23, m33);
 382 }
 383
 384 /*
 385  * Spherical linear interpolation
 386  */
 387 void q_slerp(float q[4], const float a[4], const float b[4], float t)
 388 {
 389     float c, r, s, u, v;
 390     int i = +1;
 391
 392     if (t <= 0.0f)
 393     {
 394         q_cpy(q, a);
 395         return;
 396     }
 397
 398     if (1.0f <= t)
 399     {
 400         q_cpy(q, b);
 401         return;
 402     }
 403
 404     /*
 405      * a . b = |a||b| cos A
 406      * |a| = |b| = 1
 407      */
 408
 409     c = q_dot(a, b);
 410
 411     /* Ensure the shortest path. */
 412
 413     if (c < 0)
 414     {
 415         c = -c;
 416         i = -1;
 417     }
 418
 419     /* Fall back to normalized lerp for very similar orientations. */
 420
 421     if (1.0f - c < TINY)
 422     {
 423         u = 1.0f - t;
 424         v = t;
 425
 426         i = 1;
 427     }
 428     else
 429     {
 430         r = facosf(c);
 431         s = fsinf(r);
 432         u = fsinf((1.0f - t) * r) / s;
 433         v = fsinf((t)        * r) / s * i;
 434
 435         i = 0;
 436     }
 437
 438     q[0] = a[0] * u + b[0] * v;
 439     q[1] = a[1] * u + b[1] * v;
 440     q[2] = a[2] * u + b[2] * v;
 441     q[3] = a[3] * u + b[3] * v;
 442
 443     if (i) q_nrm(q, q);
 444 }
 445
 446 /*---------------------------------------------------------------------------*/