new functions, bug correction, new type quat for quaternion

sweet_macro.h

@@ -22,6 +22,8 @@
 #define _OVERLOAD_2_(_1, _2, NAME, ...) NAME
 #define _OVERLOAD_3_(_1, _2, _3, NAME, ...) NAME
 #define _OVERLOAD_4_(_1, _2, _3, _4, NAME, ...) NAME
+#define _OVERLOAD_5_(_1, _2, _3, _4, _5, NAME, ...) NAME
+#define _OVERLOAD_6_(_1, _2, _3, _4, _5, _6, NAME, ...) NAME
 
 #define vec2_new(...) _OVERLOAD_2_(__VA_ARGS__, vec2_new_2f, vec2_new_v3, NULL)(__VA_ARGS__)
 #define vec3_new(...) _OVERLOAD_3_(__VA_ARGS__, vec3_new_3f, vec3_new_v2_1f, vec3_new_v4, NULL)(__VA_ARGS__)
@@ -31,6 +33,10 @@
 #define mat3_new(...) _OVERLOAD_3_(__VA_ARGS__, mat3_new_3v, mat3_new_2v, mat3_new_m4, NULL)(__VA_ARGS__)
 #define mat4_new(...) _OVERLOAD_4_(__VA_ARGS__, mat4_new_4v, mat4_new_3v, mat4_new_2v, mat4_new_m4, NULL)(__VA_ARGS__)
 
+#define mat2_insert_col(...) _OVERLOAD_4_(__VA_ARGS__, mat2_insert_col_2f, mat2_insert_col_1v, NULL, NULL)(__VA_ARGS__)
+#define mat3_insert_col(...) _OVERLOAD_5_(__VA_ARGS__, mat3_insert_col_3f, mat3_insert_col_1v1f, mat3_insert_col_1v, NULL, NULL)(__VA_ARGS__)
+#define mat4_insert_col(...) _OVERLOAD_6_(__VA_ARGS__, mat4_insert_col_4f, mat4_insert_col_1v2f, mat4_insert_col_1v1f, mat4_insert_col_1v, NULL, NULL)(__VA_ARGS__)
+
 #define swap(a, b, type) \
 {\
     type t;\

sweet_math.c

@@ -222,6 +222,33 @@ vec4_square_dist (vec4 a, vec4 b)
     return (dx * dx + dy * dy + dz * dz);
 }
 
+vec2 vec2_interpolate (vec2 u, vec2 v, float t)
+{
+    float _t = 1.0 - t;
+    u.x = u.x * _t + v.x * t;
+    u.y = u.y * _t + v.y * t;
+    return u;
+}
+
+vec3 vec3_interpolate (vec3 u, vec3 v, float t)
+{
+    float _t = 1.0 - t;
+    u.x = u.x * _t + v.x * t;
+    u.y = u.y * _t + v.y * t;
+    u.z = u.z * _t + v.z * t;
+    return u;
+}
+
+vec4 vec4_interpolate (vec4 u, vec4 v, float t)
+{
+    float _t = 1.0 - t;
+    u.x = u.x * _t + v.x * t;
+    u.y = u.y * _t + v.y * t;
+    u.z = u.z * _t + v.z * t;
+    u.w = u.w * _t + v.w * t;
+    return u;
+}
+
 float
 vec2_dot (vec2 a, vec2 b)
 {
@@ -577,10 +604,10 @@ vec4_product (vec4 v1, vec4 v2)
     return v1;
 }
 
-quaternion
+quat
 quat_new (float w, float x, float y, float z)
 {
-    quaternion q;
+    quat q;
     q.w = w;
     q.x = x;
     q.y = y;
@@ -589,14 +616,13 @@ quat_new (float w, float x, float y, float z)
     return q;
 }
 
-quaternion
+quat
 quat_rotation (float angle, float x, float y, float z)
 {
-    quaternion q;
+    quat q;
     float s;
 
     s = sin (0.5 * angle);
-
     q.w = cos (0.5 * angle);
     q.x = x * s;
     q.y = y * s;
@@ -605,10 +631,10 @@ quat_rotation (float angle, float x, float y, float z)
     return q;
 }
 
