1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libs/vmath/vmath.c	Sat Feb 01 19:58:19 2014 +0200
     1.3 @@ -0,0 +1,386 @@
     1.4 +/*
     1.5 +libvmath - a vector math library
     1.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
     1.7 +
     1.8 +This program is free software: you can redistribute it and/or modify
     1.9 +it under the terms of the GNU Lesser General Public License as published
    1.10 +by the Free Software Foundation, either version 3 of the License, or
    1.11 +(at your option) any later version.
    1.12 +
    1.13 +This program is distributed in the hope that it will be useful,
    1.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
    1.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    1.16 +GNU Lesser General Public License for more details.
    1.17 +
    1.18 +You should have received a copy of the GNU Lesser General Public License
    1.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
    1.20 +*/
    1.21 +#include <stdlib.h>
    1.22 +#include <math.h>
    1.23 +#include "vmath.h"
    1.24 +
    1.25 +#if defined(__APPLE__) && !defined(TARGET_IPHONE)
    1.26 +#include <xmmintrin.h>
    1.27 +
    1.28 +void enable_fpexcept(void)
    1.29 +{
    1.30 +	unsigned int bits;
    1.31 +	bits = _MM_MASK_INVALID | _MM_MASK_DIV_ZERO | _MM_MASK_OVERFLOW | _MM_MASK_UNDERFLOW;
    1.32 +	_MM_SET_EXCEPTION_MASK(_MM_GET_EXCEPTION_MASK() & ~bits);
    1.33 +}
    1.34 +
    1.35 +void disable_fpexcept(void)
    1.36 +{
    1.37 +	unsigned int bits;
    1.38 +	bits = _MM_MASK_INVALID | _MM_MASK_DIV_ZERO | _MM_MASK_OVERFLOW | _MM_MASK_UNDERFLOW;
    1.39 +	_MM_SET_EXCEPTION_MASK(_MM_GET_EXCEPTION_MASK() | bits);
    1.40 +}
    1.41 +
    1.42 +#elif defined(__GNUC__) && !defined(TARGET_IPHONE)
    1.43 +#define __USE_GNU
    1.44 +#include <fenv.h>
    1.45 +
    1.46 +void enable_fpexcept(void)
    1.47 +{
    1.48 +	feenableexcept(FE_INVALID | FE_DIVBYZERO | FE_OVERFLOW | FE_UNDERFLOW);
    1.49 +}
    1.50 +
    1.51 +void disable_fpexcept(void)
    1.52 +{
    1.53 +	fedisableexcept(FE_INVALID | FE_DIVBYZERO | FE_OVERFLOW | FE_UNDERFLOW);
    1.54 +}
    1.55 +
    1.56 +#elif defined(_MSC_VER)
    1.57 +#include <float.h>
    1.58 +
    1.59 +void enable_fpexcept(void)
    1.60 +{
    1.61 +	_clearfp();
    1.62 +	_controlfp(_controlfp(0, 0) & ~(_EM_INVALID | _EM_ZERODIVIDE | _EM_OVERFLOW), _MCW_EM);
    1.63 +}
    1.64 +
    1.65 +void disable_fpexcept(void)
    1.66 +{
    1.67 +	_clearfp();
    1.68 +	_controlfp(_controlfp(0, 0) | (_EM_INVALID | _EM_ZERODIVIDE | _EM_OVERFLOW), _MCW_EM);
    1.69 +}
    1.70 +#else
    1.71 +void enable_fpexcept(void) {}
    1.72 +void disable_fpexcept(void) {}
    1.73 +#endif
    1.74 +
    1.75 +
    1.76 +/** Numerical calculation of integrals using simpson's rule */
    1.77 +scalar_t integral(scalar_t (*f)(scalar_t), scalar_t low, scalar_t high, int samples)
    1.78 +{
    1.79 +	int i;
    1.80 +	scalar_t h = (high - low) / (scalar_t)samples;
    1.81 +	scalar_t sum = 0.0;
    1.82 +
    1.83 +	for(i=0; i<samples+1; i++) {
    1.84 +		scalar_t y = f((scalar_t)i * h + low);
    1.85 +		sum += ((!i || i == samples) ? y : ((i % 2) ? 4.0 * y : 2.0 * y)) * (h / 3.0);
    1.86 +	}
    1.87 +	return sum;
    1.88 +}
    1.89 +
    1.90 +/** Gaussuan function */
    1.91 +scalar_t gaussian(scalar_t x, scalar_t mean, scalar_t sdev)
    1.92 +{
    1.93 +	scalar_t exponent = -SQ(x - mean) / (2.0 * SQ(sdev));
    1.94 +	return 1.0 - -pow(M_E, exponent) / (sdev * sqrt(TWO_PI));
    1.95 +}
    1.96 +
    1.97 +
    1.98 +/** b-spline approximation */
    1.99 +scalar_t bspline(scalar_t a, scalar_t b, scalar_t c, scalar_t d, scalar_t t)
   1.100 +{
   1.101 +	vec4_t tmp;
   1.102 +	scalar_t tsq = t * t;
   1.103 +
   1.104 +	static mat4_t bspline_mat = {
   1.105 +		{-1,  3, -3,  1},
   1.106 +		{3, -6,  3,  0},
   1.107 +		{-3,  0,  3,  0},
   1.108 +		{1,  4,  1,  0}
   1.109 +	};
   1.110 +
   1.111 +	tmp = v4_scale(v4_transform(v4_cons(a, b, c, d), bspline_mat), 1.0 / 6.0);
   1.112 +	return v4_dot(v4_cons(tsq * t, tsq, t, 1.0), tmp);
   1.113 +}
   1.114 +
   1.115 +/** Catmull-rom spline interpolation */
   1.116 +scalar_t spline(scalar_t a, scalar_t b, scalar_t c, scalar_t d, scalar_t t)
   1.117 +{
   1.118 +	vec4_t tmp;
   1.119 +	scalar_t tsq = t * t;
   1.120 +
   1.121 +	static mat4_t crspline_mat = {
   1.122 +		{-1,  3, -3,  1},
   1.123 +		{2, -5,  4, -1},
   1.124 +		{-1,  0,  1,  0},
   1.125 +		{0,  2,  0,  0}
   1.126 +	};
   1.127 +
   1.128 +	tmp = v4_scale(v4_transform(v4_cons(a, b, c, d), crspline_mat), 0.5);
   1.129 +	return v4_dot(v4_cons(tsq * t, tsq, t, 1.0), tmp);
   1.130 +}
   1.131 +
   1.132 +/** Bezier interpolation */
   1.133 +scalar_t bezier(scalar_t a, scalar_t b, scalar_t c, scalar_t d, scalar_t t)
   1.134 +{
   1.135 +	scalar_t omt, omt3, t3, f;
   1.136 +	t3 = t * t * t;
   1.137 +	omt = 1.0f - t;
   1.138 +	omt3 = omt * omt * omt;
   1.139 +	f = 3 * t * omt;
   1.140 +
   1.141 +	return (a * omt3) + (b * f * omt) + (c * f * t) + (d * t3);
   1.142 +}
   1.143 +
   1.144 +/* ---- Ken Perlin's implementation of noise ---- */
   1.145 +
   1.146 +#define B	0x100
   1.147 +#define BM	0xff
   1.148 +#define N	0x1000
   1.149 +#define NP	12   /* 2^N */
   1.150 +#define NM	0xfff
   1.151 +
   1.152 +#define s_curve(t) (t * t * (3.0f - 2.0f * t))
   1.153 +
   1.154 +#define setup(elem, b0, b1, r0, r1) \
   1.155 +	do {							\
   1.156 +		scalar_t t = elem + N;		\
   1.157 +		b0 = ((int)t) & BM;			\
   1.158 +		b1 = (b0 + 1) & BM;			\
   1.159 +		r0 = t - (int)t;			\
   1.160 +		r1 = r0 - 1.0f;				\
   1.161 +	} while(0)
   1.162 +
   1.163 +
   1.164 +static int perm[B + B + 2];			/* permuted index from g_n onto themselves */
   1.165 +static vec3_t grad3[B + B + 2];		/* 3D random gradients */
   1.166 +static vec2_t grad2[B + B + 2];		/* 2D random gradients */
   1.167 +static scalar_t grad1[B + B + 2];	/* 1D random ... slopes */
   1.168 +static int tables_valid;
   1.169 +
   1.170 +static void init_noise()
   1.171 +{
   1.172 +	int i;
   1.173 +
   1.174 +	/* calculate random gradients */
   1.175 +	for(i=0; i<B; i++) {
   1.176 +		perm[i] = i;	/* .. and initialize permutation mapping to identity */
   1.177 +
   1.178 +		grad1[i] = (scalar_t)((rand() % (B + B)) - B) / B;
   1.179 +
   1.180 +		grad2[i].x = (scalar_t)((rand() % (B + B)) - B) / B;
   1.181 +		grad2[i].y = (scalar_t)((rand() % (B + B)) - B) / B;
   1.182 +		grad2[i] = v2_normalize(grad2[i]);
   1.183 +
   1.184 +		grad3[i].x = (scalar_t)((rand() % (B + B)) - B) / B;
   1.185 +		grad3[i].y = (scalar_t)((rand() % (B + B)) - B) / B;
   1.186 +		grad3[i].z = (scalar_t)((rand() % (B + B)) - B) / B;
   1.187 +		grad3[i] = v3_normalize(grad3[i]);
   1.188 +	}
   1.189 +
   1.190 +	/* permute indices by swapping them randomly */
   1.191 +	for(i=0; i<B; i++) {
   1.192 +		int rand_idx = rand() % B;
   1.193 +
   1.194 +		int tmp = perm[i];
   1.195 +		perm[i] = perm[rand_idx];
   1.196 +		perm[rand_idx] = tmp;
   1.197 +	}
   1.198 +
   1.199 +	/* fill up the rest of the arrays by duplicating the existing gradients */
   1.200 +	/* and permutations */
   1.201 +	for(i=0; i<B+2; i++) {
   1.202 +		perm[B + i] = perm[i];
   1.203 +		grad1[B + i] = grad1[i];
   1.204 +		grad2[B + i] = grad2[i];
   1.205 +		grad3[B + i] = grad3[i];
   1.206 +	}
   1.207 +}
   1.208 +
   1.209 +scalar_t noise1(scalar_t x)
   1.210 +{
   1.211 +	int bx0, bx1;
   1.212 +	scalar_t rx0, rx1, sx, u, v;
   1.213 +
   1.214 +	if(!tables_valid) {
   1.215 +		init_noise();
   1.216 +		tables_valid = 1;
   1.217 +	}
   1.218 +
   1.219 +	setup(x, bx0, bx1, rx0, rx1);
   1.220 +	sx = s_curve(rx0);
   1.221 +	u = rx0 * grad1[perm[bx0]];
   1.222 +	v = rx1 * grad1[perm[bx1]];
   1.223 +
   1.224 +	return lerp(u, v, sx);
   1.225 +}
   1.226 +
   1.227 +scalar_t noise2(scalar_t x, scalar_t y)
   1.228 +{
   1.229 +	int i, j, b00, b10, b01, b11;
   1.230 +	int bx0, bx1, by0, by1;
   1.231 +	scalar_t rx0, rx1, ry0, ry1;
   1.232 +	scalar_t sx, sy, u, v, a, b;
   1.233 +
   1.234 +	if(!tables_valid) {
   1.235 +		init_noise();
   1.236 +		tables_valid = 1;
   1.237 +	}
   1.238 +
   1.239 +	setup(x, bx0, bx1, rx0, rx1);
   1.240 +	setup(y, by0, by1, ry0, ry1);
   1.241 +
   1.242 +	i = perm[bx0];
   1.243 +	j = perm[bx1];
   1.244 +
   1.245 +	b00 = perm[i + by0];
   1.246 +	b10 = perm[j + by0];
   1.247 +	b01 = perm[i + by1];
   1.248 +	b11 = perm[j + by1];
   1.249 +
   1.250 +	/* calculate hermite inteprolating factors */
   1.251 +	sx = s_curve(rx0);
   1.252 +	sy = s_curve(ry0);
   1.253 +
   1.254 +	/* interpolate along the left edge */
   1.255 +	u = v2_dot(grad2[b00], v2_cons(rx0, ry0));
   1.256 +	v = v2_dot(grad2[b10], v2_cons(rx1, ry0));
   1.257 +	a = lerp(u, v, sx);
   1.258 +
   1.259 +	/* interpolate along the right edge */
   1.260 +	u = v2_dot(grad2[b01], v2_cons(rx0, ry1));
   1.261 +	v = v2_dot(grad2[b11], v2_cons(rx1, ry1));
   1.262 +	b = lerp(u, v, sx);
   1.263 +
   1.264 +	/* interpolate between them */
   1.265 +	return lerp(a, b, sy);
   1.266 +}
   1.267 +
   1.268 +scalar_t noise3(scalar_t x, scalar_t y, scalar_t z)
   1.269 +{
   1.270 +	int i, j;
   1.271 +	int bx0, bx1, by0, by1, bz0, bz1;
   1.272 +	int b00, b10, b01, b11;
   1.273 +	scalar_t rx0, rx1, ry0, ry1, rz0, rz1;
   1.274 +	scalar_t sx, sy, sz;
   1.275 +	scalar_t u, v, a, b, c, d;
   1.276 +
   1.277 +	if(!tables_valid) {
   1.278 +		init_noise();
   1.279 +		tables_valid = 1;
   1.280 +	}
   1.281 +
   1.282 +	setup(x, bx0, bx1, rx0, rx1);
   1.283 +	setup(y, by0, by1, ry0, ry1);
   1.284 +	setup(z, bz0, bz1, rz0, rz1);
   1.285 +
   1.286 +	i = perm[bx0];
   1.287 +	j = perm[bx1];
   1.288 +
   1.289 +	b00 = perm[i + by0];
   1.290 +	b10 = perm[j + by0];
   1.291 +	b01 = perm[i + by1];
   1.292 +	b11 = perm[j + by1];
   1.293 +
   1.294 +	/* calculate hermite interpolating factors */
   1.295 +	sx = s_curve(rx0);
   1.296 +	sy = s_curve(ry0);
   1.297 +	sz = s_curve(rz0);
   1.298 +
   1.299 +	/* interpolate along the top slice of the cell */
   1.300 +	u = v3_dot(grad3[b00 + bz0], v3_cons(rx0, ry0, rz0));
   1.301 +	v = v3_dot(grad3[b10 + bz0], v3_cons(rx1, ry0, rz0));
   1.302 +	a = lerp(u, v, sx);
   1.303 +
   1.304 +	u = v3_dot(grad3[b01 + bz0], v3_cons(rx0, ry1, rz0));
   1.305 +	v = v3_dot(grad3[b11 + bz0], v3_cons(rx1, ry1, rz0));
   1.306 +	b = lerp(u, v, sx);
   1.307 +
   1.308 +	c = lerp(a, b, sy);
   1.309 +
   1.310 +	/* interpolate along the bottom slice of the cell */
   1.311 +	u = v3_dot(grad3[b00 + bz0], v3_cons(rx0, ry0, rz1));
   1.312 +	v = v3_dot(grad3[b10 + bz0], v3_cons(rx1, ry0, rz1));
   1.313 +	a = lerp(u, v, sx);
   1.314 +
   1.315 +	u = v3_dot(grad3[b01 + bz0], v3_cons(rx0, ry1, rz1));
   1.316 +	v = v3_dot(grad3[b11 + bz0], v3_cons(rx1, ry1, rz1));
   1.317 +	b = lerp(u, v, sx);
   1.318 +
   1.319 +	d = lerp(a, b, sy);
   1.320 +
   1.321 +	/* interpolate between slices */
   1.322 +	return lerp(c, d, sz);
   1.323 +}
   1.324 +
   1.325 +scalar_t fbm1(scalar_t x, int octaves)
   1.326 +{
   1.327 +	int i;
   1.328 +	scalar_t res = 0.0f, freq = 1.0f;
   1.329 +	for(i=0; i<octaves; i++) {
   1.330 +		res += noise1(x * freq) / freq;
   1.331 +		freq *= 2.0f;
   1.332 +	}
   1.333 +	return res;
   1.334 +}
   1.335 +
   1.336 +scalar_t fbm2(scalar_t x, scalar_t y, int octaves)
   1.337 +{
   1.338 +	int i;
   1.339 +	scalar_t res = 0.0f, freq = 1.0f;
   1.340 +	for(i=0; i<octaves; i++) {
   1.341 +		res += noise2(x * freq, y * freq) / freq;
   1.342 +		freq *= 2.0f;
   1.343 +	}
   1.344 +	return res;
   1.345 +}
   1.346 +
   1.347 +scalar_t fbm3(scalar_t x, scalar_t y, scalar_t z, int octaves)
   1.348 +{
   1.349 +	int i;
   1.350 +	scalar_t res = 0.0f, freq = 1.0f;
   1.351 +	for(i=0; i<octaves; i++) {
   1.352 +		res += noise3(x * freq, y * freq, z * freq) / freq;
   1.353 +		freq *= 2.0f;
   1.354 +	}
   1.355 +	return res;
   1.356 +}
   1.357 +
   1.358 +scalar_t turbulence1(scalar_t x, int octaves)
   1.359 +{
   1.360 +	int i;
   1.361 +	scalar_t res = 0.0f, freq = 1.0f;
   1.362 +	for(i=0; i<octaves; i++) {
   1.363 +		res += fabs(noise1(x * freq) / freq);
   1.364 +		freq *= 2.0f;
   1.365 +	}
   1.366 +	return res;
   1.367 +}
   1.368 +
   1.369 +scalar_t turbulence2(scalar_t x, scalar_t y, int octaves)
   1.370 +{
   1.371 +	int i;
   1.372 +	scalar_t res = 0.0f, freq = 1.0f;
   1.373 +	for(i=0; i<octaves; i++) {
   1.374 +		res += fabs(noise2(x * freq, y * freq) / freq);
   1.375 +		freq *= 2.0f;
   1.376 +	}
   1.377 +	return res;
   1.378 +}
   1.379 +
   1.380 +scalar_t turbulence3(scalar_t x, scalar_t y, scalar_t z, int octaves)
   1.381 +{
   1.382 +	int i;
   1.383 +	scalar_t res = 0.0f, freq = 1.0f;
   1.384 +	for(i=0; i<octaves; i++) {
   1.385 +		res += fabs(noise3(x * freq, y * freq, z * freq) / freq);
   1.386 +		freq *= 2.0f;
   1.387 +	}
   1.388 +	return res;
   1.389 +}