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     n[0] = v[0] / d;
  36     n[1] = v[1] / d;
  37     n[2] = v[2] / d;
  38 }
  39
  40 void v_crs(float *u, const float *v, const float *w)
  41 {
  42     u[0] = v[1] * w[2] - v[2] * w[1];
  43     u[1] = v[2] * w[0] - v[0] * w[2];
  44     u[2] = v[0] * w[1] - v[1] * w[0];
  45 }
  46
  47 /*---------------------------------------------------------------------------*/
  48
  49 void m_cpy(float *M, const float *N)
  50 {
  51     M[0] = N[0]; M[1] = N[1]; M[2] = N[2]; M[3] = N[3];
  52     M[4] = N[4]; M[5] = N[5]; M[6] = N[6]; M[7] = N[7];
  53     M[8] = N[8]; M[9] = N[9]; M[A] = N[A]; M[B] = N[B];
  54     M[C] = N[C]; M[D] = N[D]; M[E] = N[E]; M[F] = N[F];
  55 }
  56
  57 void m_xps(float *M, const float *N)
  58 {
  59     M[0] = N[0]; M[1] = N[4]; M[2] = N[8]; M[3] = N[C];
  60     M[4] = N[1]; M[5] = N[5]; M[6] = N[9]; M[7] = N[D];
  61     M[8] = N[2]; M[9] = N[6]; M[A] = N[A]; M[B] = N[E];
  62     M[C] = N[3]; M[D] = N[7]; M[E] = N[B]; M[F] = N[F];
  63 }
  64
  65 int  m_inv(float *I, const float *N)
  66 {
  67     double T[16], M[16];
  68     double d;
  69     int i;
  70
  71     for (i = 0; i < 16; i++)
  72         M[i] = N[i];
  73
  74     T[0] = +(M[5] * (M[A] * M[F] - M[B] * M[E]) -
  75              M[9] * (M[6] * M[F] - M[7] * M[E]) +
  76              M[D] * (M[6] * M[B] - M[7] * M[A]));
  77     T[1] = -(M[4] * (M[A] * M[F] - M[B] * M[E]) -
  78              M[8] * (M[6] * M[F] - M[7] * M[E]) +
  79              M[C] * (M[6] * M[B] - M[7] * M[A]));
  80     T[2] = +(M[4] * (M[9] * M[F] - M[B] * M[D]) -
  81              M[8] * (M[5] * M[F] - M[7] * M[D]) +
  82              M[C] * (M[5] * M[B] - M[7] * M[9]));
  83     T[3] = -(M[4] * (M[9] * M[E] - M[A] * M[D]) -
  84              M[8] * (M[5] * M[E] - M[6] * M[D]) +
  85              M[C] * (M[5] * M[A] - M[6] * M[9]));
  86
  87     T[4] = -(M[1] * (M[A] * M[F] - M[B] * M[E]) -
  88              M[9] * (M[2] * M[F] - M[3] * M[E]) +
  89              M[D] * (M[2] * M[B] - M[3] * M[A]));
  90     T[5] = +(M[0] * (M[A] * M[F] - M[B] * M[E]) -
  91              M[8] * (M[2] * M[F] - M[3] * M[E]) +
  92              M[C] * (M[2] * M[B] - M[3] * M[A]));
  93     T[6] = -(M[0] * (M[9] * M[F] - M[B] * M[D]) -
  94              M[8] * (M[1] * M[F] - M[3] * M[D]) +
  95              M[C] * (M[1] * M[B] - M[3] * M[9]));
  96     T[7] = +(M[0] * (M[9] * M[E] - M[A] * M[D]) -
  97              M[8] * (M[1] * M[E] - M[2] * M[D]) +
  98              M[C] * (M[1] * M[A] - M[2] * M[9]));
  99
 100     T[8] = +(M[1] * (M[6] * M[F] - M[7] * M[E]) -
 101              M[5] * (M[2] * M[F] - M[3] * M[E]) +
 102              M[D] * (M[2] * M[7] - M[3] * M[6]));
 103     T[9] = -(M[0] * (M[6] * M[F] - M[7] * M[E]) -
 104              M[4] * (M[2] * M[F] - M[3] * M[E]) +
 105              M[C] * (M[2] * M[7] - M[3] * M[6]));
 106     T[A] = +(M[0] * (M[5] * M[F] - M[7] * M[D]) -
 107              M[4] * (M[1] * M[F] - M[3] * M[D]) +
 108              M[C] * (M[1] * M[7] - M[3] * M[5]));
 109     T[B] = -(M[0] * (M[5] * M[E] - M[6] * M[D]) -
 110              M[4] * (M[1] * M[E] - M[2] * M[D]) +
 111              M[C] * (M[1] * M[6] - M[2] * M[5]));
 112
 113     T[C] = -(M[1] * (M[6] * M[B] - M[7] * M[A]) -
 114              M[5] * (M[2] * M[B] - M[3] * M[A]) +
 115              M[9] * (M[2] * M[7] - M[3] * M[6]));
 116     T[D] = +(M[0] * (M[6] * M[B] - M[7] * M[A]) -
 117              M[4] * (M[2] * M[B] - M[3] * M[A]) +
 118              M[8] * (M[2] * M[7] - M[3] * M[6]));
 119     T[E] = -(M[0] * (M[5] * M[B] - M[7] * M[9]) -
 120              M[4] * (M[1] * M[B] - M[3] * M[9]) +
 121              M[8] * (M[1] * M[7] - M[3] * M[5]));
 122     T[F] = +(M[0] * (M[5] * M[A] - M[6] * M[9]) -
 123              M[4] * (M[1] * M[A] - M[2] * M[9]) +
 124              M[8] * (M[1] * M[6] - M[2] * M[5]));
 125
 126     d = M[0] * T[0] + M[4] * T[4] + M[8] * T[8] + M[C] * T[C];
 127
 128     if (fabs(d) > TINY)
 129     {
 130         I[0] = T[0] / d;
 131         I[1] = T[4] / d;
 132         I[2] = T[8] / d;
 133         I[3] = T[C] / d;
 134         I[4] = T[1] / d;
 135         I[5] = T[5] / d;
 136         I[6] = T[9] / d;
 137         I[7] = T[D] / d;
 138         I[8] = T[2] / d;
 139         I[9] = T[6] / d;
 140         I[A] = T[A] / d;
 141         I[B] = T[E] / d;
 142         I[C] = T[3] / d;
 143         I[D] = T[7] / d;
 144         I[E] = T[B] / d;
 145         I[F] = T[F] / d;
 146
 147         return 1;
 148     }
 149     return 0;
 150 }
 151
 152 /*---------------------------------------------------------------------------*/
 153
 154 void m_ident(float *M)
 155 {
 156     M[0] = 1.f; M[4] = 0.f; M[8] = 0.f; M[C] = 0.f;
 157     M[1] = 0.f; M[5] = 1.f; M[9] = 0.f; M[D] = 0.f;
 158     M[2] = 0.f; M[6] = 0.f; M[A] = 1.f; M[E] = 0.f;
 159     M[3] = 0.f; M[7] = 0.f; M[B] = 0.f; M[F] = 1.f;
 160 }
 161
 162 void m_basis(float *M,
 163              const float e0[3],
 164              const float e1[3],
 165              const float e2[3])
 166 {
 167     M[0] = e0[0]; M[4] = e1[0]; M[8] = e2[0]; M[C] = 0.f;
 168     M[1] = e0[1]; M[5] = e1[1]; M[9] = e2[1]; M[D] = 0.f;
 169     M[2] = e0[2]; M[6] = e1[2]; M[A] = e2[2]; M[E] = 0.f;
 170     M[3] =   0.f; M[7] =   0.f; M[B] =   0.f; M[F] = 1.f;
 171 }
 172
 173 /*---------------------------------------------------------------------------*/
 174
 175 void m_xlt(float *M, const float *v)
 176 {
 177     M[0] = 1.f; M[4] = 0.f; M[8] = 0.f; M[C] = v[0];
 178     M[1] = 0.f; M[5] = 1.f; M[9] = 0.f; M[D] = v[1];
 179     M[2] = 0.f; M[6] = 0.f; M[A] = 1.f; M[E] = v[2];
 180     M[3] = 0.f; M[7] = 0.f; M[B] = 0.f; M[F] =  1.f;
 181 }
 182
 183 void m_scl(float *M, const float *v)
 184 {
 185     M[0] = v[0]; M[4] =  0.f; M[8] =  0.f; M[C] = 0.f;
 186     M[1] =  0.f; M[5] = v[1]; M[9] =  0.f; M[D] = 0.f;
 187     M[2] =  0.f; M[6] =  0.f; M[A] = v[2]; M[E] = 0.f;
 188     M[3] =  0.f; M[7] =  0.f; M[B] =  0.f; M[F] = 1.f;
 189 }
 190
 191 void m_rot(float *M, const float *v, float a)
 192 {
 193     float u[3];
 194     float U[16];
 195     float S[16];
 196
 197     const float s = fsinf(a);
 198     const float c = fcosf(a);
 199
 200     v_nrm(u, v);
 201
 202     U[0] = u[0] * u[0]; U[4] = u[0] * u[1]; U[8] = u[0] * u[2];
 203     U[1] = u[1] * u[0]; U[5] = u[1] * u[1]; U[9] = u[1] * u[2];
 204     U[2] = u[2] * u[0]; U[6] = u[2] * u[1]; U[A] = u[2] * u[2];
 205
 206     S[0] =   0.f; S[4] = -u[2]; S[8] =  u[1];
 207     S[1] =  u[2]; S[5] =   0.f; S[9] = -u[0];
 208     S[2] = -u[1]; S[6] =  u[0]; S[A] =   0.f;
 209
 210     M[0] = U[0] + c * (1.f - U[0]) + s * S[0];
 211     M[1] = U[1] + c * (0.f - U[1]) + s * S[1];
 212     M[2] = U[2] + c * (0.f - U[2]) + s * S[2];
 213     M[3] = 0.f;
 214     M[4] = U[4] + c * (0.f - U[4]) + s * S[4];
 215     M[5] = U[5] + c * (1.f - U[5]) + s * S[5];
 216     M[6] = U[6] + c * (0.f - U[6]) + s * S[6];
 217     M[7] = 0.f;
 218     M[8] = U[8] + c * (0.f - U[8]) + s * S[8];
 219     M[9] = U[9] + c * (0.f - U[9]) + s * S[9];
 220     M[A] = U[A] + c * (1.f - U[A]) + s * S[A];
 221     M[B] = 0.f;
 222     M[C] = 0.f;
 223     M[D] = 0.f;
 224     M[E] = 0.f;
 225     M[F] = 1.f;
 226 }
 227
 228 /*---------------------------------------------------------------------------*/
 229
 230 void m_mult(float *M, const float *N, const float *O)
 231 {
 232     M[0] = N[0] * O[0] + N[4] * O[1] + N[8] * O[2] + N[C] * O[3];
 233     M[1] = N[1] * O[0] + N[5] * O[1] + N[9] * O[2] + N[D] * O[3];
 234     M[2] = N[2] * O[0] + N[6] * O[1] + N[A] * O[2] + N[E] * O[3];
 235     M[3] = N[3] * O[0] + N[7] * O[1] + N[B] * O[2] + N[F] * O[3];
 236
 237     M[4] = N[0] * O[4] + N[4] * O[5] + N[8] * O[6] + N[C] * O[7];
 238     M[5] = N[1] * O[4] + N[5] * O[5] + N[9] * O[6] + N[D] * O[7];
 239     M[6] = N[2] * O[4] + N[6] * O[5] + N[A] * O[6] + N[E] * O[7];
 240     M[7] = N[3] * O[4] + N[7] * O[5] + N[B] * O[6] + N[F] * O[7];
 241
 242     M[8] = N[0] * O[8] + N[4] * O[9] + N[8] * O[A] + N[C] * O[B];
 243     M[9] = N[1] * O[8] + N[5] * O[9] + N[9] * O[A] + N[D] * O[B];
 244     M[A] = N[2] * O[8] + N[6] * O[9] + N[A] * O[A] + N[E] * O[B];
 245     M[B] = N[3] * O[8] + N[7] * O[9] + N[B] * O[A] + N[F] * O[B];
 246
 247     M[C] = N[0] * O[C] + N[4] * O[D] + N[8] * O[E] + N[C] * O[F];
 248     M[D] = N[1] * O[C] + N[5] * O[D] + N[9] * O[E] + N[D] * O[F];
 249     M[E] = N[2] * O[C] + N[6] * O[D] + N[A] * O[E] + N[E] * O[F];
 250     M[F] = N[3] * O[C] + N[7] * O[D] + N[B] * O[E] + N[F] * O[F];
 251 }
 252
 253 void m_pxfm(float *v, const float *M, const float *w)
 254 {
 255     float d = w[0] * M[3] + w[1] * M[7] + w[2] * M[B] + M[F];
 256
 257     v[0] = (w[0] * M[0] + w[1] * M[4] + w[2] * M[8] + M[C]) / d;
 258     v[1] = (w[0] * M[1] + w[1] * M[5] + w[2] * M[9] + M[D]) / d;
 259     v[2] = (w[0] * M[2] + w[1] * M[6] + w[2] * M[A] + M[E]) / d;
 260 }
 261
 262 void m_vxfm(float *v, const float *M, const float *w)
 263 {
 264     v[0] = (w[0] * M[0] + w[1] * M[4] + w[2] * M[8]);
 265     v[1] = (w[0] * M[1] + w[1] * M[5] + w[2] * M[9]);
 266     v[2] = (w[0] * M[2] + w[1] * M[6] + w[2] * M[A]);
 267 }
 268
 269 /*---------------------------------------------------------------------------*/
 270
 271 void m_view(float *M,
 272             const float *c,
 273             const float *p,
 274             const float *u)
 275 {
 276     float x[3];
 277     float y[3];
 278     float z[3];
 279
 280     v_sub(z, p, c);
 281     v_nrm(z, z);
 282     v_crs(x, u, z);
 283     v_nrm(x, x);
 284     v_crs(y, z, x);
 285
 286     m_basis(M, x, y, z);
 287 }
 288
 289 /*---------------------------------------------------------------------------*/