-quaternion
+quat
-quat_conjugate (quaternion q)
+quat_conjugate (quat q)
 {
-    quaternion conjugate;
+    quat conjugate;
 
     conjugate.x = -q.x;
     conjugate.y = -q.y;
@@ -618,10 +644,10 @@ quat_conjugate (quaternion q)
     return conjugate;
 }
 
-quaternion
+quat
-quat_add (quaternion q1, quaternion q2)
+quat_add (quat q1, quat q2)
 {
-    quaternion sum;
+    quat sum;
 
     sum.w = q1.w + q2.w;
     sum.x = q1.x + q2.x;
@@ -631,10 +657,10 @@ quat_add (quaternion q1, quaternion q2)
     return sum;
 }
 
-quaternion
+quat
-quat_product (quaternion q1, quaternion q2)
+quat_product (quat q1, quat q2)
 {
-    quaternion prod;
+    quat 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);
@@ -645,12 +671,43 @@ quat_product (quaternion q1, quaternion q2)
 }
 
 float
-quat_norm (quaternion q)
+quat_norm (quat q)
 {
     return sqrt ((q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z));
 }
 
+quat
+quat_inverse (quat q)
+{
+    quat q_;
+    float m = 1.0 / vec4_dot(q, q);
 
+    q_.w =  q.w * m;
+    q_.x = -q.x * m;
+    q_.y = -q.y * m;
+    q_.z = -q.z * m;
+
+    return q_;
+}
+
+quat
+quat_normalize (quat q)
+{
+    return vec4_normalize (q);
+}
+
+
+quat
+quat_scale (quat q, float s)
+{
+    return vec4_scale (q, s);
+}
+
+quat
+quat_interpolate (quat q1, quat q2, float t)
+{
+    return quat_add (quat_scale (q1, 1.0 - t), quat_scale (q2, t));
+}
 
 int
 nearest_integer (int number, int size, ...)

sweet_math.h

@@ -53,7 +53,7 @@
                 (a <= (b+epsilon) && a >= (b-epsilon))
 
 #define float_approx_zero(a, epsilon) \
-                (a <= (epsilon) && a >= (epsilon))
+                (a <= (epsilon) && a >= (-epsilon))
 
 #define max(a, b) (a >= b ? a : b)
 #define min(a, b) (a <= b ? a : b)
@@ -100,6 +100,10 @@ float vec2_square_dist (vec2 a, vec2 b);
 float vec3_square_dist (vec3 a, vec3 b);
 float vec4_square_dist (vec4 a, vec4 b);
 
+/** Interpolate */
+vec2 vec2_interpolate (vec2 u, vec2 v, float t);
+vec3 vec3_interpolate (vec3 u, vec3 v, float t);
+vec4 vec4_interpolate (vec4 u, vec4 v, float t);
 
 /** Dot */
 /** @param v1 as vector */
@@ -195,15 +199,16 @@ vec2 vec2_product (vec2 vector1, vec2 vector2);
 vec3 vec3_product (vec3 vector1, vec3 vector2);
 vec4 vec4_product (vec4 vector1, vec4 vector2);
 
-
 /* Quaternion */
-quaternion quat_new (float w, float x, float y, float z);
+quat quat_new (float w, float x, float y, float z);
-quaternion quat_rotation (float angle, float x, float y, float z);
+quat quat_rotation (float angle, float x, float y, float z);
-quaternion quat_conjugate (quaternion q);
+quat quat_conjugate (quat q);
-quaternion quat_add (quaternion q1, quaternion q2);
+quat quat_add (quat q1, quat q2);
-quaternion quat_product (quaternion q1, quaternion q2);
+quat quat_product (quat q1, quat q2);
-float quat_norm (quaternion q);
+float quat_norm (quat q);
-
+quat quat_inverse (quat q);
+quat quat_normalize (quat q);
+quat quat_interpolate (quat q1, quat q2, float t);
 
 /** Nearest */
 /** @param number integer to compare as int */

sweet_matrix.c

@@ -17,6 +17,7 @@
  */
 
 #include <string.h>
+#include <stdio.h>
 #include <math.h>
 #include "sweet_macro.h"
 #include "sweet_matrix.h"
@@ -193,6 +194,42 @@ mat4 mat4_new_4v (vec4 e1, vec4 e2, vec4 e3, vec4 e4)
     return m;
 }
 
