777 lines
12 KiB
C
Executable File
777 lines
12 KiB
C
Executable File
/*
|
|
* Sweet is a small library for basic math and small matrix operations.
|
|
* Copyright 2014 Luc Girod.
|
|
*
|
|
* This library is free software: you can redistribute it and/or
|
|
* modify it under the terms of the GNU Lesser General Public
|
|
* License as published by the Free Software Foundation, either
|
|
* version 3 of the License, or (at your option) any later version.
|
|
*
|
|
* This library is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* Lesser General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public
|
|
* License along with this library. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
#include <stdarg.h>
|
|
#include "sweet_math.h"
|
|
#include "sweet_types.h"
|
|
|
|
|
|
vec2
|
|
vec2_zero ()
|
|
{
|
|
vec2 v;
|
|
v.x = 0;
|
|
v.y = 0;
|
|
return v;
|
|
}
|
|
|
|
vec3
|
|
vec3_zero ()
|
|
{
|
|
vec3 v;
|
|
v.x = 0;
|
|
v.y = 0;
|
|
return v;
|
|
}
|
|
|
|
vec4
|
|
vec4_zero ()
|
|
{
|
|
vec4 v;
|
|
v.x = 0;
|
|
v.y = 0;
|
|
return v;
|
|
}
|
|
|
|
vec2
|
|
vec2_new_2f (float a, float b)
|
|
{
|
|
vec2 v;
|
|
v.x = a;
|
|
v.y = b;
|
|
return v;
|
|
}
|
|
|
|
vec3
|
|
vec3_new_3f (float a, float b, float c)
|
|
{
|
|
vec3 v;
|
|
v.x = a;
|
|
v.y = b;
|
|
v.z = c;
|
|
return v;
|
|
}
|
|
|
|
vec4
|
|
vec4_new_4f (float a, float b, float c, float d)
|
|
{
|
|
vec4 v;
|
|
v.x = a;
|
|
v.y = b;
|
|
v.z = c;
|
|
v.w = d;
|
|
|
|
return v;
|
|
}
|
|
|
|
vec2 vec2_new_v3(vec3 w)
|
|
{
|
|
vec2 v;
|
|
v.x = w.x;
|
|
v.y = w.y;
|
|
return v;
|
|
}
|
|
|
|
vec3 vec3_new_v2_1f(vec2 w, float z)
|
|
{
|
|
vec3 v;
|
|
v.x = w.x;
|
|
v.y = w.y;
|
|
v.z = z;
|
|
return v;
|
|
}
|
|
|
|
vec3 vec3_new_v4(vec4 w)
|
|
{
|
|
vec3 v;
|
|
v.x = w.x;
|
|
v.y = w.y;
|
|
v.z = w.z;
|
|
return v;
|
|
}
|
|
|
|
vec4 vec4_new_v2_2f(vec2 u, float z, float w)
|
|
{
|
|
vec4 v;
|
|
v.x = u.x;
|
|
v.y = u.y;
|
|
v.z = z;
|
|
v.w = w;
|
|
return v;
|
|
}
|
|
|
|
vec4 vec4_new_v3_1f(vec3 u, float w)
|
|
{
|
|
vec4 v;
|
|
v.x = u.x;
|
|
v.y = u.y;
|
|
v.z = u.z;
|
|
v.w = w;
|
|
return v;
|
|
}
|
|
|
|
vec4 vec4_new_v4 (vec4 v)
|
|
{
|
|
return v;
|
|
}
|
|
|
|
|
|
float
|
|
vec2_norm (vec2 v)
|
|
{
|
|
return sqrt (v.x * v.x + v.y * v.y);
|
|
}
|
|
|
|
float
|
|
vec3_norm (vec3 v)
|
|
{
|
|
return sqrt (v.x * v.x + v.y * v.y + v.z * v.z);
|
|
}
|
|
|
|
float
|
|
vec4_norm (vec4 v)
|
|
{
|
|
return sqrt (v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
|
|
}
|
|
|
|
float
|
|
vec2_square_norm (vec2 v)
|
|
{
|
|
return (v.x * v.x + v.y * v.y);
|
|
}
|
|
|
|
float
|
|
vec3_square_norm (vec3 v)
|
|
{
|
|
return (v.x * v.x + v.y * v.y + v.z * v.z);
|
|
}
|
|
|
|
float
|
|
vec4_square_norm (vec4 v)
|
|
{
|
|
return (v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
|
|
}
|
|
|
|
float
|
|
vec2_dist (vec2 a, vec2 b)
|
|
{
|
|
float dx = a.x - b.x;
|
|
float dy = a.y - b.y;
|
|
return sqrt (dx * dx + dy * dy);
|
|
}
|
|
|
|
float
|
|
vec3_dist (vec3 a, vec3 b)
|
|
{
|
|
float dx = a.x - b.x;
|
|
float dy = a.y - b.y;
|
|
float dz = a.z - b.z;
|
|
return sqrt (dx * dx + dy * dy + dz * dz);
|
|
}
|
|
|
|
float
|
|
vec4_dist (vec4 a, vec4 b)
|
|
{
|
|
float dx = a.x - b.x;
|
|
float dy = a.y - b.y;
|
|
float dz = a.z - b.z;
|
|
return sqrt (dx * dx + dy * dy + dz * dz);
|
|
}
|
|
|
|
float
|
|
vec2_square_dist (vec2 a, vec2 b)
|
|
{
|
|
float dx = a.x - b.x;
|
|
float dy = a.y - b.y;
|
|
return (dx * dx + dy * dy);
|
|
}
|
|
|
|
float
|
|
vec3_square_dist (vec3 a, vec3 b)
|
|
{
|
|
float dx = a.x - b.x;
|
|
float dy = a.y - b.y;
|
|
float dz = a.z - b.z;
|
|
return (dx * dx + dy * dy + dz * dz);
|
|
}
|
|
|
|
float
|
|
vec4_square_dist (vec4 a, vec4 b)
|
|
{
|
|
float dx = a.x - b.x;
|
|
float dy = a.y - b.y;
|
|
float dz = a.z - b.z;
|
|
return (dx * dx + dy * dy + dz * dz);
|
|
}
|
|
|
|
float
|
|
vec2_dot (vec2 a, vec2 b)
|
|
{
|
|
return (a.x * b.x) + (a.y * b.y);
|
|
}
|
|
|
|
float
|
|
vec3_dot (vec3 a, vec3 b)
|
|
{
|
|
return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
|
|
}
|
|
|
|
float
|
|
vec4_dot (vec4 a, vec4 b)
|
|
{
|
|
return (a.x * b.x) + (a.y * b.y) + (a.z * b.z) + (a.w * b.w);
|
|
}
|
|
|
|
vec3
|
|
cross (vec3 v1, vec3 v2)
|
|
{
|
|
vec3 n;
|
|
n.x = (v1.y * v2.z) - (v1.z * v2.y);
|
|
n.y = -(v1.x * v2.z) + (v1.z * v2.x);
|
|
n.z = (v1.x * v2.y) - (v1.y * v2.x);
|
|
|
|
return n;
|
|
}
|
|
|
|
float
|
|
triple_product (vec3 v1, vec3 v2, vec3 v3)
|
|
{
|
|
v2 = cross (v2, v3);
|
|
|
|
return vec3_dot (v1, v2);
|
|
}
|
|
|
|
vec2
|
|
vec2_normalize (vec2 v)
|
|
{
|
|
vec2 nv;
|
|
|
|
float inv_size = 1.0 / sqrt ((v.x * v.x) + (v.y * v.y));
|
|
|
|
nv.x = v.x * inv_size;
|
|
nv.y = v.y * inv_size;
|
|
|
|
return nv;
|
|
}
|
|
|
|
vec3
|
|
vec3_normalize (vec3 v)
|
|
{
|
|
vec3 nv;
|
|
|
|
float inv_size = 1.0 / sqrt ((v.x * v.x) + (v.y * v.y) + (v.z * v.z));
|
|
|
|
nv.x = v.x * inv_size;
|
|
nv.y = v.y * inv_size;
|
|
nv.z = v.z * inv_size;
|
|
|
|
return nv;
|
|
}
|
|
|
|
vec4
|
|
vec4_normalize (vec4 v)
|
|
{
|
|
vec4 nv;
|
|
|
|
float inv_size = 1.0 / sqrt ((v.x * v.x) + (v.y * v.y) + (v.z * v.z) + (v.w * v.w));
|
|
|
|
nv.x = v.x * inv_size;
|
|
nv.y = v.y * inv_size;
|
|
nv.z = v.z * inv_size;
|
|
nv.w = v.w * inv_size;
|
|
|
|
return nv;
|
|
}
|
|
|
|
|
|
vec2
|
|
vec2_scale (vec2 v, float s)
|
|
{
|
|
v.x *= s;
|
|
v.y *= s;
|
|
|
|
return v;
|
|
}
|
|
|
|
vec3
|
|
vec3_scale (vec3 v, float s)
|
|
{
|
|
v.x *= s;
|
|
v.y *= s;
|
|
v.z *= s;
|
|
|
|
return v;
|
|
}
|
|
|
|
vec4
|
|
vec4_scale (vec4 v, float s)
|
|
{
|
|
v.x *= s;
|
|
v.y *= s;
|
|
v.z *= s;
|
|
v.w *= s;
|
|
|
|
return v;
|
|
}
|
|
|
|
vec2
|
|
vec2_rescale (vec2 v, float size)
|
|
{
|
|
v = vec2_normalize (v);
|
|
v = vec2_scale (v, size);
|
|
|
|
return v;
|
|
}
|
|
|
|
vec3
|
|
vec3_rescale (vec3 v, float size)
|
|
{
|
|
v = vec3_normalize (v);
|
|
v = vec3_scale (v, size);
|
|
|
|
return v;
|
|
}
|
|
|
|
vec4
|
|
vec4_rescale (vec4 v, float size)
|
|
{
|
|
v = vec4_normalize (v);
|
|
v = vec4_scale (v, size);
|
|
|
|
return v;
|
|
}
|
|
|
|
vec2
|
|
vec2_inverse (vec2 v)
|
|
{
|
|
v.x = - v.x;
|
|
v.y = - v.y;
|
|
return v;
|
|
}
|
|
|
|
vec3
|
|
vec3_inverse (vec3 v)
|
|
{
|
|
v.x = - v.x;
|
|
v.y = - v.y;
|
|
v.z = - v.z;
|
|
return v;
|
|
}
|
|
|
|
vec4
|
|
vec4_inverse (vec4 v)
|
|
{
|
|
v.x = - v.x;
|
|
v.y = - v.y;
|
|
v.z = - v.z;
|
|
v.w = - v.w;
|
|
return v;
|
|
}
|
|
|
|
vec2
|
|
vec2_add (vec2 v1, vec2 v2)
|
|
{
|
|
v1.x += v2.x;
|
|
v1.y += v2.y;
|
|
|
|
return v1;
|
|
}
|
|
|
|
vec3
|
|
vec3_add (vec3 v1, vec3 v2)
|
|
{
|
|
v1.x += v2.x;
|
|
v1.y += v2.y;
|
|
v1.z += v2.z;
|
|
|
|
return v1;
|
|
}
|
|
|
|
vec4
|
|
vec4_add (vec4 v1, vec4 v2)
|
|
{
|
|
v1.x += v2.x;
|
|
v1.y += v2.y;
|
|
v1.z += v2.z;
|
|
v1.w += v2.w;
|
|
|
|
return v1;
|
|
}
|
|
|
|
vec2
|
|
vec2_middle (vec2 vector1, vec2 vector2)
|
|
{
|
|
vec2 v;
|
|
v.x = (vector1.x + vector2.x) * 0.5;
|
|
v.y = (vector1.y + vector2.y) * 0.5;
|
|
return v;
|
|
}
|
|
|
|
vec3
|
|
vec3_middle (vec3 vector1, vec3 vector2)
|
|
{
|
|
vec3 v;
|
|
v.x = (vector1.x + vector2.x) * 0.5;
|
|
v.y = (vector1.y + vector2.y) * 0.5;
|
|
v.z = (vector1.z + vector2.z) * 0.5;
|
|
return v;
|
|
}
|
|
|
|
vec4
|
|
vec4_middle (vec4 vector1, vec4 vector2)
|
|
{
|
|
vec4 v;
|
|
v.x = (vector1.x + vector2.x) * 0.5;
|
|
v.y = (vector1.y + vector2.y) * 0.5;
|
|
v.z = (vector1.z + vector2.z) * 0.5;
|
|
v.w = (vector1.w + vector2.w) * 0.5;
|
|
return v;
|
|
}
|
|
|
|
vec2
|
|
vec2_sub (vec2 v1, vec2 v2)
|
|
{
|
|
v1.x -= v2.x;
|
|
v1.y -= v2.y;
|
|
|
|
return v1;
|
|
}
|
|
|
|
vec3
|
|
vec3_sub (vec3 v1, vec3 v2)
|
|
{
|
|
v1.x -= v2.x;
|
|
v1.y -= v2.y;
|
|
v1.z -= v2.z;
|
|
|
|
return v1;
|
|
}
|
|
|
|
vec4
|
|
vec4_sub (vec4 v1, vec4 v2)
|
|
{
|
|
v1.x -= v2.x;
|
|
v1.y -= v2.y;
|
|
v1.z -= v2.z;
|
|
v1.w -= v2.w;
|
|
|
|
return v1;
|
|
}
|
|
|
|
float
|
|
vec2_angle (vec2 a, vec2 b)
|
|
{
|
|
vec2 na = vec2_normalize (a);
|
|
vec2 nb = vec2_normalize (b);
|
|
|
|
float dot = vec2_dot (na, nb);
|
|
|
|
return acos (dot);
|
|
}
|
|
|
|
float
|
|
vec3_angle (vec3 a, vec3 b)
|
|
{
|
|
vec3 na = vec3_normalize (a);
|
|
vec3 nb = vec3_normalize (b);
|
|
|
|
float dot = vec3_dot (na, nb);
|
|
|
|
return acos (dot);
|
|
}
|
|
|
|
vec2
|
|
vec2_random ()
|
|
{
|
|
vec2 v;
|
|
v.x = (float)rand() / (float)RAND_MAX;
|
|
v.y = (float)rand() / (float)RAND_MAX;
|
|
v.x = 2.0 * (v.x - 0.5);
|
|
v.y = 2.0 * (v.y - 0.5);
|
|
return vec2_normalize (v);
|
|
}
|
|
|
|
vec3
|
|
vec3_random ()
|
|
{
|
|
vec3 v;
|
|
v.x = (float)rand() / (float)RAND_MAX;
|
|
v.y = (float)rand() / (float)RAND_MAX;
|
|
v.z = (float)rand() / (float)RAND_MAX;
|
|
|
|
v.x = 2.0 * (v.x - 0.5);
|
|
v.y = 2.0 * (v.y - 0.5);
|
|
v.z = 2.0 * (v.z - 0.5);
|
|
return vec3_normalize (v);
|
|
}
|
|
|
|
vec4
|
|
vec4_random ()
|
|
{
|
|
vec4 v;
|
|
v.x = (float)rand() / (float)RAND_MAX;
|
|
v.y = (float)rand() / (float)RAND_MAX;
|
|
v.z = (float)rand() / (float)RAND_MAX;
|
|
v.w = (float)rand() / (float)RAND_MAX;
|
|
|
|
v.x = 2.0 * (v.x - 0.5);
|
|
v.y = 2.0 * (v.y - 0.5);
|
|
v.z = 2.0 * (v.z - 0.5);
|
|
v.w = 2.0 * (v.w - 0.5);
|
|
return vec4_normalize (v);
|
|
}
|
|
|
|
vec3
|
|
vec3_random_ortho(vec3 v)
|
|
{
|
|
vec3 u = vec3_random ();
|
|
v = cross(u, v);
|
|
return vec3_normalize(v);
|
|
}
|
|
|
|
vec2
|
|
vec2_product (vec2 v1, vec2 v2)
|
|
{
|
|
v1.x *= v2.x;
|
|
v1.y *= v2.y;
|
|
|
|
return v1;
|
|
}
|
|
|
|
vec3
|
|
vec3_product (vec3 v1, vec3 v2)
|
|
{
|
|
v1.x *= v2.x;
|
|
v1.y *= v2.y;
|
|
v1.z *= v2.z;
|
|
|
|
return v1;
|
|
}
|
|
|
|
vec4
|
|
vec4_product (vec4 v1, vec4 v2)
|
|
{
|
|
v1.x *= v2.x;
|
|
v1.y *= v2.y;
|
|
v1.z *= v2.z;
|
|
v1.w *= v2.w;
|
|
|
|
return v1;
|
|
}
|
|
|
|
quaternion
|
|
quat_new (float w, float x, float y, float z)
|
|
{
|
|
quaternion q;
|
|
q.w = w;
|
|
q.x = x;
|
|
q.y = y;
|
|
q.z = z;
|
|
|
|
return q;
|
|
}
|
|
|
|
quaternion
|
|
quat_rotation (float angle, float x, float y, float z)
|
|
{
|
|
quaternion q;
|
|
float s;
|
|
|
|
s = sin (0.5 * angle);
|
|
|
|
q.w = cos (0.5 * angle);
|
|
q.x = x * s;
|
|
q.y = y * s;
|
|
q.z = z * s;
|
|
|
|
return q;
|
|
}
|
|
|
|
quaternion
|
|
quat_conjugate (quaternion q)
|
|
{
|
|
quaternion conjugate;
|
|
|
|
conjugate.x = -q.x;
|
|
conjugate.y = -q.y;
|
|
conjugate.z = -q.z;
|
|
conjugate.w = q.w;
|
|
|
|
return conjugate;
|
|
}
|
|
|
|
quaternion
|
|
quat_add (quaternion q1, quaternion q2)
|
|
{
|
|
quaternion sum;
|
|
|
|
sum.w = q1.w + q2.w;
|
|
sum.x = q1.x + q2.x;
|
|
sum.y = q1.y + q2.y;
|
|
sum.z = q1.z + q2.z;
|
|
|
|
return sum;
|
|
}
|
|
|
|
quaternion
|
|
quat_product (quaternion q1, quaternion q2)
|
|
{
|
|
quaternion prod;
|
|
|
|
prod.w = (q1.w * q2.w) - (q1.x * q2.x) - (q1.y * q2.y) - (q1.z * q2.z);
|
|
prod.x = (q1.w * q2.x) + (q1.x * q2.w) + (q1.y * q2.z) - (q1.z * q2.y);
|
|
prod.y = (q1.w * q2.y) - (q1.x * q2.z) + (q1.y * q2.w) + (q1.z * q2.x);
|
|
prod.z = (q1.w * q2.z) + (q1.x * q2.y) - (q1.y * q2.x) + (q1.z * q2.w);
|
|
|
|
return prod;
|
|
}
|
|
|
|
float
|
|
quat_norm (quaternion q)
|
|
{
|
|
return sqrt ((q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z));
|
|
}
|
|
|
|
|
|
|
|
int
|
|
nearest_integer (int number, int size, ...)
|
|
{
|
|
int i;
|
|
int nearest;
|
|
va_list ap;
|
|
|
|
va_start (ap, size);
|
|
|
|
/* nearest = va_arg (ap, int); */
|
|
|
|
for (i = 0; i < size; i++)
|
|
{
|
|
int arg;
|
|
arg = va_arg (ap, int);
|
|
|
|
if (abs (number - arg) < abs (number - nearest))
|
|
{
|
|
nearest = arg;
|
|
}
|
|
}
|
|
|
|
va_end (ap);
|
|
|
|
return nearest;
|
|
}
|
|
|
|
int
|
|
quadratic_polynomial (vec2 * r, float a, float b, float c)
|
|
{
|
|
float delta;
|
|
float a2;
|
|
|
|
a2 = a * 2;
|
|
delta = b * b - 2*a2 * c;
|
|
|
|
if (delta > 0)
|
|
{
|
|
float d = sqrt (delta);
|
|
r->x = (d - b)/a2;
|
|
r->y = (-d - b)/a2;
|
|
return 2;
|
|
}
|
|
else if (float_approx_zero (delta, EPSILON))
|
|
{
|
|
r->x = -b/a2;
|
|
r->y = r->x;
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
#define FRAC_1_ON_3 0.3333333333333333333333333333333333333333333333333333333333333333333
|
|
#define FRAC_2_ON_3 0.6666666666666666666666666666666666666666666666666666666666666666667
|
|
#define FRAC_1_ON_6 0.1666666666666666666666666666666666666666666666666666666666666666667
|
|
#define FRAC_1_ON_9 0.1111111111111111111111111111111111111111111111111111111111111111111
|
|
#define FRAC_2_ON_27 0.0740740740740740740740740740740740740740740740740740740740740740741
|
|
#define FRAC_1_ON_27 0.0370370370370370370370370370370370370370370370370370370370370370370
|
|
|
|
int
|
|
cubic_polynomial (vec3 * roots, float t, float a, float b, float c)
|
|
{
|
|
double aa;
|
|
double a_over_3;
|
|
double p;
|
|
double q;
|
|
double ppp;
|
|
double D;
|
|
double r;
|
|
double s;
|
|
|
|
a /= t;
|
|
b /= t;
|
|
c /= t;
|
|
|
|
aa = a * a;
|
|
|
|
p = -FRAC_1_ON_9 * aa + FRAC_1_ON_3 * b;
|
|
q = FRAC_1_ON_27 * aa * a - FRAC_1_ON_6 * a * b + 0.5 * c;
|
|
|
|
ppp = p * p * p;
|
|
D = -(ppp + q * q);
|
|
|
|
s = sqrt (-D);
|
|
r = cbrt (-q + s);
|
|
s = cbrt (-q - s);
|
|
|
|
a_over_3 = a/3;
|
|
|
|
if (D < 0)
|
|
{
|
|
roots->x = (r + s) - a_over_3;
|
|
roots->y = -r - a_over_3;
|
|
roots->z = roots->y;
|
|
return 1;
|
|
}
|
|
else if (float_approx_zero (D, EPSILON))
|
|
{
|
|
roots->x = r * 2 - a_over_3;
|
|
roots->y = -r - a_over_3;
|
|
roots->z = roots->y;
|
|
return 2;
|
|
}
|
|
else
|
|
{
|
|
float theta;
|
|
float m2;
|
|
|
|
m2 = 2 * sqrt (-p);
|
|
|
|
theta = FRAC_1_ON_3 * acos (-q / sqrt(-ppp));
|
|
|
|
roots->x = m2 * cos (theta) - a_over_3;
|
|
roots->y = m2 * cos (theta + 2 * PI_OVER_3) - a_over_3;
|
|
roots->z = m2 * cos (theta - 2 * PI_OVER_3) - a_over_3;
|
|
return 3;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|