luc/sweet
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
sweet/sweet_math.c

704 lines
12 KiB
C
Raw Blame History

/*
* 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 <math.h>
#include <stdarg.h>
#include "sweet_math.h"
#include "sweet_types.h"
#define EPSILON 0.0000000001
int
sweet_math_nearest (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 (fabs (number - arg) < fabs (number - nearest))
{
nearest = arg;
}
}
va_end (ap);
return nearest;
}
int
sweet_math_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 (sweet_math_approx_equals (delta, 0, 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.6666666666666666666666666666666666666666666666666666666666666666666
#define FRAC_1_ON_6 0.1666666666666666666666666666666666666666666666666666666666666666666
#define FRAC_1_ON_9 0.1111111111111111111111111111111111111111111111111111111111111111111
#define FRAC_2_ON_27 0.0740740740740740740740740740740740740740740740740740740740740740740
#define FRAC_1_ON_27 0.0370370370370370370370370370370370370370370370370370370370370370370
int
sweet_math_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;
return 1;
}
else if (sweet_math_approx_equals (D, 0, 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 * SWEET_PI_OVER_3) - a_over_3;
roots->z = m2 * cos (theta - 2 * SWEET_PI_OVER_3) - a_over_3;
return 3;
}
return 0;
}
vec2
sweet_vector_new2 (float a, float b)
{
vec2 v;
v.x = a;
v.y = b;
return v;
}
vec3
sweet_vector_new3 (float a, float b, float c)
{
vec3 v;
v.x = a;
v.y = b;
v.z = c;
return v;
}
vec4
sweet_vector_new4 (float a, float b, float c, float d)
{
vec4 v;
v.x = a;
v.y = b;
v.z = c;
v.w = d;
return v;
}
float
sweet_vector_norm2 (vec2 v)
{
return sqrt (v.x * v.x + v.y * v.y);
}
float
sweet_vector_norm3 (vec3 v)
{
return sqrt (v.x * v.x + v.y * v.y + v.z * v.z);
}
float
sweet_vector_norm4 (vec4 v)
{
return sqrt (v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
}
float
sweet_vector_norm2h (vec3 v)
{
v.x = v.x / v.z;
v.y = v.y / v.z;
return sqrt ((v.x * v.x) + (v.y * v.y));
}
float
sweet_vector_norm3h (vec4 v)
{
v.x = v.x / v.w;
v.y = v.y / v.w;
v.z = v.z / v.w;
return sqrt ((v.x * v.x) + (v.y * v.y) + (v.z * v.z));
}
float
sweet_vector_square_norm2 (vec2 v)
{
return (v.x * v.x + v.y * v.y);
}
float
sweet_vector_square_norm3 (vec3 v)
{
return (v.x * v.x + v.y * v.y + v.z * v.z);
}
float
sweet_vector_square_norm4 (vec4 v)
{
return (v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
}
float
sweet_vector_square_norm2h (vec3 v)
{
v.x = v.x / v.z;
v.y = v.y / v.z;
return (v.x * v.x + v.y * v.y);
}
float
sweet_vector_square_norm3h (vec4 v)
{
v.x = v.x / v.w;
v.y = v.y / v.w;
v.z = v.z / v.w;
return (v.x * v.x + v.y * v.y + v.z * v.z);
}
float
sweet_vector_dist2 (vec2 a, vec2 b)
{
float dx = a.x - b.x;
float dy = a.y - b.y;
return sqrt (dx * dx + dy * dy);
}
float
sweet_vector_dist3 (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
sweet_vector_dist4 (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
sweet_vector_square_dist2 (vec2 a, vec2 b)
{
float dx = a.x - b.x;
float dy = a.y - b.y;
return (dx * dx + dy * dy);
}
float
sweet_vector_square_dist3 (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
sweet_vector_square_dist4 (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
sweet_vector_dot2 (vec2 a, vec2 b)
{
return (a.x * b.x) + (a.y * b.y);
}
float
sweet_vector_dot3 (vec3 a, vec3 b)
{
return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
}
float
sweet_vector_dot4 (vec4 a, vec4 b)
{
return (a.x * b.x) + (a.y * b.y) + (a.z * b.z) + (a.w * b.w);
}
float
sweet_vector_dot2h (vec3 a, vec3 b)
{
a.x = a.x / a.z;
a.y = a.y / a.z;
b.x = b.x / b.z;
b.y = b.y / b.z;
return (a.x * b.x) + (a.y * b.y);
}
float
sweet_vector_dot3h (vec4 a, vec4 b)
{
a.x = a.x / a.w;
a.y = a.y / a.w;
a.z = a.z / a.w;
b.x = b.x / b.w;
b.y = b.y / b.w;
b.z = b.z / b.w;
return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
}
vec3
sweet_vector_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;
}
vec4
sweet_vector_crossh (vec4 v1, vec4 v2)
{
vec4 n;
v1.x = v1.x / v1.w;
v1.y = v1.y / v1.w;
v1.z = v1.z / v1.w;
v2.x = v2.x / v2.w;
v2.y = v2.y / v2.w;
v2.z = v2.z / v2.w;
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);
n.w = 1.0;
return n;
}
float
sweet_vector_triple_product (vec3 v1, vec3 v2, vec3 v3)
{
v2 = sweet_vector_cross (v2, v3);
return sweet_vector_dot3 (v1, v2);
}
float
sweet_vector_triple_producth (vec4 v1, vec4 v2, vec4 v3)
{
v2 = sweet_vector_crossh (v2, v3);
return sweet_vector_dot3h (v1, v2);
}
vec2
sweet_vector_normalize2 (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
sweet_vector_normalize3 (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
sweet_vector_normalize4 (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
sweet_vector_scale2 (vec2 v, float s)
{
v.x *= s;
v.y *= s;
return v;
}
vec3
sweet_vector_scale3 (vec3 v, float s)
{
v.x *= s;
v.y *= s;
v.z *= s;
return v;
}
vec4
sweet_vector_scale4 (vec4 v, float s)
{
v.x *= s;
v.y *= s;
v.z *= s;
v.w *= s;
return v;
}
vec2
sweet_vector_rescale2 (vec2 v, float size)
{
v = sweet_vector_normalize2 (v);
v = sweet_vector_scale2 (v, size);
return v;
}
vec3
sweet_vector_rescale3 (vec3 v, float size)
{
v = sweet_vector_normalize3 (v);
v = sweet_vector_scale3 (v, size);
return v;
}
vec4
sweet_vector_rescale4 (vec4 v, float size)
{
v = sweet_vector_normalize4 (v);
v = sweet_vector_scale4 (v, size);
return v;
}
vec2
sweet_vector_add2 (vec2 v1, vec2 v2)
{
v1.x += v2.x;
v1.y += v2.y;
return v1;
}
vec3
sweet_vector_add3 (vec3 v1, vec3 v2)
{
v1.x += v2.x;
v1.y += v2.y;
v1.z += v2.z;
return v1;
}
vec4
sweet_vector_add4 (vec4 v1, vec4 v2)
{
v1.x += v2.x;
v1.y += v2.y;
v1.z += v2.z;
v1.w += v2.w;
return v1;
}
vec2
sweet_vector_middle2 (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
sweet_vector_middle3 (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
sweet_vector_middle4 (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
sweet_vector_sub2 (vec2 v1, vec2 v2)
{
v1.x -= v2.x;
v1.y -= v2.y;
return v1;
}
vec3
sweet_vector_sub3 (vec3 v1, vec3 v2)
{
v1.x -= v2.x;
v1.y -= v2.y;
v1.z -= v2.z;
return v1;
}
vec4
sweet_vector_sub4 (vec4 v1, vec4 v2)
{
v1.x -= v2.x;
v1.y -= v2.y;
v1.z -= v2.z;
v1.w -= v2.w;
return v1;
}
float
sweet_vector_angle2 (vec2 a, vec2 b)
{
vec2 na = sweet_vector_normalize2 (a);
vec2 nb = sweet_vector_normalize2 (b);
float dot = sweet_vector_dot2 (na, nb);
return acos (dot);
}
float
sweet_vector_angle3 (vec3 a, vec3 b)
{
vec3 na = sweet_vector_normalize3 (a);
vec3 nb = sweet_vector_normalize3 (b);
float dot = sweet_vector_dot3 (na, nb);
return acos (dot);
}
vec2
sweet_vector_product2 (vec2 v1, vec2 v2)
{
v1.x *= v2.x;
v1.y *= v2.y;
return v1;
}
vec3
sweet_vector_product3 (vec3 v1, vec3 v2)
{
v1.x *= v2.x;
v1.y *= v2.y;
v1.z *= v2.z;
return v1;
}
vec4
sweet_vector_product4 (vec4 v1, vec4 v2)
{
v1.x *= v2.x;
v1.y *= v2.y;
v1.z *= v2.z;
v1.w *= v2.w;
return v1;
}
quaternion
sweet_quaternion_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
sweet_quaternion_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
sweet_quaternion_conjugate (quaternion q)
{
quaternion conjugate;
conjugate.x = -q.x;
conjugate.y = -q.y;
conjugate.z = -q.z;
conjugate.w = q.w;
return conjugate;
}
quaternion
sweet_quaternion_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
sweet_quaternion_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
sweet_quaternion_norm (quaternion q)
{
return sqrt ((q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z));
}
Reference in New Issue View Git Blame Copy Permalink