source/blender/blenlib/intern/math_vector_inline.c

   1 /**
   2  * $Id$
   3  *
   4  * ***** BEGIN GPL LICENSE BLOCK *****
   5  *
   6  * This program is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU General Public License
   8  * as published by the Free Software Foundation; either version 2
   9  * of the License, or (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software Foundation,
  18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  19  *
  20  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
  21  * All rights reserved.
  22
  23  * The Original Code is: some of this file.
  24  *
  25  * ***** END GPL LICENSE BLOCK *****
  26  * */
  27
  28 #include "BLI_math.h"
  29
  30 #ifndef BLI_MATH_VECTOR_INLINE
  31 #define BLI_MATH_VECTOR_INLINE
  32
  33 /********************************** Init *************************************/
  34
  35 MINLINE void zero_v2(float r[2])
  36 {
  37         r[0]= 0.0f;
  38         r[1]= 0.0f;
  39 }
  40
  41 MINLINE void zero_v3(float r[3])
  42 {
  43         r[0]= 0.0f;
  44         r[1]= 0.0f;
  45         r[2]= 0.0f;
  46 }
  47
  48 MINLINE void zero_v4(float r[4])
  49 {
  50         r[0]= 0.0f;
  51         r[1]= 0.0f;
  52         r[2]= 0.0f;
  53         r[3]= 0.0f;
  54 }
  55
  56 MINLINE void copy_v2_v2(float r[2], const float a[2])
  57 {
  58         r[0]= a[0];
  59         r[1]= a[1];
  60 }
  61
  62 MINLINE void copy_v3_v3(float r[3], const float a[3])
  63 {
  64         r[0]= a[0];
  65         r[1]= a[1];
  66         r[2]= a[2];
  67 }
  68
  69 MINLINE void copy_v4_v4(float r[4], const float a[4])
  70 {
  71         r[0]= a[0];
  72         r[1]= a[1];
  73         r[2]= a[2];
  74         r[3]= a[3];
  75 }
  76
  77 MINLINE void swap_v2_v2(float a[2], float b[2])
  78 {
  79         SWAP(float, a[0], b[0]);
  80         SWAP(float, a[1], b[1]);
  81 }
  82
  83 MINLINE void swap_v3_v3(float a[3], float b[3])
  84 {
  85         SWAP(float, a[0], b[0]);
  86         SWAP(float, a[1], b[1]);
  87         SWAP(float, a[2], b[2]);
  88 }
  89
  90 MINLINE void swap_v4_v4(float a[4], float b[4])
  91 {
  92         SWAP(float, a[0], b[0]);
  93         SWAP(float, a[1], b[1]);
  94         SWAP(float, a[2], b[2]);
  95         SWAP(float, a[3], b[3]);
  96 }
  97
  98 /********************************* Arithmetic ********************************/
  99
 100 MINLINE void add_v2_v2(float *r, const float *a)
 101 {
 102         r[0] += a[0];
 103         r[1] += a[1];
 104 }
 105
 106 MINLINE void add_v2_v2v2(float *r, const float *a, const float *b)
 107 {
 108         r[0]= a[0] + b[0];
 109         r[1]= a[1] + b[1];
 110 }
 111
 112 MINLINE void add_v3_v3(float *r, const float *a)
 113 {
 114         r[0] += a[0];
 115         r[1] += a[1];
 116         r[2] += a[2];
 117 }
 118
 119 MINLINE void add_v3_v3v3(float *r, const float *a, const float *b)
 120 {
 121         r[0]= a[0] + b[0];
 122         r[1]= a[1] + b[1];
 123         r[2]= a[2] + b[2];
 124 }
 125
 126 MINLINE void sub_v2_v2(float *r, const float *a)
 127 {
 128         r[0] -= a[0];
 129         r[1] -= a[1];
 130 }
 131
 132 MINLINE void sub_v2_v2v2(float *r, const float *a, const float *b)
 133 {
 134         r[0]= a[0] - b[0];
 135         r[1]= a[1] - b[1];
 136 }
 137
 138 MINLINE void sub_v3_v3(float *r, const float *a)
 139 {
 140         r[0] -= a[0];
 141         r[1] -= a[1];
 142         r[2] -= a[2];
 143 }
 144
 145 MINLINE void sub_v3_v3v3(float r[3], const float a[3], const float b[3])
 146 {
 147         r[0]= a[0] - b[0];
 148         r[1]= a[1] - b[1];
 149         r[2]= a[2] - b[2];
 150 }
 151
 152 MINLINE void mul_v2_fl(float *v1, float f)
 153 {
 154         v1[0]*= f;
 155         v1[1]*= f;
 156 }
 157
 158 MINLINE void mul_v2_v2fl(float r[2], const float a[2], float f)
 159 {
 160         r[0]= a[0]*f;
 161         r[1]= a[1]*f;
 162 }
 163
 164 MINLINE void mul_v3_fl(float r[3], float f)
 165 {
 166         r[0] *= f;
 167         r[1] *= f;
 168         r[2] *= f;
 169 }
 170
 171 MINLINE void mul_v3_v3fl(float r[3], const float a[3], float f)
 172 {
 173         r[0]= a[0]*f;
 174         r[1]= a[1]*f;
 175         r[2]= a[2]*f;
 176 }
 177
 178 MINLINE void mul_v2_v2(float r[2], const float a[2])
 179 {
 180         r[0] *= a[0];
 181         r[1] *= a[1];
 182 }
 183
 184 MINLINE void mul_v3_v3(float r[3], const float a[3])
 185 {
 186         r[0] *= a[0];
 187         r[1] *= a[1];
 188         r[2] *= a[2];
 189 }
 190
 191 MINLINE void mul_v4_fl(float r[4], float f)
 192 {
 193         r[0]*= f;
 194         r[1]*= f;
 195         r[2]*= f;
 196         r[3]*= f;
 197 }
 198
 199 MINLINE void madd_v2_v2fl(float r[2], const float a[2], float f)
 200 {
 201         r[0] += a[0]*f;
 202         r[1] += a[1]*f;
 203 }
 204
 205 MINLINE void madd_v3_v3fl(float r[3], const float a[3], float f)
 206 {
 207         r[0] += a[0]*f;
 208         r[1] += a[1]*f;
 209         r[2] += a[2]*f;
 210 }
 211
 212 MINLINE void madd_v3_v3v3(float r[3], const float a[3], const float b[3])
 213 {
 214         r[0] += a[0]*b[0];
 215         r[1] += a[1]*b[1];
 216         r[2] += a[2]*b[2];
 217 }
 218
 219 MINLINE void madd_v2_v2v2fl(float r[2], const float a[2], const float b[2], float f)
 220 {
 221         r[0] = a[0] + b[0]*f;
 222         r[1] = a[1] + b[1]*f;
 223 }
 224
 225 MINLINE void madd_v3_v3v3fl(float r[3], const float a[3], const float b[3], float f)
 226 {
 227         r[0] = a[0] + b[0]*f;
 228         r[1] = a[1] + b[1]*f;
 229         r[2] = a[2] + b[2]*f;
 230 }
 231
 232 MINLINE void madd_v3_v3v3v3(float r[3], const float a[3], const float b[3], const float c[3])
 233 {
 234         r[0] = a[0] + b[0]*c[0];
 235         r[1] = a[1] + b[1]*c[1];
 236         r[2] = a[2] + b[2]*c[2];
 237 }
 238
 239 MINLINE void madd_v4_v4fl(float r[4], const float a[4], float f)
 240 {
 241         r[0] += a[0]*f;
 242         r[1] += a[1]*f;
 243         r[2] += a[2]*f;
 244         r[3] += a[3]*f;
 245 }
 246
 247 MINLINE void mul_v3_v3v3(float *v, const float *v1, const float *v2)
 248 {
 249         v[0] = v1[0] * v2[0];
 250         v[1] = v1[1] * v2[1];
 251         v[2] = v1[2] * v2[2];
 252 }
 253
 254 MINLINE void negate_v3(float r[3])
 255 {
 256         r[0]= -r[0];
 257         r[1]= -r[1];
 258         r[2]= -r[2];
 259 }
 260
 261 MINLINE void negate_v3_v3(float r[3], const float a[3])
 262 {
 263         r[0]= -a[0];
 264         r[1]= -a[1];
 265         r[2]= -a[2];
 266 }
 267
 268 MINLINE float dot_v2v2(const float a[2], const float b[2])
 269 {
 270         return a[0]*b[0] + a[1]*b[1];
 271 }
 272
 273 MINLINE float dot_v3v3(const float a[3], const float b[3])
 274 {
 275         return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
 276 }
 277
 278 MINLINE float cross_v2v2(const float a[2], const float b[2])
 279 {
 280          return a[0]*b[1] - a[1]*b[0];
 281 }
 282
 283 MINLINE void cross_v3_v3v3(float r[3], const float a[3], const float b[3])
 284 {
 285         r[0]= a[1]*b[2] - a[2]*b[1];
 286         r[1]= a[2]*b[0] - a[0]*b[2];
 287         r[2]= a[0]*b[1] - a[1]*b[0];
 288 }
 289
 290 MINLINE void star_m3_v3(float mat[][3], float *vec)
 291 {
 292         mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
 293         mat[0][1]= -vec[2];
 294         mat[0][2]= vec[1];
 295         mat[1][0]= vec[2];
 296         mat[1][2]= -vec[0];
 297         mat[2][0]= -vec[1];
 298         mat[2][1]= vec[0];
 299 }
 300
 301 /*********************************** Length **********************************/
 302
 303 MINLINE float len_v2(const float v[2])
 304 {
 305         return (float)sqrtf(v[0]*v[0] + v[1]*v[1]);
 306 }
 307
 308 MINLINE float len_v2v2(const float v1[2], const float v2[2])
 309 {
 310         float x, y;
 311
 312         x = v1[0]-v2[0];
 313         y = v1[1]-v2[1];
 314         return (float)sqrtf(x*x+y*y);
 315 }
 316
 317 MINLINE float len_v3(const float a[3])
 318 {
 319         return sqrtf(dot_v3v3(a, a));
 320 }
 321
 322 MINLINE float len_squared_v2v2(const float a[3], const float b[3])
 323 {
 324         float d[2];
 325
 326         sub_v2_v2v2(d, b, a);
 327         return dot_v2v2(d, d);
 328 }
 329
 330 MINLINE float len_v3v3(const float a[3], const float b[3])
 331 {
 332         float d[3];
 333
 334         sub_v3_v3v3(d, b, a);
 335         return len_v3(d);
 336 }
 337
 338 MINLINE float len_squared_v3v3(const float a[3], const float b[3])
 339 {
 340         float d[3];
 341
 342         sub_v3_v3v3(d, b, a);
 343         return dot_v3v3(d, d);
 344 }
 345
 346 MINLINE float normalize_v2_v2(float r[2], const float a[2])
 347 {
 348         float d= dot_v2v2(a, a);
 349
 350         if(d > 1.0e-35f) {
 351                 d= sqrtf(d);
 352                 mul_v2_v2fl(r, a, 1.0f/d);
 353         } else {
 354                 zero_v2(r);
 355                 d= 0.0f;
 356         }
 357
 358         return d;
 359 }
 360
 361 MINLINE float normalize_v2(float n[2])
 362 {
 363         return normalize_v2_v2(n, n);
 364 }
 365
 366 MINLINE float normalize_v3_v3(float r[3], const float a[3])
 367 {
 368         float d= dot_v3v3(a, a);
 369
 370         /* a larger value causes normalize errors in a
 371            scaled down models with camera xtreme close */
 372         if(d > 1.0e-35f) {
 373                 d= sqrtf(d);
 374                 mul_v3_v3fl(r, a, 1.0f/d);
 375         }
 376         else {
 377                 zero_v3(r);
 378                 d= 0.0f;
 379         }
 380
 381         return d;
 382 }
 383
 384 MINLINE float normalize_v3(float n[3])
 385 {
 386         return normalize_v3_v3(n, n);
 387 }
 388
 389 MINLINE void normal_short_to_float_v3(float *out, const short *in)
 390 {
 391         out[0] = in[0]*(1.0f/32767.0f);
 392         out[1] = in[1]*(1.0f/32767.0f);
 393         out[2] = in[2]*(1.0f/32767.0f);
 394 }
 395
 396 MINLINE void normal_float_to_short_v3(short *out, const float *in)
 397 {
 398         out[0] = (short)(in[0]*32767.0f);
 399         out[1] = (short)(in[1]*32767.0f);
 400         out[2] = (short)(in[2]*32767.0f);
 401 }
 402
 403 /********************************* Comparison ********************************/
 404
 405 MINLINE int is_zero_v3(float *v)
 406 {
 407         return (v[0] == 0 && v[1] == 0 && v[2] == 0);
 408 }
 409
 410 MINLINE int is_one_v3(float *v)
 411 {
 412         return (v[0] == 1 && v[1] == 1 && v[2] == 1);
 413 }
 414
 415 MINLINE int equals_v3v3(float *v1, float *v2)
 416 {
 417         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
 418 }
 419
 420 MINLINE int equals_v4v4(float *v1, float *v2)
 421 {
 422         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]) && (v1[3]==v2[3]));
 423 }
 424
 425 MINLINE int compare_v3v3(float *v1, float *v2, float limit)
 426 {
 427         if(fabs(v1[0]-v2[0])<limit)
 428                 if(fabs(v1[1]-v2[1])<limit)
 429                         if(fabs(v1[2]-v2[2])<limit)
 430                                 return 1;
 431
 432         return 0;
 433 }
 434
 435 MINLINE int compare_len_v3v3(float *v1, float *v2, float limit)
 436 {
 437         float x,y,z;
 438
 439         x=v1[0]-v2[0];
 440         y=v1[1]-v2[1];
 441         z=v1[2]-v2[2];
 442
 443         return ((x*x + y*y + z*z) < (limit*limit));
 444 }
 445
 446 MINLINE int compare_v4v4(float *v1, float *v2, float limit)
 447 {
 448         if(fabs(v1[0]-v2[0])<limit)
 449                 if(fabs(v1[1]-v2[1])<limit)
 450                         if(fabs(v1[2]-v2[2])<limit)
 451                                 if(fabs(v1[3]-v2[3])<limit)
 452                                         return 1;
 453
 454         return 0;
 455 }
 456
 457 #endif /* BLI_MATH_VECTOR_INLINE */
 458