From 17fb4378eb0824ee898cea4cf1e6caf0d4b8a3f8 Mon Sep 17 00:00:00 2001 From: Luc Girod Date: Wed, 16 Apr 2014 01:58:26 +0200 Subject: [PATCH] First commit for the free version. --- COPYING.LESSER | 166 +++++++ Makefile | 25 + sweet.h | 29 ++ sweet_math.c | 673 ++++++++++++++++++++++++++ sweet_math.h | 226 +++++++++ sweet_matrix.c | 1218 ++++++++++++++++++++++++++++++++++++++++++++++++ sweet_matrix.h | 169 +++++++ sweet_types.h | 62 +++ 8 files changed, 2568 insertions(+) create mode 100644 COPYING.LESSER create mode 100644 Makefile create mode 100644 sweet.h create mode 100644 sweet_math.c create mode 100644 sweet_math.h create mode 100644 sweet_matrix.c create mode 100644 sweet_matrix.h create mode 100644 sweet_types.h diff --git a/COPYING.LESSER b/COPYING.LESSER new file mode 100644 index 0000000..341c30b --- /dev/null +++ b/COPYING.LESSER @@ -0,0 +1,166 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. + diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..6ae8c96 --- /dev/null +++ b/Makefile @@ -0,0 +1,25 @@ +# Makefile + +UNAME = $(shell uname) +CC = gcc + +PROG = libsweet.so + +OBJS = sweet_math.o sweet_matrix.o + +CFLAGS = -pedantic -static -fPIC +LD = -shared + +all: $(PROG) +$(PROG): $(OBJS) + $(CC) $(LD) $(OBJS) -o $(PROG) + +.c.o: + $(CC) $(CFLAGS) -c $*.c + +clean: + rm -rf $(OBJS) + +mrproper: + rm -rf $(OBJS) $(PROG) + diff --git a/sweet.h b/sweet.h new file mode 100644 index 0000000..e56c25f --- /dev/null +++ b/sweet.h @@ -0,0 +1,29 @@ +/* + * Sweet is a small library for basic math and small matrix operation. + * 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 . + */ + + + +#ifndef SWEET_H +#define SWEET_H + +#include "sweet_types.h" +#include "sweet_math.h" +#include "sweet_matrix.h" + +#endif + diff --git a/sweet_math.c b/sweet_math.c new file mode 100644 index 0000000..f0f86fd --- /dev/null +++ b/sweet_math.c @@ -0,0 +1,673 @@ +/* + * Sweet is a small library for basic math and small matrix operation. + * 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 . + */ + +#include +#include +#include +#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_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)); +} + diff --git a/sweet_math.h b/sweet_math.h new file mode 100644 index 0000000..a12a838 --- /dev/null +++ b/sweet_math.h @@ -0,0 +1,226 @@ +/* + * Sweet is a small library for basic math and small matrix operation. + * 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 . + */ + + + +#ifndef SWEET_MATH_H +#define SWEET_MATH_H + +#include "sweet_types.h" + +/* + * Functions with name finished by 2, 3 or 4 use vec2, vec3, vec4 respectively. + * + * Functions with name finished by 2h 3h use vec3, vec4 repectively but + * treat last component as homogeneous coordinates. + * +**/ + +/** This macro define the PI number with a relative great precision */ +#define SWEET_PI_OVER_3 1.047197551196597746154214 +#define SWEET_PI_OVER_2 1.570796326794896619231321 +#define SWEET_PI 3.141592653589793238462643 +#define SWEET_2PI 6.283185307179586476925286 + +/** RadianToDegree */ +/** @param Radian as float or double */ +/** @return Degree as float */ +#define sweet_math_radian_2_degree(radian) \ + radian * 57.2957795130823208767981548 + +/** DegreeToRadian */ +/** @param Degree as float or double */ +/** @return Radian as float */ +#define sweet_math_degree_2_radian(degree) \ + degree * 0.01745329251994329576923690768488 + +#define sweet_math_approx_equals(a, b, epsilon) \ + (a <= (b+epsilon) && a >= (b-epsilon)) + +/** Invsqrt */ +/** @param x argument of square root as float */ +/** @return 1 over square root as float */ +float sweet_math_invsqrt (float x); + +/** Nearest */ +/** @param number integer to compare as int */ +/** @param nb_args number of arguments as int */ +/** @param ... integer arguments */ +/** @return nearest argument to number */ +int sweet_math_nearest (int number, int nb_args, ...); + +/** Quadratic_polynomial */ +/** @param r 2D Vector for the reals roots as vec2 pointer */ +/** @param a coefficiant of x^2 as float */ +/** @param b coefficiant of x as float */ +/** @param c coefficiant of constant as float */ +/** @return number of roots as int */ +int sweet_math_quadratic_polynomial (vec2 * r, float a, float b, float c); + +/** Cubic_polynomial */ +/** @param r 3D Vector containing the reals roots as vec3 pointer */ +/** @param a coefficiant of x^3 as float */ +/** @param b coefficiant of x^2 as float */ +/** @param c coefficiant of x as float */ +/** @param d coefficiant of constant as float */ +/** @return number of roots as int */ +int sweet_math_cubic_polynomial (vec3 * r, float a, float b, float c, float d); + +/* Vector */ + +/** New */ +/** @param Vector componants as float */ +/** @return Vector */ +vec2 sweet_vector_new2 (float x, float y); +vec3 sweet_vector_new3 (float x, float y, float z); +vec4 sweet_vector_new4 (float x, float y, float z, float w); + +/** Norm */ +/** @param v as vec2, vec3 or vec4*/ +/** @return vector norm or squared norm */ +float sweet_vector_norm2 (vec2 v); +float sweet_vector_norm3 (vec3 v); +float sweet_vector_norm4 (vec4 v); +float sweet_vector_norm2h (vec3 v); +float sweet_vector_norm3h (vec4 v); + +float sweet_vector_square_norm2 (vec2 v); +float sweet_vector_square_norm3 (vec3 v); +float sweet_vector_square_norm4 (vec4 v); +float sweet_vector_square_norm2h (vec3 v); +float sweet_vector_square_norm3h (vec4 v); + +/** Dist */ +/** @param a as vec2, vec3 or vec4 */ +/** @param b as vec2, vec3 or vec4 */ +/** @return distance or squared distance between a and b */ +float sweet_vector_dist2 (vec2 a, vec2 b); +float sweet_vector_dist3 (vec3 a, vec3 b); +float sweet_vector_dist4 (vec4 a, vec4 b); + +float sweet_vector_square_dist2 (vec2 a, vec2 b); +float sweet_vector_square_dist3 (vec3 a, vec3 b); +float sweet_vector_square_dist4 (vec4 a, vec4 b); + + +/** Dot */ +/** @param v1 as vector */ +/** @param v2 as vector */ +/** @return Dot product as float */ +float sweet_vector_dot2 (vec2 vector1, vec2 vector2); +float sweet_vector_dot3 (vec3 vector1, vec3 vector2); +float sweet_vector_dot4 (vec4 vector1, vec4 vector2); + +/** Dot homogeneous */ +/** @param v1 as vector */ +/** @param v2 as vector */ +/** @return Dot product as float */ +float sweet_vector_dot2h (vec3 vector1, vec3 vector2); +float sweet_vector_dot3h (vec4 vector1, vec4 vector2); + +/** Cross */ +/** @param vector1 as vec3 */ +/** @param vector1 as vec3 */ +/** @return Cross product as vec3 */ +vec3 sweet_vector_cross (vec3 vector1, vec3 vector2); + +/** Cross homogeneous */ +/** @param vector1 as vec4 */ +/** @param vector1 as vec4 */ +/** @return Cross product as vec4 */ +vec4 sweet_vector_crossh (vec4 vector1, vec4 vector2); + +/** TripleProduct */ +/** @param vector1 as vec3 */ +/** @param vector2 as vec3 */ +/** @param vector3 as vec3 */ +/** @return v1 x v2 . v3 as float */ +float sweet_vector_triple_product (vec3 vector1, vec3 vector2, vec3 vector3); + +/** TripleProduct */ +/** @param vector1 as vec3 */ +/** @param vector2 as vec3 */ +/** @param vector3 as vec3 */ +/** @return v1 x v2 . v3 as float */ +float sweet_vector_triple_producth (vec4 vector1, vec4 vector2, vec4 vector3); + +/** Normalize */ +/** @param vector as vec2, vec3 or vec4 */ +/** @return Normalized vector */ +vec2 sweet_vector_normalize2 (vec2 vector); +vec3 sweet_vector_normalize3 (vec3 vector); +vec4 sweet_vector_normalize4 (vec4 vector); + +/** Scale */ +/** @param Vector as vector */ +/** @param Scalar as float */ +/** @return Vector * scalar */ +vec2 sweet_vector_scale2 (vec2 vector, float scalar); +vec3 sweet_vector_scale3 (vec3 vector, float scalar); +vec4 sweet_vector_scale4 (vec4 vector, float scalar); + +/** Rescale */ +/** @param Vector as vector */ +/** @param Size as float */ +/** @return Vector / lenght * size */ +vec2 sweet_vector_rescale2 (vec2 vector, float size); +vec3 sweet_vector_rescale3 (vec3 vector, float size); +vec4 sweet_vector_rescale4 (vec4 vector, float size); + +/** Add */ +/** @param Vector1 as vector */ +/** @param Vector2 as vector */ +/** @return vector1 + vector2 */ +vec2 sweet_vector_add2 (vec2 vector1, vec2 vector2); +vec3 sweet_vector_add3 (vec3 vector1, vec3 vector2); +vec4 sweet_vector_add4 (vec4 vector1, vec4 vector2); + +/** Sub */ +/** @param Vector1 as vector */ +/** @param Vector2 as vector */ +/** @return vector1 - vector2 */ +vec2 sweet_vector_sub2 (vec2 vector1, vec2 vector2); +vec3 sweet_vector_sub3 (vec3 vector1, vec3 vector2); +vec4 sweet_vector_sub4 (vec4 vector1, vec4 vector2); + +/** Angle */ +/** @param Vector1 as vector */ +/** @param Vector2 as vector */ +/** @return Angle between the two vector in radian */ +float sweet_vector_angle2 (vec2 vector1, vec2 vector2); +float sweet_vector_angle3 (vec3 vector1, vec3 vector2); + +/** Mult */ +/** @param Vector1 as vector */ +/** @param Vector2 as vector */ +/** @return Direct product of vector1 and vector2 */ +vec2 sweet_vector_product2 (vec2 vector1, vec2 vector2); +vec3 sweet_vector_product3 (vec3 vector1, vec3 vector2); +vec4 sweet_vector_product4 (vec4 vector1, vec4 vector2); + + +/* Quaternion */ +quaternion sweet_quaternion_new (float w, float x, float y, float z); +quaternion sweet_quaternion_rotation (float angle, float x, float y, float z); +quaternion sweet_quaternion_conjugate (quaternion q); +quaternion sweet_quaternion_add (quaternion q1, quaternion q2); +quaternion sweet_quaternion_product (quaternion q1, quaternion q2); +float sweet_quaternion_norm (quaternion q); + +#endif /*MATH_H */ + diff --git a/sweet_matrix.c b/sweet_matrix.c new file mode 100644 index 0000000..e6e91d7 --- /dev/null +++ b/sweet_matrix.c @@ -0,0 +1,1218 @@ +/* + * Sweet is a small library for basic math and small matrix operation. + * 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 . + */ + + + +#include +#include +#include "sweet_matrix.h" +#include "sweet_math.h" +#include "sweet_types.h" + +mat2 +sweet_matrix_null2 () +{ + mat2 m; + + m.v[0] = 0.0; + m.v[1] = 0.0; + + m.v[2] = 0.0; + m.v[3] = 0.0; + + return m; +} + +mat3 +sweet_matrix_null3 () +{ + mat3 m; + + m.v[0] = 0.0; + m.v[1] = 0.0; + m.v[2] = 0.0; + + m.v[3] = 0.0; + m.v[4] = 0.0; + m.v[5] = 0.0; + + m.v[6] = 0.0; + m.v[7] = 0.0; + m.v[8] = 0.0; + + return m; +} + +mat4 +sweet_matrix_null4 () +{ + mat4 m; + + m.v[0] = 0.0; + m.v[1] = 0.0; + m.v[2] = 0.0; + m.v[3] = 0.0; + + m.v[4] = 0.0; + m.v[5] = 0.0; + m.v[6] = 0.0; + m.v[7] = 0.0; + + m.v[8] = 0.0; + m.v[9] = 0.0; + m.v[10] = 0.0; + m.v[11] = 0.0; + + m.v[12] = 0.0; + m.v[13] = 0.0; + m.v[14] = 0.0; + m.v[15] = 0.0; + + return m; +} + + +mat2 +sweet_matrix_identity2 () +{ + mat2 m; + + m.v[0] = 1.0; + m.v[1] = 0.0; + + m.v[2] = 0.0; + m.v[3] = 1.0; + + return m; +} + +mat3 +sweet_matrix_identity3 () +{ + mat3 m; + + m.v[0] = 1.0; + m.v[1] = 0.0; + m.v[2] = 0.0; + + m.v[3] = 0.0; + m.v[4] = 1.0; + m.v[5] = 0.0; + + m.v[6] = 0.0; + m.v[7] = 0.0; + m.v[8] = 1.0; + + return m; +} + +mat4 +sweet_matrix_identity4 () +{ + mat4 m; + + m.v[0] = 1.0; + m.v[1] = 0.0; + m.v[2] = 0.0; + m.v[3] = 0.0; + + m.v[4] = 0.0; + m.v[5] = 1.0; + m.v[6] = 0.0; + m.v[7] = 0.0; + + m.v[8] = 0.0; + m.v[9] = 0.0; + m.v[10] = 1.0; + m.v[11] = 0.0; + + m.v[12] = 0.0; + m.v[13] = 0.0; + m.v[14] = 0.0; + m.v[15] = 1.0; + + return m; +} + +mat4 +sweet_matrix_perspective (double fovy, double aspect, double z_near, double z_far) +{ + mat4 m; + float f; + + fovy = sweet_math_degree_2_radian (fovy); + f = 1.0 / tan (fovy / 2.0); + + m.v[0] = f / aspect; + m.v[1] = 0.0; + m.v[2] = 0.0; + m.v[3] = 0.0; + + m.v[4] = 0.0; + m.v[5] = f; + m.v[6] = 0.0; + m.v[7] = 0.0; + + m.v[8] = 0.0; + m.v[9] = 0.0; + m.v[10] = (z_far + z_near) / (z_near - z_far); + m.v[11] = -1.0; + + m.v[12] = 0.0; + m.v[13] = 0.0; + m.v[14] = (2.0 * z_far * z_near) / (z_near - z_far); + m.v[15] = 0.0; + + return m; +} + +mat4 +sweet_matrix_frustum (float left, float right, + float bottom, float top, + float z_near, float z_far) +{ + mat4 m; + + m.v[0] = 2.0 * z_near / (right - left); + m.v[1] = 0.0; + m.v[2] = 0.0; + m.v[3] = 0.0; + + m.v[4] = 0.0; + m.v[5] = 2.0 * z_near / (top - bottom); + m.v[6] = 0.0; + m.v[7] = 0.0; + + m.v[8] = (right + left) / (right - left); + m.v[9] = (top + bottom) / (top - bottom); + m.v[10] = (z_far + z_near) / (z_near - z_far); + m.v[11] = -1.0; + + m.v[12] = 0.0; + m.v[13] = 0.0; + m.v[14] = (2 * z_far * z_near) / (z_near - z_far); + m.v[15] = 0.0; + + return m; +} + +mat4 +sweet_matrix_ortho (float left, float right, + float bottom, float top, + float zNear, float zFar) +{ + mat4 m; + + m.v[0] = 2/(right - left); + m.v[1] = 0; + m.v[2] = 0; + m.v[3] = 0; + m.v[4] = 0; + m.v[5] = 2/(top - bottom); + m.v[6] = 0; + m.v[7] = 0; + m.v[8] = 0; + m.v[9] = 0; + m.v[10] = -2 / (zFar - zNear); + m.v[11] = 0; + m.v[12] = - (right + left) / (right - left); + m.v[13] = - (top + bottom) / (top - bottom); + m.v[14] = - (zFar + zNear) / (zFar - zNear); + m.v[15] = 1; + + return m; +} + +mat4 +sweet_matrix_look_at (vec3 pos, vec3 dir, vec3 up) +{ + mat4 m; + mat4 t; + vec3 cross; + cross = sweet_vector_cross (up, dir); + + m.v[0] = cross.x; + m.v[1] = up.x; + m.v[2] = -dir.x; + m.v[3] = 0; + + m.v[4] = cross.y; + m.v[5] = up.y; + m.v[6] = -dir.y; + m.v[7] = 0; + + m.v[8] = cross.z; + m.v[9] = up.z; + m.v[10] = -dir.z; + m.v[11] = 0; + + m.v[12] = -pos.x * cross.x - pos.y * cross.y - pos.z * cross.z; + m.v[13] = -pos.x * up.x - pos.y * up.y - pos.z * up.z; + m.v[14] = pos.x * dir.x + pos.y * dir.y + pos.z * dir.z; + m.v[15] = 1; + + return m; +} + +mat3 +sweet_matrix_quat_rotation3 (quaternion q) +{ + mat3 m; + + m.v[0] = - 2 * q.y * q.y - 2 * q.z * q.z + 1; + m.v[1] = 2 * q.x * q.y + 2 * q.w * q.z; + m.v[2] = 2 * q.x * q.z - 2 * q.w * q.y; + + m.v[3] = 2 * q.x * q.y - 2 * q.w * q.z; + m.v[4] = - 2 * q.x * q.x - 2 * q.z * q.z + 1; + m.v[5] = 2 * q.y * q.z + 2 * q.w * q.x; + + m.v[6] = 2 * q.x * q.z + 2 * q.w * q.y; + m.v[7] = 2 * q.y * q.z - 2 * q.w * q.x; + m.v[8] = - 2 * q.x * q.x - 2 * q.y * q.y; + + return m; +} + +mat4 +sweet_matrix_quat_rotation3h (quaternion q) +{ + mat4 m; + + m.v[0] = - 2 * q.y * q.y - 2 * q.z * q.z + 1; + m.v[1] = 2 * q.x * q.y + 2 * q.w * q.z; + m.v[2] = 2 * q.x * q.z - 2 * q.w * q.y; + m.v[3] = 0; + + m.v[4] = 2 * q.x * q.y - 2 * q.w * q.z; + m.v[5] = - 2 * q.x * q.x - 2 * q.z * q.z + 1; + m.v[6] = 2 * q.y * q.z + 2 * q.w * q.x; + m.v[7] = 0; + + m.v[8] = 2 * q.x * q.z + 2 * q.w * q.y; + m.v[9] = 2 * q.y * q.z - 2 * q.w * q.x; + m.v[10] = - 2 * q.x * q.x - 2 * q.y * q.y; + m.v[11] = 0; + + m.v[12] = 0; + m.v[13] = 0; + m.v[14] = 0; + m.v[15] = 1; + + return m; +} + +mat4 +sweet_matrix_rotation3h (float angle, float x, float y, float z) +{ + mat4 m; + + m.v[0] = x * x * (1 - cos (angle)) + cos (angle); + m.v[1] = x * y * (1 - cos (angle)) + z * sin (angle); + m.v[2] = x * z * (1 - cos (angle)) - y * sin (angle); + m.v[3] = 0; + + m.v[4] = x * y * (1 - cos (angle)) - z * sin (angle); + m.v[5] = y * y * (1 - cos (angle)) + cos (angle); + m.v[6] = y * z * (1 - cos (angle)) + x * sin (angle); + m.v[7] = 0; + + m.v[8] = x * z * (1 - cos (angle)) + y * sin (angle); + m.v[9] = y * z * (1 - cos (angle)) - x * sin (angle); + m.v[10] = z * z * (1 - cos (angle)) + cos (angle); + m.v[11] = 0; + + m.v[12] = 0; + m.v[13] = 0; + m.v[14] = 0; + m.v[15] = 1; + + return m; +} + +mat3 +sweet_matrix_rotation3 (float angle, float x, float y, float z) +{ + mat3 m; + + m.v[0] = x * x * (1 - cos (angle)) + cos (angle); + m.v[1] = x * y * (1 - cos (angle)) + z * sin (angle); + m.v[2] = x * z * (1 - cos (angle)) - y * sin (angle); + + m.v[3] = x * y * (1 - cos (angle)) - z * sin (angle); + m.v[4] = y * y * (1 - cos (angle)) + cos (angle); + m.v[5] = y * z * (1 - cos (angle)) + x * sin (angle); + + m.v[6] = x * z * (1 - cos (angle)) + y * sin (angle); + m.v[7] = y * z * (1 - cos (angle)) - x * sin (angle); + m.v[8] = z * z * (1 - cos (angle)) + cos (angle); + + return m; +} + +mat2 +sweet_matrix_rotation2 (float angle) +{ + mat2 m; + + m.v[0] = cos (angle); + m.v[1] = sin (angle); + m.v[2] = -sin (angle); + m.v[3] = cos (angle); + + return m; +} + +mat3 +sweet_matrix_rotation2h (float angle) +{ + mat3 m; + + m.v[0] = cos (angle); + m.v[1] = sin (angle); + m.v[2] = 0.0; + m.v[3] = -sin (angle); + m.v[4] = cos (angle); + m.v[5] = 0.0; + m.v[6] = 0.0; + m.v[7] = 0.0; + m.v[8] = 1.0; + + return m; +} + +mat4 +sweet_matrix_translation3h (vec3 v) +{ + mat4 m; + + m.v[0] = 1.0; + m.v[1] = 0.0; + m.v[2] = 0.0; + m.v[3] = 0.0; + + m.v[4] = 0.0; + m.v[5] = 1.0; + m.v[6] = 0.0; + m.v[7] = 0.0; + + m.v[8] = 0.0; + m.v[9] = 0.0; + m.v[10] = 1.0; + m.v[11] = 0.0; + + m.v[12] = v.x; + m.v[13] = v.y; + m.v[14] = v.z; + m.v[15] = 1.0; + + return m; +} + +mat3 +sweet_matrix_translation2h (vec2 v) +{ + mat3 m; + + m.v[0] = 1.0; + m.v[1] = 0.0; + m.v[2] = 0.0; + + m.v[4] = 0.0; + m.v[5] = 1.0; + m.v[6] = 0.0; + + m.v[8] = v.x; + m.v[9] = v.y; + m.v[10] = 1.0; + + return m; +} + +mat4 +sweet_matrix_scale4 (float x, float y, float z, float w) +{ + mat4 m; + + m.v[0] = x; + m.v[1] = 0; + m.v[2] = 0; + m.v[3] = 0; + + m.v[4] = 0; + m.v[5] = y; + m.v[6] = 0; + m.v[7] = 0; + + m.v[8] = 0; + m.v[9] = 0; + m.v[10] = z; + m.v[11] = 0; + + m.v[12] = 0; + m.v[13] = 0; + m.v[14] = 0; + m.v[15] = w; + + return m; +} + +mat3 +sweet_matrix_scale3 (float x, float y, float z) +{ + mat3 m; + + m.v[0] = x; + m.v[1] = 0; + m.v[2] = 0; + + m.v[3] = 0; + m.v[4] = y; + m.v[5] = 0; + + m.v[6] = 0; + m.v[7] = 0; + m.v[8] = z; + + return m; +} + +mat2 +sweet_matrix_scale2 (float x, float y) +{ + mat2 m; + + m.v[0] = x; + m.v[1] = 0; + + m.v[2] = 0; + m.v[3] = y; + + return m; +} + +mat4 +sweet_matrix_texture_bias () +{ + mat4 m; + m.v[0] = 0.5; + m.v[1] = 0.0; + m.v[2] = 0.0; + m.v[3] = 0.0; + + m.v[4] = 0.0; + m.v[5] = 0.5; + m.v[6] = 0.0; + m.v[7] = 0.0; + + m.v[8] = 0.0; + m.v[9] = 0.0; + m.v[10] = 0.5; + m.v[11] = 0.0; + + m.v[12] = 0.5; + m.v[13] = 0.5; + m.v[14] = 0.5; + m.v[15] = 1.0; + + return m; +} + +void +sweet_matrix_transpose2 (mat2 * mt, mat2 * m) +{ + mt->v[0] = m->v[0]; + mt->v[1] = m->v[2]; + mt->v[2] = m->v[1]; + mt->v[3] = m->v[3]; +} + +void +sweet_matrix_transpose3 (mat3 * mt, mat3 * m) +{ + mt->v[0] = m->v[0]; + mt->v[1] = m->v[3]; + mt->v[2] = m->v[6]; + mt->v[3] = m->v[1]; + mt->v[4] = m->v[4]; + mt->v[5] = m->v[7]; + mt->v[6] = m->v[2]; + mt->v[7] = m->v[5]; + mt->v[8] = m->v[8]; +} + +void +sweet_matrix_transpose4 (mat4 * mt, mat4 * m) +{ + mt->v[0] = m->v[0]; + mt->v[1] = m->v[4]; + mt->v[2] = m->v[8]; + mt->v[3] = m->v[12]; + mt->v[4] = m->v[2]; + mt->v[5] = m->v[5]; + mt->v[6] = m->v[9]; + mt->v[7] = m->v[13]; + mt->v[8] = m->v[2]; + mt->v[9] = m->v[6]; + mt->v[10] = m->v[10]; + mt->v[11] = m->v[14]; + mt->v[12] = m->v[3]; + mt->v[13] = m->v[7]; + mt->v[14] = m->v[11]; + mt->v[15] = m->v[15]; +} + +float +sweet_matrix_det2 (mat2 * m) +{ + return m->v[0] * m->v[3] - m->v[1] * m->v[2]; +} + +float +sweet_matrix_det3 (mat3 * m) +{ + float det = + m->v[0] * (m->v[4] * m->v[8] - m->v[5] * m->v[7]) - + m->v[1] * (m->v[3] * m->v[8] - m->v[5] * m->v[6]) + + m->v[2] * (m->v[3] * m->v[7] - m->v[4] * m->v[6]); + + return det; +} + +float +sweet_matrix_det4 (mat4 * m) +{ + float det = + m->v[0] * (m->v[ 5] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) - + m->v[ 6] * (m->v[ 9] * m->v[15] - m->v[11] * m->v[13]) + + m->v[ 7] * (m->v[ 9] * m->v[14] - m->v[10] * m->v[13])) + -m->v[1] * (m->v[ 4] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) - + m->v[ 6] * (m->v[ 8] * m->v[15] - m->v[11] * m->v[12]) + + m->v[ 7] * (m->v[ 8] * m->v[14] - m->v[10] * m->v[12])) + +m->v[2] * (m->v[ 4] * (m->v[ 9] * m->v[15] - m->v[11] * m->v[13]) - + m->v[ 5] * (m->v[ 8] * m->v[15] - m->v[11] * m->v[12]) + + m->v[ 7] * (m->v[ 8] * m->v[13] - m->v[ 9] * m->v[12])) + -m->v[3] * (m->v[ 4] * (m->v[ 9] * m->v[14] - m->v[10] * m->v[13]) - + m->v[ 5] * (m->v[ 8] * m->v[14] - m->v[10] * m->v[12]) + + m->v[ 6] * (m->v[ 8] * m->v[13] - m->v[ 9] * m->v[12])); + + return det; +} + +void +sweet_matrix_inverse2 (mat2 * inv, mat2 * m) +{ + float invdet = 1.0 / sweet_matrix_det2 (m); + + inv->v[0] = m->v[3] * invdet; + inv->v[1] = m->v[1] * (-invdet); + inv->v[2] = m->v[2] * (-invdet); + inv->v[3] = m->v[0] * invdet; +} + +void +sweet_matrix_inverse3 (mat3 * inv, mat3 * m) +{ + float invdet; + + invdet = 1.0 / sweet_matrix_det3 (m); + + inv->v[0] = ((m->v[4] * m->v[8]) - (m->v[5] * m->v[7])) * invdet; + inv->v[1] = (-(m->v[1] * m->v[8]) + (m->v[2] * m->v[7])) * invdet; + inv->v[2] = ((m->v[1] * m->v[5]) - (m->v[2] * m->v[4])) * invdet; + + inv->v[3] = (-(m->v[3] * m->v[8]) + (m->v[5] * m->v[6])) * invdet; + inv->v[4] = ((m->v[0] * m->v[8]) - (m->v[2] * m->v[6])) * invdet; + inv->v[5] = (-(m->v[0] * m->v[5]) + (m->v[2] * m->v[3])) * invdet; + + inv->v[6] = ((m->v[3] * m->v[7]) - (m->v[4] * m->v[6])) * invdet; + inv->v[7] = (-(m->v[0] * m->v[7]) + (m->v[1] * m->v[6])) * invdet; + inv->v[8] = ((m->v[0] * m->v[4]) - (m->v[1] * m->v[3])) * invdet; +} + +static void +matrix_inverse4 (mat4 * inv, mat4 * m) +{ + float invdet = 1.0 / sweet_matrix_det4 (m); + + inv->v[0] = m->v[5] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) - + m->v[6] * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) + + m->v[7] * (m->v[9] * m->v[14] - m->v[10] * m->v[13]); + + inv->v[1] = -m->v[1] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) + + m->v[2] * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) - + m->v[3] * (m->v[9] * m->v[14] - m->v[10] * m->v[13]); + + inv->v[2] = m->v[1] * (m->v[6] * m->v[15] - m->v[7] * m->v[14]) - + m->v[2] * (m->v[5] * m->v[15] - m->v[7] * m->v[13]) + + m->v[3] * (m->v[5] * m->v[14] - m->v[6] * m->v[13]); + + inv->v[3] = -m->v[1] * (m->v[6] * m->v[11] - m->v[7] * m->v[10]) + + m->v[2] * (m->v[5] * m->v[11] - m->v[7] * m->v[9]) - + m->v[3] * (m->v[5] * m->v[10] - m->v[6] * m->v[9]); + + inv->v[4] = -m->v[4] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) + + m->v[6] * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) - + m->v[7] * (m->v[8] * m->v[14] - m->v[10] * m->v[12]); + + inv->v[5] = m->v[0] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) - + m->v[2] * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) + + m->v[3] * (m->v[8] * m->v[14] - m->v[10] * m->v[12]); + + inv->v[6] = -m->v[0] * (m->v[6] * m->v[15] - m->v[7] * m->v[14]) + + m->v[2] * (m->v[4] * m->v[15] - m->v[7] * m->v[12]) - + m->v[3] * (m->v[4] * m->v[14] - m->v[6] * m->v[12]); + + inv->v[7] = m->v[0] * (m->v[6] * m->v[11] - m->v[7] * m->v[10]) - + m->v[2] * (m->v[4] * m->v[11] - m->v[7] * m->v[8]) + + m->v[3] * (m->v[4] * m->v[10] - m->v[6] * m->v[8]); + + inv->v[8] = m->v[4] * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) - + m->v[5] * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) + + m->v[7] * (m->v[8] * m->v[13] - m->v[9] * m->v[12]); + + inv->v[9] = -m->v[0] * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) + + m->v[1] * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) - + m->v[3] * (m->v[8] * m->v[13] - m->v[9] * m->v[12]); + + inv->v[10] = m->v[0] * (m->v[5] * m->v[15] - m->v[7] * m->v[13]) - + m->v[1] * (m->v[4] * m->v[15] - m->v[7] * m->v[12]) + + m->v[3] * (m->v[4] * m->v[13] - m->v[5] * m->v[12]); + + inv->v[11] = -m->v[0] * (m->v[5] * m->v[11] - m->v[7] * m->v[9]) + + m->v[1] * (m->v[4] * m->v[11] - m->v[7] * m->v[8]) - + m->v[3] * (m->v[4] * m->v[9] - m->v[5] * m->v[8]); + + inv->v[12] = -m->v[4] * (m->v[9] * m->v[14] - m->v[10] * m->v[13]) + + m->v[5] * (m->v[8] * m->v[14] - m->v[10] * m->v[12]) - + m->v[6] * (m->v[8] * m->v[13] - m->v[9] * m->v[12]); + + inv->v[13] = m->v[0] * (m->v[9] * m->v[14] - m->v[10] * m->v[13]) - + m->v[1] * (m->v[8] * m->v[14] - m->v[10] * m->v[12]) + + m->v[2] * (m->v[8] * m->v[13] - m->v[9] * m->v[12]); + + inv->v[14] = -m->v[0] * (m->v[5] * m->v[14] - m->v[6] * m->v[13]) + + m->v[1] * (m->v[4] * m->v[14] - m->v[6] * m->v[12]) - + m->v[2] * (m->v[4] * m->v[13] - m->v[5] * m->v[12]); + + inv->v[15] = m->v[0] * (m->v[5] * m->v[10] - m->v[6] * m->v[9]) - + m->v[1] * (m->v[4] * m->v[10] - m->v[6] * m->v[8]) + + m->v[2] * (m->v[4] * m->v[9] - m->v[5] * m->v[8]); + + inv->v[ 0] *= invdet; + inv->v[ 1] *= invdet; + inv->v[ 2] *= invdet; + inv->v[ 3] *= invdet; + inv->v[ 4] *= invdet; + inv->v[ 5] *= invdet; + inv->v[ 6] *= invdet; + inv->v[ 7] *= invdet; + inv->v[ 8] *= invdet; + inv->v[ 9] *= invdet; + inv->v[10] *= invdet; + inv->v[11] *= invdet; + inv->v[12] *= invdet; + inv->v[13] *= invdet; + inv->v[14] *= invdet; + inv->v[15] *= invdet; +} + +void +sweet_matrix_inverse4 (mat4 * inv, mat4 * m) +{ + float detp; + float invdet; + mat2 rp; + mat2 pq; + mat2 rpq; + mat2 s; + mat2 p; + + detp = m->v[0] * m->v[5] - m->v[1] * m->v[4]; + + if (sweet_math_approx_equals (detp, 0, 0.0000001)) + { + /* In this case the standard algorithm is used */ + matrix_inverse4 (m, inv); + return; + } + + /* In this case we can use a better approach */ + + invdet = 1.0 / detp; + + p.v[0] = m->v[5] * invdet; + p.v[1] = -m->v[1] * invdet; + p.v[2] = -m->v[4] * invdet; + p.v[3] = m->v[0] * invdet; + + rp.v[0] = (p.v[0] * m->v[2] + p.v[1] * m->v[6]); + rp.v[1] = (p.v[0] * m->v[3] + p.v[1] * m->v[7]); + rp.v[2] = (p.v[2] * m->v[2] + p.v[3] * m->v[6]); + rp.v[3] = (p.v[2] * m->v[3] + p.v[3] * m->v[7]); + + pq.v[0] = (p.v[0] * m->v[8] + p.v[2] * m->v[9]); + pq.v[1] = (p.v[1] * m->v[8] + p.v[3] * m->v[9]); + pq.v[2] = (p.v[0] * m->v[12] + p.v[2] * m->v[13]); + pq.v[3] = (p.v[1] * m->v[12] + p.v[3] * m->v[13]); + + rpq.v[0] = rp.v[0] * m->v[8] + rp.v[2] * m->v[9]; + rpq.v[1] = rp.v[1] * m->v[8] + rp.v[3] * m->v[9]; + rpq.v[2] = rp.v[0] * m->v[12] + rp.v[2] * m->v[13]; + rpq.v[3] = rp.v[1] * m->v[12] + rp.v[3] * m->v[13]; + + s.v[0] = m->v[10] - rpq.v[0]; + s.v[1] = m->v[11] - rpq.v[1]; + s.v[2] = m->v[14] - rpq.v[2]; + s.v[3] = m->v[15] - rpq.v[3]; + + invdet = 1.0 / (s.v[0] * s.v[3] - s.v[1] * s.v[2]); + + inv->v[10] = s.v[3] * invdet; + inv->v[11] = - s.v[1] * invdet; + inv->v[14] = - s.v[2] * invdet; + inv->v[15] = s.v[0] * invdet; + + inv->v[2] = -inv->v[10] * rp.v[0] - m->v[14] * rp.v[1]; + inv->v[3] = -inv->v[11] * rp.v[0] - m->v[15] * rp.v[1]; + inv->v[6] = -inv->v[10] * rp.v[2] - m->v[14] * rp.v[3]; + inv->v[7] = -inv->v[11] * rp.v[2] - m->v[15] * rp.v[3]; + + inv->v[8] = -pq.v[0] * inv->v[10] - pq.v[2] * inv->v[11]; + inv->v[9] = -pq.v[1] * inv->v[10] - pq.v[3] * inv->v[11]; + inv->v[12] = -pq.v[0] * inv->v[14] - pq.v[2] * inv->v[15]; + inv->v[13] = -pq.v[1] * inv->v[14] - pq.v[3] * inv->v[15]; + + inv->v[0] = p.v[0] - (pq.v[0] * inv->v[2] + pq.v[2] * inv->v[3]); + inv->v[1] = p.v[1] - (pq.v[1] * inv->v[2] + pq.v[3] * inv->v[3]); + inv->v[4] = p.v[2] - (pq.v[0] * inv->v[6] + pq.v[2] * inv->v[7]); + inv->v[5] = p.v[3] - (pq.v[1] * inv->v[6] + pq.v[3] * inv->v[7]); +} + +void +sweet_matrix_product2 (mat2 * p, mat2 * a, mat2 * b) +{ + p->v[0] = a->v[0] * b->v[0] + a->v[2] * b->v[1]; + p->v[1] = a->v[1] * b->v[0] + a->v[3] * b->v[1]; + p->v[2] = a->v[0] * b->v[2] + a->v[2] * b->v[3]; + p->v[3] = a->v[1] * b->v[2] + a->v[3] * b->v[3]; +} + +void +sweet_matrix_product3 (mat3 * p, mat3 * a, mat3 * b) +{ + p->v[0] = a->v[0] * b->v[0] + a->v[3] * b->v[1] + a->v[6] * b->v[2]; + p->v[1] = a->v[1] * b->v[0] + a->v[4] * b->v[1] + a->v[7] * b->v[2]; + p->v[2] = a->v[2] * b->v[0] + a->v[5] * b->v[1] + a->v[8] * b->v[2]; + + p->v[3] = a->v[0] * b->v[3] + a->v[3] * b->v[4] + a->v[6] * b->v[5]; + p->v[4] = a->v[1] * b->v[3] + a->v[4] * b->v[4] + a->v[7] * b->v[5]; + p->v[5] = a->v[2] * b->v[3] + a->v[5] * b->v[4] + a->v[8] * b->v[5]; + + p->v[6] = a->v[0] * b->v[6] + a->v[3] * b->v[7] + a->v[6] * b->v[8]; + p->v[7] = a->v[1] * b->v[6] + a->v[4] * b->v[7] + a->v[7] * b->v[8]; + p->v[8] = a->v[2] * b->v[6] + a->v[5] * b->v[7] + a->v[8] * b->v[8]; +} + +void +sweet_matrix_product4 (mat4 * p, mat4 * a, mat4 * b) +{ + p->v[0] = a->v[0] * b->v[0] + a->v[4] * b->v[1] + a->v[8] * b->v[2] + a->v[12] * b->v[3]; + p->v[1] = a->v[1] * b->v[0] + a->v[5] * b->v[1] + a->v[9] * b->v[2] + a->v[13] * b->v[3]; + p->v[2] = a->v[2] * b->v[0] + a->v[6] * b->v[1] + a->v[10] * b->v[2] + a->v[14] * b->v[3]; + p->v[3] = a->v[3] * b->v[0] + a->v[7] * b->v[1] + a->v[11] * b->v[2] + a->v[15] * b->v[3]; + + p->v[4] = a->v[0] * b->v[4] + a->v[4] * b->v[5] + a->v[8] * b->v[6] + a->v[12] * b->v[7]; + p->v[5] = a->v[1] * b->v[4] + a->v[5] * b->v[5] + a->v[9] * b->v[6] + a->v[13] * b->v[7]; + p->v[6] = a->v[2] * b->v[4] + a->v[6] * b->v[5] + a->v[10] * b->v[6] + a->v[14] * b->v[7]; + p->v[7] = a->v[3] * b->v[4] + a->v[7] * b->v[5] + a->v[11] * b->v[6] + a->v[15] * b->v[7]; + + p->v[8] = a->v[0] * b->v[8] + a->v[4] * b->v[9] + a->v[8] * b->v[10] + a->v[12] * b->v[11]; + p->v[9] = a->v[1] * b->v[8] + a->v[5] * b->v[9] + a->v[9] * b->v[10] + a->v[13] * b->v[11]; + p->v[10] = a->v[2] * b->v[8] + a->v[6] * b->v[9] + a->v[10] * b->v[10] + a->v[14] * b->v[11]; + p->v[11] = a->v[3] * b->v[8] + a->v[7] * b->v[9] + a->v[11] * b->v[10] + a->v[15] * b->v[11]; + + p->v[12] = a->v[0] * b->v[12] + a->v[4] * b->v[13] + a->v[8] * b->v[14] + a->v[12] * b->v[15]; + p->v[13] = a->v[1] * b->v[12] + a->v[5] * b->v[13] + a->v[9] * b->v[14] + a->v[13] * b->v[15]; + p->v[14] = a->v[2] * b->v[12] + a->v[6] * b->v[13] + a->v[10] * b->v[14] + a->v[14] * b->v[15]; + p->v[15] = a->v[3] * b->v[12] + a->v[7] * b->v[13] + a->v[11] * b->v[14] + a->v[15] * b->v[15]; +} + +void +sweet_matrix_sub3 (mat2 * sub, mat3 * m) +{ + sub->v[0] = m->v[0]; + sub->v[1] = m->v[1]; + sub->v[2] = m->v[4]; + sub->v[3] = m->v[5]; +} + +void +sweet_matrix_sub4 (mat3 * sub, mat4 * m) +{ + sub->v[0] = m->v[0]; + sub->v[1] = m->v[1]; + sub->v[2] = m->v[2]; + sub->v[3] = m->v[4]; + sub->v[4] = m->v[5]; + sub->v[5] = m->v[6]; + sub->v[6] = m->v[8]; + sub->v[7] = m->v[9]; + sub->v[8] = m->v[10]; +} + +vec2 +sweet_matrix_column2 (mat2 * m, int idx) +{ + vec2 v; + idx = idx * 2; + v.x = m->v[idx]; + v.y = m->v[idx+1]; + + return v; +} + +vec3 +sweet_matrix_column3 (mat3 * m, int idx) +{ + vec3 v; + idx = idx * 3; + v.x = m->v[idx]; + v.y = m->v[idx+1]; + v.z = m->v[idx+2]; + + return v; +} + +vec4 +sweet_matrix_column4 (mat4 * m, int idx) +{ + vec4 v; + idx = idx * 4; + v.x = m->v[idx]; + v.y = m->v[idx+1]; + v.z = m->v[idx+2]; + v.w = m->v[idx+3]; + + return v; +} + +vec2 +sweet_matrix_mult2 (mat2 * mat, vec2 vec) +{ + vec2 result; + + result.x = mat->v[0] * vec.x + mat->v[2] * vec.y; + result.y = mat->v[1] * vec.x + mat->v[3] * vec.y; + + return result; +} + +vec3 +sweet_matrix_mult3 (mat3 * mat, vec3 vec) +{ + vec3 r; + + r.x = (mat->v[0] * vec.x) + (mat->v[3] * vec.y) + (mat->v[6] * vec.z); + r.y = (mat->v[1] * vec.x) + (mat->v[4] * vec.y) + (mat->v[7] * vec.z); + r.z = (mat->v[2] * vec.x) + (mat->v[5] * vec.y) + (mat->v[8] * vec.z); + + return r; +} + +vec4 +sweet_matrix_mult4 (mat4 * mat, vec4 vec) +{ + vec4 result; + + result.x = (mat->v[0] * vec.x) + (mat->v[4] * vec.y) + (mat->v[8] * vec.z) + (mat->v[12] * vec.w); + result.y = (mat->v[1] * vec.x) + (mat->v[5] * vec.y) + (mat->v[9] * vec.z) + (mat->v[13] * vec.w); + result.z = (mat->v[2] * vec.x) + (mat->v[6] * vec.y) + (mat->v[10] * vec.z) + (mat->v[14] * vec.w); + result.w = (mat->v[3] * vec.x) + (mat->v[7] * vec.y) + (mat->v[11] * vec.z) + (mat->v[15] * vec.w); + + return result; +} + +int +sweet_matrix_eigen_value3 (float * h1, float * h2, float * h3, mat3 * m) +{ + float a; + float b; + float c; + int n; + vec3 v; + + a = m->v[0] + m->v[4] + m->v[8]; + + b = m->v[1] * m->v[3] + m->v[2] * m->v[6] + m->v[7] * m->v[5] - + m->v[0] * m->v[4] - m->v[0] * m->v[8] - m->v[4] * m->v[8]; + + c = m->v[0] * m->v[4] * m->v[8] - m->v[0] * m->v[7] * m->v[5] - + m->v[1] * m->v[3] * m->v[8] + m->v[1] * m->v[6] * m->v[5] + + m->v[2] * m->v[3] * m->v[7] - m->v[2] * m->v[6] * m->v[4]; + + n = sweet_math_cubic_polynomial (&v, -1, a, b, c); + *h1 = v.x; + *h2 = v.y; + *h3 = v.z; + return n; +} + +/** +|a b c x| +|d e f y| +|g h i z| +*/ + +/**/ +static int find_pivot (float * max, mat3 * a, int k) +{ + int i_max; + + if (k == 0) + { + *max = fabs (a->v[k]); i_max = 0; + if (fabs (a->v[k+1]) > *max) { *max = fabs (a->v[k+1]); i_max = 1;} + if (fabs (a->v[k+2]) > *max) { *max = fabs (a->v[k+2]); i_max = 2;} + } + else if (k == 1) + { + k = 4; + *max = fabs (a->v[k]); i_max = 1; + if (fabs (a->v[k+1]) > *max) { *max = fabs (a->v[k+1]); i_max = 2;} + } + else + { + k = 8; + *max = fabs (a->v[k]); + i_max = 2; + } + return i_max; +} + +static void swap_rows (mat3 * a, float * s, int l1, int l2) +{ + float tmp; + if (l1 == l2) { return; } + + /* swap left part */ + tmp = s[l1]; + s[l1] = s[l2]; + s[l2] = tmp; + + /* swap first column */ + tmp = a->v[l1]; + a->v[l1] = a->v[l2]; + a->v[l2] = tmp; + + /* swap second column */ + l1 += 3; + l2 += 3; + tmp = a->v[l1]; + a->v[l1] = a->v[l2]; + a->v[l2] = tmp; + + /* swap third column */ + l1 += 3; + l2 += 3; + tmp = a->v[l1]; + a->v[l1] = a->v[l2]; + a->v[l2] = tmp; +} + +static void normalize_row (mat3 * a, float * s, int l, int i) +{ + float d = a->v[l+i*3]; + a->v[l+i*3] = 1.0; + + s[l] = s[l] / d; + if (i <= 1) { a->v[l+6] = a->v[l+6] / d; } + if (i == 0) { a->v[l+3] = a->v[l+3] / d; } +} + +static void substract_row (mat3 * a, float * s, int l1, int l2, int i) +{ + float r; + if (i == 0) + { + r = - a->v[l2]; + a->v[l2] = 0; + a->v[l2+3] = a->v[l2+3] + a->v[l1+3] * r; + a->v[l2+6] = a->v[l2+6] + a->v[l1+6] * r; + } + else if (i == 1) + { + r = - a->v[l2+3]; + a->v[l2+3] = 0; + a->v[l2+6] = a->v[l2+6] + a->v[l1+6] * r; + } + else if (i == 2) + { + r = - a->v[l2+6]; + a->v[l2+6] = 0; + } + + s[l2] = s[l2] + s[l1] * r; + +} + +#define EPSILON 0.00005 + +int +sweet_matrix_gaussian_elimination3 (mat3 * a, vec3 * v) +{ + float s[3]; + float max; + int i_max; + + s[0] = v->x; + s[1] = v->y; + s[2] = v->z; + + i_max = find_pivot (&max, a, 0); + /* If matrix is singular return error */ + if (sweet_math_approx_equals (max, 0, EPSILON)) { return -1; } + swap_rows (a, s, 0, i_max); + normalize_row (a, s, 0, 0); + substract_row (a, s, 0, 1, 0); + substract_row (a, s, 0, 2, 0); + + i_max = find_pivot (&max, a, 1); + /* If matrix is singular return error */ + if (sweet_math_approx_equals (max, 0, EPSILON)) { return -1; } + swap_rows (a, s, 1, i_max); + normalize_row (a, s, 1, 1); + substract_row (a, s, 1, 2, 1); + + /* If matrix is singular return error */ + if (sweet_math_approx_equals (a->v[8], 0, EPSILON)) { return -1; } + normalize_row (a, s, 2, 2); + + v->x = s[0]; + v->y = s[1]; + v->z = s[2]; + return 0; +} + +int +sweet_matrix_solve3 (mat3 * m, vec3 * v) +{ + mat3 a = *m; + if (sweet_matrix_gaussian_elimination3 (&a, v) < 0) { return -1; } + + v->y = v->y - v->z * a.v[7]; + v->x = v->x - v->z * a.v[6] - v->y * a.v[3]; + return 0; +} + +static int is_identity (mat3 * m) +{ + if (!sweet_math_approx_equals (m->v[0], 1.0, EPSILON)) { return 0; } + if (!sweet_math_approx_equals (m->v[1], 0.0, EPSILON)) { return 0; } + if (!sweet_math_approx_equals (m->v[2], 0.0, EPSILON)) { return 0; } + + if (!sweet_math_approx_equals (m->v[3], 0.0, EPSILON)) { return 0; } + if (!sweet_math_approx_equals (m->v[4], 1.0, EPSILON)) { return 0; } + if (!sweet_math_approx_equals (m->v[5], 0.0, EPSILON)) { return 0; } + + if (!sweet_math_approx_equals (m->v[6], 0.0, EPSILON)) { return 0; } + if (!sweet_math_approx_equals (m->v[7], 0.0, EPSILON)) { return 0; } + if (!sweet_math_approx_equals (m->v[8], 1.0, EPSILON)) { return 0; } + return 1; +} + +static void get_eigen_vector (vec3 * v, mat3 * a, float h) +{ + char free_value; + a->v[0] = a->v[0] - h; + a->v[4] = a->v[4] - h; + a->v[8] = a->v[8] - h; + + v->x = 0; + v->y = 0; + v->z = 0; + free_value = 0; + + sweet_matrix_gaussian_elimination3 (a, v); + + if (sweet_math_approx_equals (a->v[0], 0, EPSILON) && + sweet_math_approx_equals (a->v[1], 0, EPSILON) && + sweet_math_approx_equals (a->v[2], 0, EPSILON)) + { + free_value = 1; + v->x = 1; + } + if (sweet_math_approx_equals (a->v[3], 0, EPSILON) && + sweet_math_approx_equals (a->v[4], 0, EPSILON) && + sweet_math_approx_equals (a->v[5], 0, EPSILON)) + { + free_value = 1; + v->y = 1; + } + if (sweet_math_approx_equals (a->v[6], 0, EPSILON) && + sweet_math_approx_equals (a->v[7], 0, EPSILON) && + sweet_math_approx_equals (a->v[8], 0, EPSILON)) + { + free_value = 1; + v->z = 1; + } + + if (!free_value && + sweet_math_approx_equals (a->v[2], 0, EPSILON) && + sweet_math_approx_equals (a->v[5], 0, EPSILON) && + sweet_math_approx_equals (a->v[8], 0, EPSILON)) + { + v->z = 1; + v->y = - a->v[7]; + v->x = - a->v[3] * v->y - a->v[6]; + } + *v = sweet_vector_normalize3 (*v); +} + +int +sweet_matrix_eigen_vector3 (vec3 * vectors, mat3 * m) +{ + /* (A - hI)V = 0 */ + mat3 a; + int n; + float h1, h2, h3; + + if (is_identity (m)) + { + vectors[0] = sweet_vector_new3 (1,0,0); + vectors[1] = sweet_vector_new3 (0,1,0); + vectors[2] = sweet_vector_new3 (0,0,1); + return 3; + } + + n = sweet_matrix_eigen_value3 (&h1, &h2, &h3, m); + + /* vector associated with h1 */ + a = *m; + get_eigen_vector (&vectors[0], &a, h1); + + /* vector associated with h2 */ + a = *m; + get_eigen_vector (&vectors[1], &a, h2); + + /* vector associated with h3 */ + a = *m; + get_eigen_vector (&vectors[2], &a, h3); + return n; +} + diff --git a/sweet_matrix.h b/sweet_matrix.h new file mode 100644 index 0000000..e79a9b0 --- /dev/null +++ b/sweet_matrix.h @@ -0,0 +1,169 @@ +/* + * Sweet is a small library for basic math and small matrix operation. + * 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 . + */ + + +#ifndef SWEET_MATRIX_H +#define SWEET_MATRIX_H + +#include "sweet_types.h" + +/* Matrix */ + +/** Null Matrix */ +/** @return Null matrix */ +mat2 sweet_matrix_null2 (); +mat3 sweet_matrix_null3 (); +mat4 sweet_matrix_null4 (); + +/** Identity */ +/** @return Indentity matrix */ +mat2 sweet_matrix_identity2 (); +mat3 sweet_matrix_identity3 (); +mat4 sweet_matrix_identity4 (); + +/** Frustum */ +/** @param Right as flaot */ +/** @param Left as float */ +/** @param Top as float */ +/** @param Bottom as float */ +/** @param zNear as float */ +/** @param zFar as float */ +/** @return Perspective matrix 4x4 */ +mat4 sweet_matrix_frustum (float left, float right, + float bottom, float top, + float zNear, float zFar); + +/** Perspective */ +/** @param fovy of the camera in degree as flaot */ +/** @param aspect of the view as float */ +/** @param zNear as float */ +/** @param zFar as float */ +/** @return Perspective matrix 4x4 */ +mat4 sweet_matrix_perspective (double fovy, double aspect, double z_near, double z_far); + +/** Ortho */ +/** @param Right as flaot */ +/** @param Left as float */ +/** @param Top as float */ +/** @param Bottom as float */ +/** @param zNear as float */ +/** @param zFar as float */ +/** @return Orthogonal matrix 4x4 */ +mat4 sweet_matrix_ortho (float left, float right, + float bottom, float top, + float zNear, float zFar); + +/** @return camera matrix */ +mat4 sweet_matrix_look_at (vec3 pos, vec3 dir, vec3 up); + +/** Rotation from quaterion */ +/** @param quaternion representing rotation */ +/** @return 3d Rotation matrix */ +mat3 sweet_matrix_quat_rotation3 (quaternion q); +mat4 sweet_matrix_quat_rotation3h (quaternion q); + +/** Rotation */ +/** @param Angle in radian as flaot */ +/** @param X, Y, Z as rotation component */ +/** @return 3d Rotation matrix */ +mat2 sweet_matrix_rotation2 (float angle); +mat3 sweet_matrix_rotation2h (float angle); +mat3 sweet_matrix_rotation3 (float angle, float x, float y, float z); +mat4 sweet_matrix_rotation3h (float angle, float x, float y, float z); + +/** Translation */ +/** @param X,Y,Z or vector */ +/** @return Translation matrix */ +mat4 sweet_matrix_translation3h (vec3 v); +mat3 sweet_matrix_translation2h (vec2 v); + +/** Scale */ +/** @param X,Y,Z scale component */ +/** @return Scale matrix */ +mat4 sweet_matrix_scale4 (float x, float y, float z, float w); +mat3 sweet_matrix_scale3 (float x, float y, float z); +mat2 sweet_matrix_scale2 (float x, float y); + +/** Texture bias */ +/** @return texture bias matrix */ +mat4 sweet_matrix_texture_bias (); + +/** Transpose */ +/** @param Matrix to transpose as mat2 *, mat3 * or mat4 * */ +void sweet_matrix_transpose2 (mat2 * transpose, mat2 * matrix); +void sweet_matrix_transpose3 (mat3 * transpose, mat3 * matrix); +void sweet_matrix_transpose4 (mat4 * transpose, mat4 * matrix); + +/** Det */ +/** @param Matrix as mat2, mat3 or mat4 */ +/** @return Determinant as float */ +float sweet_matrix_det2 (mat2 * matrix); +float sweet_matrix_det3 (mat3 * matrix); +float sweet_matrix_det4 (mat4 * matrix); + +/** Inverse */ +/** @param matrix as mat2 *, mat3 * or mat4 * */ +/** @param matrix to inverse as mat2 *, mat3 * or mat4 * */ +void sweet_matrix_inverse2 (mat2 * inverse, mat2 * matrix); +void sweet_matrix_inverse3 (mat3 * inverse, mat3 * matrix); +void sweet_matrix_inverse4 (mat4 * inverse, mat4 * matrix); + +/** Product */ +/** @param Product of the Matrix1 times Matrix2 as mat2 *, mat3 * or mat4 * */ +/** @param Matrix1, Matrix2 as mat2 *, mat3 * or mat4 * */ +void sweet_matrix_product2 (mat2 * product, mat2 * matrix1, mat2 * matrix2); +void sweet_matrix_product3 (mat3 * product, mat3 * matrix1, mat3 * matrix2); +void sweet_matrix_product4 (mat4 * product, mat4 * matrix1, mat4 * matrix2); + +/** Matrix Cast */ +/** Extract upper left sub matrix */ +/** @param sub as mat2 * or mat3 * */ +/** @param Matrix */ +void sweet_matrix_sub3 (mat2 * sub, mat3 * matrix); +void sweet_matrix_sub4 (mat3 * sub, mat4 * matrix); + +/** Column*/ +/** @param Matrix */ +/** @param Column id as column number : begin at 0 */ +/** @return matrix column as vec2, vec3 or vec4 */ +vec2 sweet_matrix_column2 (mat2 * matrix, int column_id); +vec3 sweet_matrix_column3 (mat3 * matrix, int column_id); +vec4 sweet_matrix_column4 (mat4 * matrix, int column_id); + +/** Mult*/ +/** @param Matrix */ +/** @param Vector */ +/** @return Matrix * Vector */ +vec2 sweet_matrix_mult2 (mat2 * matrix, vec2 vector); +vec3 sweet_matrix_mult3 (mat3 * matrix, vec3 vector); +vec4 sweet_matrix_mult4 (mat4 * matrix, vec4 vector); + +/** Eigen Value */ +int sweet_matrix_eigen_value3 (float * a, float * b, float * c, mat3 * m); + +/** Gaussian elimination */ +int sweet_matrix_gaussian_elimination3 (mat3 * m, vec3 * v); + +/** Solve linear system */ +int sweet_matrix_solve3 (mat3 * a, vec3 * v); + +/** Eigen Vector */ +int sweet_matrix_eigen_vector3 (vec3 * vectors, mat3 * m); + +#endif + diff --git a/sweet_types.h b/sweet_types.h new file mode 100644 index 0000000..1854150 --- /dev/null +++ b/sweet_types.h @@ -0,0 +1,62 @@ +/* + * Sweet is a small library for basic math and small matrix operation. + * 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 . + */ + + +#ifndef SWEET_TYPES_H +#define SWEET_TYPES_H + +typedef struct mat4 { + float v[16]; +} mat4; + +typedef struct mat3 { + float v[9]; +} mat3; + +typedef struct mat2 { + float v[4]; +} mat2; + +typedef struct vec4 { + float x; + float y; + float z; + float w; +} vec4; + +typedef struct vec3 { + float x; + float y; + float z; +} vec3; + +typedef struct vec2 { + float x; + float y; +} vec2; + + +typedef struct complex { + float r; + float i; +} complex; + +typedef vec4 quaternion; + +#endif +