+void mat2_to_str(mat2 * m, char * str, int n)
+{
+    float *v = m->v;
+    snprintf(str, n,
+             "%f %f\n"
+             "%f %f\n",
+             v[0], v[2],
+             v[1], v[3]);
+}
+
+void mat3_to_str(mat3 * m, char * str, int n)
+{
+    float *v = m->v;
+    snprintf(str, n,
+             "%f %f %f\n"
+             "%f %f %f\n"
+             "%f %f %f\n",
+             v[0], v[3], v[6],
+             v[1], v[4], v[7],
+             v[2], v[5], v[8]);
+}
+
+void mat4_to_str(mat4 * m, char * str, int n)
+{
+    float *v = m->v;
+    snprintf(str, n,
+             "%f %f %f %f\n"
+             "%f %f %f %f\n"
+             "%f %f %f %f\n"
+             "%f %f %f %f\n",
+             v[0], v[4], v[8 ], v[12],
+             v[1], v[5], v[9 ], v[13],
+             v[2], v[6], v[10], v[14],
+             v[3], v[7], v[11], v[15]);
+}
+
 
 mat2
 mat2_null (void)
@@ -552,8 +589,19 @@ matrix_look_at_ortho (vec3 pos, vec3 dir, vec3 up, vec3 right)
     return mat3h_ortho_basis (pos, right, up, dir);
 }
 
+quat
+mat3_to_quaternion (mat3 * m)
+{
+    quat q;
+    q.w = 0.5 * sqrt(1 + m->v[0] * m->v[0] + m->v[4] * m->v[4] + m->v[8] * m->v[8]);
+    q.x = (m->v[7] - m->v[5]) / (4 * q.w);
+    q.y = (m->v[2] - m->v[6]) / (4 * q.w);
+    q.z = (m->v[3] - m->v[1]) / (4 * q.w);
+    return q;
+}
+
 mat3
-mat3_quat_rotation (quaternion q)
+mat3_quat_rotation (quat q)
 {
     mat3 m;
 
@@ -567,13 +615,13 @@ mat3_quat_rotation (quaternion q)
 
     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;
+    m.v[8] = - 2 * q.x * q.x - 2 * q.y * q.y + 1;
 
     return m;
 }
 
 mat4
-mat3h_quat_rotation (quaternion q)
+mat3h_quat_rotation (quat q)
 {
     mat4 m;
 
@@ -589,7 +637,7 @@ mat3h_quat_rotation (quaternion q)
 
     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[10] = - 2 * q.x * q.x - 2 * q.y * q.y + 1;
     m.v[11] =   0;
 
     m.v[12] =   0;
@@ -1067,7 +1115,6 @@ mat4_inverse (mat4 * inv, mat4 * m)
     }
 
     /* In this case we can use a better approach */
-
     invdet = 1.0 / detp;
 
     p.v[0] = m->v[5] * invdet;
@@ -1301,6 +1348,72 @@ mat4_column (mat4 * m, int idx)
     return v;
 }
 
