1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libs/vmath/vector.inl	Sun May 31 00:40:26 2015 +0300
     1.3 @@ -0,0 +1,812 @@
     1.4 +/*
     1.5 +libvmath - a vector math library
     1.6 +Copyright (C) 2004-2015 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 +
    1.22 +#include <math.h>
    1.23 +
    1.24 +#ifdef __cplusplus
    1.25 +extern "C" {
    1.26 +#endif	/* __cplusplus */
    1.27 +
    1.28 +scalar_t spline(scalar_t, scalar_t, scalar_t, scalar_t, scalar_t);
    1.29 +scalar_t bspline(scalar_t, scalar_t, scalar_t, scalar_t, scalar_t);
    1.30 +
    1.31 +/* C 2D vector functions */
    1.32 +static inline vec2_t v2_cons(scalar_t x, scalar_t y)
    1.33 +{
    1.34 +	vec2_t v;
    1.35 +	v.x = x;
    1.36 +	v.y = y;
    1.37 +	return v;
    1.38 +}
    1.39 +
    1.40 +static inline void v2_print(FILE *fp, vec2_t v)
    1.41 +{
    1.42 +	fprintf(fp, "[ %.4f %.4f ]", v.x, v.y);
    1.43 +}
    1.44 +
    1.45 +static inline vec2_t v2_add(vec2_t v1, vec2_t v2)
    1.46 +{
    1.47 +	vec2_t res;
    1.48 +	res.x = v1.x + v2.x;
    1.49 +	res.y = v1.y + v2.y;
    1.50 +	return res;
    1.51 +}
    1.52 +
    1.53 +static inline vec2_t v2_sub(vec2_t v1, vec2_t v2)
    1.54 +{
    1.55 +	vec2_t res;
    1.56 +	res.x = v1.x - v2.x;
    1.57 +	res.y = v1.y - v2.y;
    1.58 +	return res;
    1.59 +}
    1.60 +
    1.61 +static inline vec2_t v2_scale(vec2_t v, scalar_t s)
    1.62 +{
    1.63 +	vec2_t res;
    1.64 +	res.x = v.x * s;
    1.65 +	res.y = v.y * s;
    1.66 +	return res;
    1.67 +}
    1.68 +
    1.69 +static inline scalar_t v2_dot(vec2_t v1, vec2_t v2)
    1.70 +{
    1.71 +	return v1.x * v2.x + v1.y * v2.y;
    1.72 +}
    1.73 +
    1.74 +static inline scalar_t v2_length(vec2_t v)
    1.75 +{
    1.76 +	return sqrt(v.x * v.x + v.y * v.y);
    1.77 +}
    1.78 +
    1.79 +static inline scalar_t v2_length_sq(vec2_t v)
    1.80 +{
    1.81 +	return v.x * v.x + v.y * v.y;
    1.82 +}
    1.83 +
    1.84 +static inline vec2_t v2_normalize(vec2_t v)
    1.85 +{
    1.86 +	scalar_t len = (scalar_t)sqrt(v.x * v.x + v.y * v.y);
    1.87 +	v.x /= len;
    1.88 +	v.y /= len;
    1.89 +	return v;
    1.90 +}
    1.91 +
    1.92 +static inline vec2_t v2_lerp(vec2_t v1, vec2_t v2, scalar_t t)
    1.93 +{
    1.94 +	vec2_t res;
    1.95 +	res.x = v1.x + (v2.x - v1.x) * t;
    1.96 +	res.y = v1.y + (v2.y - v1.y) * t;
    1.97 +	return res;
    1.98 +}
    1.99 +
   1.100 +
   1.101 +/* C 3D vector functions */
   1.102 +static inline vec3_t v3_cons(scalar_t x, scalar_t y, scalar_t z)
   1.103 +{
   1.104 +	vec3_t v;
   1.105 +	v.x = x;
   1.106 +	v.y = y;
   1.107 +	v.z = z;
   1.108 +	return v;
   1.109 +}
   1.110 +
   1.111 +static inline void v3_print(FILE *fp, vec3_t v)
   1.112 +{
   1.113 +	fprintf(fp, "[ %.4f %.4f %.4f ]", v.x, v.y, v.z);
   1.114 +}
   1.115 +
   1.116 +static inline vec3_t v3_add(vec3_t v1, vec3_t v2)
   1.117 +{
   1.118 +	v1.x += v2.x;
   1.119 +	v1.y += v2.y;
   1.120 +	v1.z += v2.z;
   1.121 +	return v1;
   1.122 +}
   1.123 +
   1.124 +static inline vec3_t v3_sub(vec3_t v1, vec3_t v2)
   1.125 +{
   1.126 +	v1.x -= v2.x;
   1.127 +	v1.y -= v2.y;
   1.128 +	v1.z -= v2.z;
   1.129 +	return v1;
   1.130 +}
   1.131 +
   1.132 +static inline vec3_t v3_neg(vec3_t v)
   1.133 +{
   1.134 +	v.x = -v.x;
   1.135 +	v.y = -v.y;
   1.136 +	v.z = -v.z;
   1.137 +	return v;
   1.138 +}
   1.139 +
   1.140 +static inline vec3_t v3_mul(vec3_t v1, vec3_t v2)
   1.141 +{
   1.142 +	v1.x *= v2.x;
   1.143 +	v1.y *= v2.y;
   1.144 +	v1.z *= v2.z;
   1.145 +	return v1;
   1.146 +}
   1.147 +
   1.148 +static inline vec3_t v3_scale(vec3_t v1, scalar_t s)
   1.149 +{
   1.150 +	v1.x *= s;
   1.151 +	v1.y *= s;
   1.152 +	v1.z *= s;
   1.153 +	return v1;
   1.154 +}
   1.155 +
   1.156 +static inline scalar_t v3_dot(vec3_t v1, vec3_t v2)
   1.157 +{
   1.158 +	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
   1.159 +}
   1.160 +
   1.161 +static inline vec3_t v3_cross(vec3_t v1, vec3_t v2)
   1.162 +{
   1.163 +	vec3_t v;
   1.164 +	v.x = v1.y * v2.z - v1.z * v2.y;
   1.165 +	v.y = v1.z * v2.x - v1.x * v2.z;
   1.166 +	v.z = v1.x * v2.y - v1.y * v2.x;
   1.167 +	return v;
   1.168 +}
   1.169 +
   1.170 +static inline scalar_t v3_length(vec3_t v)
   1.171 +{
   1.172 +	return sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
   1.173 +}
   1.174 +
   1.175 +static inline scalar_t v3_length_sq(vec3_t v)
   1.176 +{
   1.177 +	return v.x * v.x + v.y * v.y + v.z * v.z;
   1.178 +}
   1.179 +
   1.180 +static inline vec3_t v3_normalize(vec3_t v)
   1.181 +{
   1.182 +	scalar_t len = sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
   1.183 +	v.x /= len;
   1.184 +	v.y /= len;
   1.185 +	v.z /= len;
   1.186 +	return v;
   1.187 +}
   1.188 +
   1.189 +static inline vec3_t v3_transform(vec3_t v, mat4_t m)
   1.190 +{
   1.191 +	vec3_t res;
   1.192 +	res.x = m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3];
   1.193 +	res.y = m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3];
   1.194 +	res.z = m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3];
   1.195 +	return res;
   1.196 +}
   1.197 +
   1.198 +static inline vec3_t v3_rotate(vec3_t v, scalar_t x, scalar_t y, scalar_t z)
   1.199 +{
   1.200 +	void m4_rotate(mat4_t, scalar_t, scalar_t, scalar_t);
   1.201 +
   1.202 +	mat4_t m = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
   1.203 +	m4_rotate(m, x, y, z);
   1.204 +	return v3_transform(v, m);
   1.205 +}
   1.206 +
   1.207 +static inline vec3_t v3_rotate_axis(vec3_t v, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
   1.208 +{
   1.209 +	void m4_rotate_axis(mat4_t, scalar_t, scalar_t, scalar_t, scalar_t);
   1.210 +
   1.211 +	mat4_t m = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
   1.212 +	m4_rotate_axis(m, angle, x, y, z);
   1.213 +	return v3_transform(v, m);
   1.214 +}
   1.215 +
   1.216 +static inline vec3_t v3_rotate_quat(vec3_t v, quat_t q)
   1.217 +{
   1.218 +	quat_t quat_rotate_quat(quat_t, quat_t);
   1.219 +
   1.220 +	quat_t vq = v4_cons(v.x, v.y, v.z, 0.0);
   1.221 +	quat_t res = quat_rotate_quat(vq, q);
   1.222 +	return v3_cons(res.x, res.y, res.z);
   1.223 +}
   1.224 +
   1.225 +static inline vec3_t v3_reflect(vec3_t v, vec3_t n)
   1.226 +{
   1.227 +	scalar_t dot = v3_dot(v, n);
   1.228 +	return v3_sub(v3_scale(n, dot * 2.0), v);
   1.229 +}
   1.230 +
   1.231 +static inline vec3_t v3_lerp(vec3_t v1, vec3_t v2, scalar_t t)
   1.232 +{
   1.233 +	v1.x += (v2.x - v1.x) * t;
   1.234 +	v1.y += (v2.y - v1.y) * t;
   1.235 +	v1.z += (v2.z - v1.z) * t;
   1.236 +	return v1;
   1.237 +}
   1.238 +
   1.239 +/* C 4D vector functions */
   1.240 +static inline vec4_t v4_cons(scalar_t x, scalar_t y, scalar_t z, scalar_t w)
   1.241 +{
   1.242 +	vec4_t v;
   1.243 +	v.x = x;
   1.244 +	v.y = y;
   1.245 +	v.z = z;
   1.246 +	v.w = w;
   1.247 +	return v;
   1.248 +}
   1.249 +
   1.250 +static inline void v4_print(FILE *fp, vec4_t v)
   1.251 +{
   1.252 +	fprintf(fp, "[ %.4f %.4f %.4f %.4f ]", v.x, v.y, v.z, v.w);
   1.253 +}
   1.254 +
   1.255 +static inline vec4_t v4_add(vec4_t v1, vec4_t v2)
   1.256 +{
   1.257 +	v1.x += v2.x;
   1.258 +	v1.y += v2.y;
   1.259 +	v1.z += v2.z;
   1.260 +	v1.w += v2.w;
   1.261 +	return v1;
   1.262 +}
   1.263 +
   1.264 +static inline vec4_t v4_sub(vec4_t v1, vec4_t v2)
   1.265 +{
   1.266 +	v1.x -= v2.x;
   1.267 +	v1.y -= v2.y;
   1.268 +	v1.z -= v2.z;
   1.269 +	v1.w -= v2.w;
   1.270 +	return v1;
   1.271 +}
   1.272 +
   1.273 +static inline vec4_t v4_neg(vec4_t v)
   1.274 +{
   1.275 +	v.x = -v.x;
   1.276 +	v.y = -v.y;
   1.277 +	v.z = -v.z;
   1.278 +	v.w = -v.w;
   1.279 +	return v;
   1.280 +}
   1.281 +
   1.282 +static inline vec4_t v4_mul(vec4_t v1, vec4_t v2)
   1.283 +{
   1.284 +	v1.x *= v2.x;
   1.285 +	v1.y *= v2.y;
   1.286 +	v1.z *= v2.z;
   1.287 +	v1.w *= v2.w;
   1.288 +	return v1;
   1.289 +}
   1.290 +
   1.291 +static inline vec4_t v4_scale(vec4_t v, scalar_t s)
   1.292 +{
   1.293 +	v.x *= s;
   1.294 +	v.y *= s;
   1.295 +	v.z *= s;
   1.296 +	v.w *= s;
   1.297 +	return v;
   1.298 +}
   1.299 +
   1.300 +static inline scalar_t v4_dot(vec4_t v1, vec4_t v2)
   1.301 +{
   1.302 +	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w;
   1.303 +}
   1.304 +
   1.305 +static inline scalar_t v4_length(vec4_t v)
   1.306 +{
   1.307 +	return sqrt(v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
   1.308 +}
   1.309 +
   1.310 +static inline scalar_t v4_length_sq(vec4_t v)
   1.311 +{
   1.312 +	return v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w;
   1.313 +}
   1.314 +
   1.315 +static inline vec4_t v4_normalize(vec4_t v)
   1.316 +{
   1.317 +	scalar_t len = sqrt(v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
   1.318 +	v.x /= len;
   1.319 +	v.y /= len;
   1.320 +	v.z /= len;
   1.321 +	v.w /= len;
   1.322 +	return v;
   1.323 +}
   1.324 +
   1.325 +static inline vec4_t v4_transform(vec4_t v, mat4_t m)
   1.326 +{
   1.327 +	vec4_t res;
   1.328 +	res.x = m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w;
   1.329 +	res.y = m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w;
   1.330 +	res.z = m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w;
   1.331 +	res.w = m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w;
   1.332 +	return res;
   1.333 +}
   1.334 +
   1.335 +#ifdef __cplusplus
   1.336 +}	/* extern "C" */
   1.337 +
   1.338 +
   1.339 +/* --------------- C++ part -------------- */
   1.340 +
   1.341 +inline scalar_t &Vector2::operator [](int elem)
   1.342 +{
   1.343 +	return elem ? y : x;
   1.344 +}
   1.345 +
   1.346 +inline const scalar_t &Vector2::operator [](int elem) const
   1.347 +{
   1.348 +	return elem ? y : x;
   1.349 +}
   1.350 +
   1.351 +inline Vector2 operator -(const Vector2 &vec)
   1.352 +{
   1.353 +	return Vector2(-vec.x, -vec.y);
   1.354 +}
   1.355 +
   1.356 +inline scalar_t dot_product(const Vector2 &v1, const Vector2 &v2)
   1.357 +{
   1.358 +	return v1.x * v2.x + v1.y * v2.y;
   1.359 +}
   1.360 +
   1.361 +inline Vector2 operator +(const Vector2 &v1, const Vector2 &v2)
   1.362 +{
   1.363 +	return Vector2(v1.x + v2.x, v1.y + v2.y);
   1.364 +}
   1.365 +
   1.366 +inline Vector2 operator -(const Vector2 &v1, const Vector2 &v2)
   1.367 +{
   1.368 +	return Vector2(v1.x - v2.x, v1.y - v2.y);
   1.369 +}
   1.370 +
   1.371 +inline Vector2 operator *(const Vector2 &v1, const Vector2 &v2)
   1.372 +{
   1.373 +	return Vector2(v1.x * v2.x, v1.y * v2.y);
   1.374 +}
   1.375 +
   1.376 +inline Vector2 operator /(const Vector2 &v1, const Vector2 &v2)
   1.377 +{
   1.378 +	return Vector2(v1.x / v2.x, v1.y / v2.y);
   1.379 +}
   1.380 +
   1.381 +inline bool operator ==(const Vector2 &v1, const Vector2 &v2)
   1.382 +{
   1.383 +	return (fabs(v1.x - v2.x) < XSMALL_NUMBER) && (fabs(v1.y - v2.x) < XSMALL_NUMBER);
   1.384 +}
   1.385 +
   1.386 +inline void operator +=(Vector2 &v1, const Vector2 &v2)
   1.387 +{
   1.388 +	v1.x += v2.x;
   1.389 +	v1.y += v2.y;
   1.390 +}
   1.391 +
   1.392 +inline void operator -=(Vector2 &v1, const Vector2 &v2)
   1.393 +{
   1.394 +	v1.x -= v2.x;
   1.395 +	v1.y -= v2.y;
   1.396 +}
   1.397 +
   1.398 +inline void operator *=(Vector2 &v1, const Vector2 &v2)
   1.399 +{
   1.400 +	v1.x *= v2.x;
   1.401 +	v1.y *= v2.y;
   1.402 +}
   1.403 +
   1.404 +inline void operator /=(Vector2 &v1, const Vector2 &v2)
   1.405 +{
   1.406 +	v1.x /= v2.x;
   1.407 +	v1.y /= v2.y;
   1.408 +}
   1.409 +
   1.410 +inline Vector2 operator +(const Vector2 &vec, scalar_t scalar)
   1.411 +{
   1.412 +	return Vector2(vec.x + scalar, vec.y + scalar);
   1.413 +}
   1.414 +
   1.415 +inline Vector2 operator +(scalar_t scalar, const Vector2 &vec)
   1.416 +{
   1.417 +	return Vector2(vec.x + scalar, vec.y + scalar);
   1.418 +}
   1.419 +
   1.420 +inline Vector2 operator -(const Vector2 &vec, scalar_t scalar)
   1.421 +{
   1.422 +	return Vector2(vec.x - scalar, vec.y - scalar);
   1.423 +}
   1.424 +
   1.425 +inline Vector2 operator *(const Vector2 &vec, scalar_t scalar)
   1.426 +{
   1.427 +	return Vector2(vec.x * scalar, vec.y * scalar);
   1.428 +}
   1.429 +
   1.430 +inline Vector2 operator *(scalar_t scalar, const Vector2 &vec)
   1.431 +{
   1.432 +	return Vector2(vec.x * scalar, vec.y * scalar);
   1.433 +}
   1.434 +
   1.435 +inline Vector2 operator /(const Vector2 &vec, scalar_t scalar)
   1.436 +{
   1.437 +	return Vector2(vec.x / scalar, vec.y / scalar);
   1.438 +}
   1.439 +
   1.440 +inline void operator +=(Vector2 &vec, scalar_t scalar)
   1.441 +{
   1.442 +	vec.x += scalar;
   1.443 +	vec.y += scalar;
   1.444 +}
   1.445 +
   1.446 +inline void operator -=(Vector2 &vec, scalar_t scalar)
   1.447 +{
   1.448 +	vec.x -= scalar;
   1.449 +	vec.y -= scalar;
   1.450 +}
   1.451 +
   1.452 +inline void operator *=(Vector2 &vec, scalar_t scalar)
   1.453 +{
   1.454 +	vec.x *= scalar;
   1.455 +	vec.y *= scalar;
   1.456 +}
   1.457 +
   1.458 +inline void operator /=(Vector2 &vec, scalar_t scalar)
   1.459 +{
   1.460 +	vec.x /= scalar;
   1.461 +	vec.y /= scalar;
   1.462 +}
   1.463 +
   1.464 +inline scalar_t Vector2::length() const
   1.465 +{
   1.466 +	return sqrt(x*x + y*y);
   1.467 +}
   1.468 +
   1.469 +inline scalar_t Vector2::length_sq() const
   1.470 +{
   1.471 +	return x*x + y*y;
   1.472 +}
   1.473 +
   1.474 +inline Vector2 lerp(const Vector2 &a, const Vector2 &b, scalar_t t)
   1.475 +{
   1.476 +	return a + (b - a) * t;
   1.477 +}
   1.478 +
   1.479 +inline Vector2 catmull_rom_spline(const Vector2 &v0, const Vector2 &v1,
   1.480 +		const Vector2 &v2, const Vector2 &v3, scalar_t t)
   1.481 +{
   1.482 +	scalar_t x = spline(v0.x, v1.x, v2.x, v3.x, t);
   1.483 +	scalar_t y = spline(v0.y, v1.y, v2.y, v3.y, t);
   1.484 +	return Vector2(x, y);
   1.485 +}
   1.486 +
   1.487 +inline Vector2 bspline(const Vector2 &v0, const Vector2 &v1,
   1.488 +		const Vector2 &v2, const Vector2 &v3, scalar_t t)
   1.489 +{
   1.490 +	scalar_t x = bspline(v0.x, v1.x, v2.x, v3.x, t);
   1.491 +	scalar_t y = bspline(v0.y, v1.y, v2.y, v3.y, t);
   1.492 +	return Vector2(x, y);
   1.493 +}
   1.494 +
   1.495 +
   1.496 +/* ------------- Vector3 -------------- */
   1.497 +
   1.498 +inline scalar_t &Vector3::operator [](int elem) {
   1.499 +	return elem ? (elem == 1 ? y : z) : x;
   1.500 +}
   1.501 +
   1.502 +inline const scalar_t &Vector3::operator [](int elem) const {
   1.503 +	return elem ? (elem == 1 ? y : z) : x;
   1.504 +}
   1.505 +
   1.506 +/* unary operations */
   1.507 +inline Vector3 operator -(const Vector3 &vec) {
   1.508 +	return Vector3(-vec.x, -vec.y, -vec.z);
   1.509 +}
   1.510 +
   1.511 +/* binary vector (op) vector operations */
   1.512 +inline scalar_t dot_product(const Vector3 &v1, const Vector3 &v2) {
   1.513 +	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
   1.514 +}
   1.515 +
   1.516 +inline Vector3 cross_product(const Vector3 &v1, const Vector3 &v2) {
   1.517 +	return Vector3(v1.y * v2.z - v1.z * v2.y,  v1.z * v2.x - v1.x * v2.z,  v1.x * v2.y - v1.y * v2.x);
   1.518 +}
   1.519 +
   1.520 +
   1.521 +inline Vector3 operator +(const Vector3 &v1, const Vector3 &v2) {
   1.522 +	return Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
   1.523 +}
   1.524 +
   1.525 +inline Vector3 operator -(const Vector3 &v1, const Vector3 &v2) {
   1.526 +	return Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
   1.527 +}
   1.528 +
   1.529 +inline Vector3 operator *(const Vector3 &v1, const Vector3 &v2) {
   1.530 +	return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
   1.531 +}
   1.532 +
   1.533 +inline Vector3 operator /(const Vector3 &v1, const Vector3 &v2) {
   1.534 +	return Vector3(v1.x / v2.x, v1.y / v2.y, v1.z / v2.z);
   1.535 +}
   1.536 +
   1.537 +inline bool operator ==(const Vector3 &v1, const Vector3 &v2) {
   1.538 +	return (fabs(v1.x - v2.x) < XSMALL_NUMBER) && (fabs(v1.y - v2.y) < XSMALL_NUMBER) && (fabs(v1.z - v2.z) < XSMALL_NUMBER);
   1.539 +}
   1.540 +
   1.541 +inline void operator +=(Vector3 &v1, const Vector3 &v2) {
   1.542 +	v1.x += v2.x;
   1.543 +	v1.y += v2.y;
   1.544 +	v1.z += v2.z;
   1.545 +}
   1.546 +
   1.547 +inline void operator -=(Vector3 &v1, const Vector3 &v2) {
   1.548 +	v1.x -= v2.x;
   1.549 +	v1.y -= v2.y;
   1.550 +	v1.z -= v2.z;
   1.551 +}
   1.552 +
   1.553 +inline void operator *=(Vector3 &v1, const Vector3 &v2) {
   1.554 +	v1.x *= v2.x;
   1.555 +	v1.y *= v2.y;
   1.556 +	v1.z *= v2.z;
   1.557 +}
   1.558 +
   1.559 +inline void operator /=(Vector3 &v1, const Vector3 &v2) {
   1.560 +	v1.x /= v2.x;
   1.561 +	v1.y /= v2.y;
   1.562 +	v1.z /= v2.z;
   1.563 +}
   1.564 +/* binary vector (op) scalar and scalar (op) vector operations */
   1.565 +inline Vector3 operator +(const Vector3 &vec, scalar_t scalar) {
   1.566 +	return Vector3(vec.x + scalar, vec.y + scalar, vec.z + scalar);
   1.567 +}
   1.568 +
   1.569 +inline Vector3 operator +(scalar_t scalar, const Vector3 &vec) {
   1.570 +	return Vector3(vec.x + scalar, vec.y + scalar, vec.z + scalar);
   1.571 +}
   1.572 +
   1.573 +inline Vector3 operator -(const Vector3 &vec, scalar_t scalar) {
   1.574 +	return Vector3(vec.x - scalar, vec.y - scalar, vec.z - scalar);
   1.575 +}
   1.576 +
   1.577 +inline Vector3 operator *(const Vector3 &vec, scalar_t scalar) {
   1.578 +	return Vector3(vec.x * scalar, vec.y * scalar, vec.z * scalar);
   1.579 +}
   1.580 +
   1.581 +inline Vector3 operator *(scalar_t scalar, const Vector3 &vec) {
   1.582 +	return Vector3(vec.x * scalar, vec.y * scalar, vec.z * scalar);
   1.583 +}
   1.584 +
   1.585 +inline Vector3 operator /(const Vector3 &vec, scalar_t scalar) {
   1.586 +	return Vector3(vec.x / scalar, vec.y / scalar, vec.z / scalar);
   1.587 +}
   1.588 +
   1.589 +inline void operator +=(Vector3 &vec, scalar_t scalar) {
   1.590 +	vec.x += scalar;
   1.591 +	vec.y += scalar;
   1.592 +	vec.z += scalar;
   1.593 +}
   1.594 +
   1.595 +inline void operator -=(Vector3 &vec, scalar_t scalar) {
   1.596 +	vec.x -= scalar;
   1.597 +	vec.y -= scalar;
   1.598 +	vec.z -= scalar;
   1.599 +}
   1.600 +
   1.601 +inline void operator *=(Vector3 &vec, scalar_t scalar) {
   1.602 +	vec.x *= scalar;
   1.603 +	vec.y *= scalar;
   1.604 +	vec.z *= scalar;
   1.605 +}
   1.606 +
   1.607 +inline void operator /=(Vector3 &vec, scalar_t scalar) {
   1.608 +	vec.x /= scalar;
   1.609 +	vec.y /= scalar;
   1.610 +	vec.z /= scalar;
   1.611 +}
   1.612 +
   1.613 +inline scalar_t Vector3::length() const {
   1.614 +	return sqrt(x*x + y*y + z*z);
   1.615 +}
   1.616 +inline scalar_t Vector3::length_sq() const {
   1.617 +	return x*x + y*y + z*z;
   1.618 +}
   1.619 +
   1.620 +inline Vector3 lerp(const Vector3 &a, const Vector3 &b, scalar_t t) {
   1.621 +	return a + (b - a) * t;
   1.622 +}
   1.623 +
   1.624 +inline Vector3 catmull_rom_spline(const Vector3 &v0, const Vector3 &v1,
   1.625 +		const Vector3 &v2, const Vector3 &v3, scalar_t t)
   1.626 +{
   1.627 +	scalar_t x = spline(v0.x, v1.x, v2.x, v3.x, t);
   1.628 +	scalar_t y = spline(v0.y, v1.y, v2.y, v3.y, t);
   1.629 +	scalar_t z = spline(v0.z, v1.z, v2.z, v3.z, t);
   1.630 +	return Vector3(x, y, z);
   1.631 +}
   1.632 +
   1.633 +inline Vector3 bspline(const Vector3 &v0, const Vector3 &v1,
   1.634 +		const Vector3 &v2, const Vector3 &v3, scalar_t t)
   1.635 +{
   1.636 +	scalar_t x = bspline(v0.x, v1.x, v2.x, v3.x, t);
   1.637 +	scalar_t y = bspline(v0.y, v1.y, v2.y, v3.y, t);
   1.638 +	scalar_t z = bspline(v0.z, v1.z, v2.z, v3.z, t);
   1.639 +	return Vector3(x, y, z);
   1.640 +}
   1.641 +
   1.642 +/* ----------- Vector4 ----------------- */
   1.643 +
   1.644 +inline scalar_t &Vector4::operator [](int elem) {
   1.645 +	return elem ? (elem == 1 ? y : (elem == 2 ? z : w)) : x;
   1.646 +}
   1.647 +
   1.648 +inline const scalar_t &Vector4::operator [](int elem) const {
   1.649 +	return elem ? (elem == 1 ? y : (elem == 2 ? z : w)) : x;
   1.650 +}
   1.651 +
   1.652 +inline Vector4 operator -(const Vector4 &vec) {
   1.653 +	return Vector4(-vec.x, -vec.y, -vec.z, -vec.w);
   1.654 +}
   1.655 +
   1.656 +inline scalar_t dot_product(const Vector4 &v1, const Vector4 &v2) {
   1.657 +	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w;
   1.658 +}
   1.659 +
   1.660 +inline Vector4 cross_product(const Vector4 &v1, const Vector4 &v2, const Vector4 &v3) {
   1.661 +	scalar_t a, b, c, d, e, f;       /* Intermediate Values */
   1.662 +    Vector4 result;
   1.663 +
   1.664 +    /* Calculate intermediate values. */
   1.665 +    a = (v2.x * v3.y) - (v2.y * v3.x);
   1.666 +    b = (v2.x * v3.z) - (v2.z * v3.x);
   1.667 +    c = (v2.x * v3.w) - (v2.w * v3.x);
   1.668 +    d = (v2.y * v3.z) - (v2.z * v3.y);
   1.669 +    e = (v2.y * v3.w) - (v2.w * v3.y);
   1.670 +    f = (v2.z * v3.w) - (v2.w * v3.z);
   1.671 +
   1.672 +    /* Calculate the result-vector components. */
   1.673 +    result.x =   (v1.y * f) - (v1.z * e) + (v1.w * d);
   1.674 +    result.y = - (v1.x * f) + (v1.z * c) - (v1.w * b);
   1.675 +    result.z =   (v1.x * e) - (v1.y * c) + (v1.w * a);
   1.676 +    result.w = - (v1.x * d) + (v1.y * b) - (v1.z * a);
   1.677 +    return result;
   1.678 +}
   1.679 +
   1.680 +inline Vector4 operator +(const Vector4 &v1, const Vector4 &v2) {
   1.681 +	return Vector4(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z, v1.w + v2.w);
   1.682 +}
   1.683 +
   1.684 +inline Vector4 operator -(const Vector4 &v1, const Vector4 &v2) {
   1.685 +	return Vector4(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z, v1.w - v2.w);
   1.686 +}
   1.687 +
   1.688 +inline Vector4 operator *(const Vector4 &v1, const Vector4 &v2) {
   1.689 +	return Vector4(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z, v1.w * v2.w);
   1.690 +}
   1.691 +
   1.692 +inline Vector4 operator /(const Vector4 &v1, const Vector4 &v2) {
   1.693 +	return Vector4(v1.x / v2.x, v1.y / v2.y, v1.z / v2.z, v1.w / v2.w);
   1.694 +}
   1.695 +
   1.696 +inline bool operator ==(const Vector4 &v1, const Vector4 &v2) {
   1.697 +	return	(fabs(v1.x - v2.x) < XSMALL_NUMBER) &&
   1.698 +			(fabs(v1.y - v2.y) < XSMALL_NUMBER) &&
   1.699 +			(fabs(v1.z - v2.z) < XSMALL_NUMBER) &&
   1.700 +			(fabs(v1.w - v2.w) < XSMALL_NUMBER);
   1.701 +}
   1.702 +
   1.703 +inline void operator +=(Vector4 &v1, const Vector4 &v2) {
   1.704 +	v1.x += v2.x;
   1.705 +	v1.y += v2.y;
   1.706 +	v1.z += v2.z;
   1.707 +	v1.w += v2.w;
   1.708 +}
   1.709 +
   1.710 +inline void operator -=(Vector4 &v1, const Vector4 &v2) {
   1.711 +	v1.x -= v2.x;
   1.712 +	v1.y -= v2.y;
   1.713 +	v1.z -= v2.z;
   1.714 +	v1.w -= v2.w;
   1.715 +}
   1.716 +
   1.717 +inline void operator *=(Vector4 &v1, const Vector4 &v2) {
   1.718 +	v1.x *= v2.x;
   1.719 +	v1.y *= v2.y;
   1.720 +	v1.z *= v2.z;
   1.721 +	v1.w *= v2.w;
   1.722 +}
   1.723 +
   1.724 +inline void operator /=(Vector4 &v1, const Vector4 &v2) {
   1.725 +	v1.x /= v2.x;
   1.726 +	v1.y /= v2.y;
   1.727 +	v1.z /= v2.z;
   1.728 +	v1.w /= v2.w;
   1.729 +}
   1.730 +
   1.731 +/* binary vector (op) scalar and scalar (op) vector operations */
   1.732 +inline Vector4 operator +(const Vector4 &vec, scalar_t scalar) {
   1.733 +	return Vector4(vec.x + scalar, vec.y + scalar, vec.z + scalar, vec.w + scalar);
   1.734 +}
   1.735 +
   1.736 +inline Vector4 operator +(scalar_t scalar, const Vector4 &vec) {
   1.737 +	return Vector4(vec.x + scalar, vec.y + scalar, vec.z + scalar, vec.w + scalar);
   1.738 +}
   1.739 +
   1.740 +inline Vector4 operator -(const Vector4 &vec, scalar_t scalar) {
   1.741 +	return Vector4(vec.x - scalar, vec.y - scalar, vec.z - scalar, vec.w - scalar);
   1.742 +}
   1.743 +
   1.744 +inline Vector4 operator *(const Vector4 &vec, scalar_t scalar) {
   1.745 +	return Vector4(vec.x * scalar, vec.y * scalar, vec.z * scalar, vec.w * scalar);
   1.746 +}
   1.747 +
   1.748 +inline Vector4 operator *(scalar_t scalar, const Vector4 &vec) {
   1.749 +	return Vector4(vec.x * scalar, vec.y * scalar, vec.z * scalar, vec.w * scalar);
   1.750 +}
   1.751 +
   1.752 +inline Vector4 operator /(const Vector4 &vec, scalar_t scalar) {
   1.753 +	return Vector4(vec.x / scalar, vec.y / scalar, vec.z / scalar, vec.w / scalar);
   1.754 +}
   1.755 +
   1.756 +inline void operator +=(Vector4 &vec, scalar_t scalar) {
   1.757 +	vec.x += scalar;
   1.758 +	vec.y += scalar;
   1.759 +	vec.z += scalar;
   1.760 +	vec.w += scalar;
   1.761 +}
   1.762 +
   1.763 +inline void operator -=(Vector4 &vec, scalar_t scalar) {
   1.764 +	vec.x -= scalar;
   1.765 +	vec.y -= scalar;
   1.766 +	vec.z -= scalar;
   1.767 +	vec.w -= scalar;
   1.768 +}
   1.769 +
   1.770 +inline void operator *=(Vector4 &vec, scalar_t scalar) {
   1.771 +	vec.x *= scalar;
   1.772 +	vec.y *= scalar;
   1.773 +	vec.z *= scalar;
   1.774 +	vec.w *= scalar;
   1.775 +}
   1.776 +
   1.777 +inline void operator /=(Vector4 &vec, scalar_t scalar) {
   1.778 +	vec.x /= scalar;
   1.779 +	vec.y /= scalar;
   1.780 +	vec.z /= scalar;
   1.781 +	vec.w /= scalar;
   1.782 +}
   1.783 +
   1.784 +inline scalar_t Vector4::length() const {
   1.785 +	return sqrt(x*x + y*y + z*z + w*w);
   1.786 +}
   1.787 +inline scalar_t Vector4::length_sq() const {
   1.788 +	return x*x + y*y + z*z + w*w;
   1.789 +}
   1.790 +
   1.791 +inline Vector4 lerp(const Vector4 &v0, const Vector4 &v1, scalar_t t)
   1.792 +{
   1.793 +	return v0 + (v1 - v0) * t;
   1.794 +}
   1.795 +
   1.796 +inline Vector4 catmull_rom_spline(const Vector4 &v0, const Vector4 &v1,
   1.797 +		const Vector4 &v2, const Vector4 &v3, scalar_t t)
   1.798 +{
   1.799 +	scalar_t x = spline(v0.x, v1.x, v2.x, v3.x, t);
   1.800 +	scalar_t y = spline(v0.y, v1.y, v2.y, v3.y, t);
   1.801 +	scalar_t z = spline(v0.z, v1.z, v2.z, v3.z, t);
   1.802 +	scalar_t w = spline(v0.w, v1.w, v2.w, v3.w, t);
   1.803 +	return Vector4(x, y, z, w);
   1.804 +}
   1.805 +
   1.806 +inline Vector4 bspline(const Vector4 &v0, const Vector4 &v1,
   1.807 +		const Vector4 &v2, const Vector4 &v3, scalar_t t)
   1.808 +{
   1.809 +	scalar_t x = bspline(v0.x, v1.x, v2.x, v3.x, t);
   1.810 +	scalar_t y = bspline(v0.y, v1.y, v2.y, v3.y, t);
   1.811 +	scalar_t z = bspline(v0.z, v1.z, v2.z, v3.z, t);
   1.812 +	scalar_t w = bspline(v0.w, v1.w, v2.w, v3.w, t);
   1.813 +	return Vector4(x, y, z, w);
   1.814 +}
   1.815 +#endif	/* __cplusplus */