+void mat2_insert_col_1v (mat2 * m, int col, vec2 v)
+{
+    m->v[col * 2] = v.x;
+    m->v[col * 2 + 1] = v.y;
+}
+
+void mat2_insert_col_2f (mat2 * m, int col, float x, float y)
+{
+    m->v[col * 2] = x;
+    m->v[col * 2 + 1] = y;
+}
+
+void mat3_insert_col_1v (mat3 * m, int col, vec3 v)
+{
+    m->v[col*3] = v.x;
+    m->v[col*3 + 1] = v.y;
+    m->v[col*3 + 2] = v.z;
+}
+
+void mat3_insert_col_1v1f (mat3 * m, int col, vec2 v, float z)
+{
+    m->v[col*3] = v.x;
+    m->v[col*3 + 1] = v.y;
+    m->v[col*3 + 2] = z;
+}
+
+void mat3_insert_col_3f (mat3 * m, int col, float x, float y, float z)
+{
+    m->v[col*3] = x;
+    m->v[col*3 + 1] = y;
+    m->v[col*3 + 2] = z;
+}
+
+void mat4_insert_col_1v (mat4 * m, int col, vec4 v)
+{
+    m->v[col * 4] = v.x;
+    m->v[col * 4 + 1] = v.y;
+    m->v[col * 4 + 2] = v.z;
+    m->v[col * 4 + 3] = v.w;
+}
+
+void mat4_insert_col_1v1f (mat4 * m, int col, vec3 v, float w)
+{
+    m->v[col * 4] = v.x;
+    m->v[col * 4 + 1] = v.y;
+    m->v[col * 4 + 2] = v.z;
+    m->v[col * 4 + 3] = w;
+}
+
+void mat4_insert_col_1v2f (mat4 * m, int col, vec2 v, float z, float w)
+{
+    m->v[col * 4] = v.x;
+    m->v[col * 4 + 1] = v.y;
+    m->v[col * 4 + 2] = z;
+    m->v[col * 4 + 3] = w;
+}
+
+void mat4_insert_col_4f (mat4 * m, int col, float x, float y, float z, float w)
+{
+    m->v[col * 4] = x;
+    m->v[col * 4 + 1] = y;
+    m->v[col * 4 + 2] = z;
+    m->v[col * 4 + 3] = w;
+}
+
+
 vec2
 mat2_mult (mat2 * mat, vec2 vec)
 {

sweet_matrix.h

@@ -37,6 +37,11 @@ mat4 mat4_new_2v (vec2 e1, vec2 e2);
 mat4 mat4_new_3v (vec3 e1, vec3 e2, vec3 e3);
 mat4 mat4_new_4v (vec4 e1, vec4 e2, vec4 e3, vec4 e4);
 
+
+void mat2_to_str(mat2 * m, char * str, int n);
+void mat3_to_str(mat3 * m, char * str, int n);
+void mat4_to_str(mat4 * m, char * str, int n);
+
 /** Null Matrix */
 /** @return Null matrix */
 mat2 mat2_null (void);
@@ -94,11 +99,14 @@ mat4 matrix_ortho (float left, float right,
 mat4 matrix_look_at (vec3 pos, vec3 dir, vec3 up);
 mat4 matrix_look_at_ortho (vec3 pos, vec3 dir, vec3 up, vec3 right);
 
+
+quat mat3_to_quaternion (mat3 * m);
+
 /** Rotation from quaterion */
 /** @param quaternion representing rotation */
 /** @return 3d Rotation matrix */
-mat3 mat3_quat_rotation (quaternion q);
+mat3 mat3_quat_rotation (quat q);
-mat4 mat3h_quat_rotation (quaternion q);
+mat4 mat3h_quat_rotation (quat q);
 
 /** Rotation */
 /** @param Angle in radian as flaot */
@@ -162,7 +170,7 @@ void mat4_product (mat4 * product, mat4 * matrix1, mat4 * matrix2);
 void mat3_submatrix (mat2 * sub, mat3 * matrix);
 void mat4_submatrix (mat3 * sub, mat4 * matrix);
 
-/** Column*/
+/** Column */
 /** @param Matrix */
 /** @param Column id as column number : begin at 0 */
 /** @return matrix column as vec2, vec3 or vec4 */
@@ -170,6 +178,23 @@ vec2 mat2_column (mat2 * matrix, int column_id);
 vec3 mat3_column (mat3 * matrix, int column_id);
 vec4 mat4_column (mat4 * matrix, int column_id);
 
+/** Insert */
+/** @param Matrix */
+/** @param Column id as column number : begin at 0 */
+/** @param column component */
+void mat2_insert_col_1v (mat2 * matrix, int column_id, vec2 v);
+void mat2_insert_col_2f (mat2 * matrix, int column_id, float x, float y);
+
+void mat3_insert_col_1v (mat3 * matrix, int column_id, vec3 v);
+void mat3_insert_col_1v1f (mat3 * matrix, int column_id, vec2 v, float z);
+void mat3_insert_col_3f (mat3 * matrix, int column_id, float x, float y, float z);
+
+void mat4_insert_col_1v (mat4 * matrix, int column_id, vec4 v);
+void mat4_insert_col_1v1f (mat4 * matrix, int column_id, vec3 v, float w);
+void mat4_insert_col_1v2f (mat4 * matrix, int column_id, vec2 v, float z, float w);
+void mat4_insert_col_4f (mat4 * matrix, int column_id, float x, float y, float z, float w);
+
+
 /** Mult*/
 /** @param Matrix */
 /** @param Vector */

sweet_types.h

@@ -56,7 +56,9 @@ typedef struct complex {
     float i;
 } complex;
 
-typedef vec4 quaternion;
+typedef vec4 quat;
+
+typedef quat quaternion;
 
 #endif