cedric pushed a commit to branch master.
http://git.enlightenment.org/core/efl.git/commit/?id=8228145ca923ed4fbf5333f409e25a1f88bd9b38
commit 8228145ca923ed4fbf5333f409e25a1f88bd9b38
Author: Cedric BAIL <ced...@osg.samsung.com>
Date: Wed Jun 24 19:33:44 2015 +0200
eina: add eina_matrix4_quaternion_get and eina_quaternion_matrix4_get.
Implementation taken from pseudo code at :
http://www.w3.org/TR/css3-transforms/#decomposing-a-3d-matrix
---
src/lib/eina/eina_quaternion.c | 310 +++++++++++++++++++++++++++++++++++++++++
src/lib/eina/eina_quaternion.h | 12 ++
2 files changed, 322 insertions(+)
diff --git a/src/lib/eina/eina_quaternion.c b/src/lib/eina/eina_quaternion.c
index 0334d57..55105be 100644
--- a/src/lib/eina/eina_quaternion.c
+++ b/src/lib/eina/eina_quaternion.c
@@ -23,6 +23,7 @@
#include "eina_private.h"
#include <math.h>
+#include <float.h>
#include "eina_fp.h"
#include "eina_matrix.h"
@@ -623,6 +624,213 @@ eina_quaternion_rotation_matrix3_get(Eina_Matrix3 *m,
m->zz = 1.0 - xx - yy;
}
+static inline double
+_max(double a, double b)
+{
+ return a > b ? a : b;
+}
+
+static inline double
+eina_point_3d_norm(Eina_Point_3D *p)
+{
+ return sqrt(p->x * p->x + p->y * p->y + p->z * p->z);
+}
+
+static inline void
+eina_point_3d_normalize(Eina_Point_3D *p, double norm)
+{
+ double tmp = 1 / norm;
+
+ p->x *= tmp;
+ p->y *= tmp;
+ p->z *= tmp;
+}
+
+static inline double
+eina_point_3d_dot(const Eina_Point_3D *a, const Eina_Point_3D *b)
+{
+ return a->x * b->x + a->y * b->y + a->z * b->z;
+}
+
+static inline void
+eina_point_3d_combine(Eina_Point_3D *out,
+ const Eina_Point_3D *a, const Eina_Point_3D *b,
+ double scale1, double scale2)
+{
+ out->x = a->x * scale1 + b->x * scale2;
+ out->y = a->y * scale1 + b->y * scale2;
+ out->z = a->z * scale1 + b->z * scale2;
+}
+
+static inline void
+eina_point3d_cross(Eina_Point_3D *out,
+ const Eina_Point_3D *a, const Eina_Point_3D *b)
+{
+ out->x = a->y * b->z - a->z * b->y;
+ out->y = a->z * b->x - a->x * b->z;
+ out->z = a->x * b->y - a->y * b->x;
+}
+
+static inline void
+eina_point3d_neg(Eina_Point_3D *out, const Eina_Point_3D *in)
+{
+ out->x = - in->x;
+ out->y = - in->y;
+ out->z = - in->z;
+}
+
+/* http://www.w3.org/TR/css3-transforms/#decomposing-a-3d-matrix */
+EAPI Eina_Bool
+eina_matrix4_quaternion_to(Eina_Quaternion *rotation,
+ Eina_Quaternion *perspective,
+ Eina_Point_3D *translation,
+ Eina_Point_3D *scale,
+ Eina_Point_3D *skew,
+ const Eina_Matrix4 *m)
+{
+ Eina_Matrix4 n, pm;
+ double det, factor;
+
+ if (m->ww == 0) return EINA_FALSE;
+
+ // Normalize the matrix.
+ factor = 1 / m->ww;
+
+ n.xx = m->xx * factor;
+ n.xy = m->xy * factor;
+ n.xz = m->xz * factor;
+ n.xw = m->xw * factor;
+ n.yx = m->yx * factor;
+ n.yy = m->yy * factor;
+ n.yz = m->yz * factor;
+ n.yw = m->yw * factor;
+ n.zx = m->zx * factor;
+ n.zy = m->zy * factor;
+ n.zz = m->zz * factor;
+ n.zw = m->zw * factor;
+ n.wx = m->wx * factor;
+ n.wy = m->wy * factor;
+ n.wz = m->wz * factor;
+ n.ww = m->ww * factor;
+
+ pm = n;
+ pm.xw = 0;
+ pm.yw = 0;
+ pm.zw = 0;
+ pm.ww = 1;
+
+ // If the perspective matrix is not invertible, we are also unable to
+ // decompose, so we'll bail early.
+ det = eina_matrix4_determinant(&pm);
+ if (fabs(det) < DBL_EPSILON) return EINA_FALSE;
+
+ // First, isolate perspective.
+ if (perspective)
+ {
+ if (fabs(n.xw) < DBL_EPSILON ||
+ fabs(n.yw) < DBL_EPSILON ||
+ fabs(n.zw) < DBL_EPSILON)
+ {
+ Eina_Quaternion tmp;
+ Eina_Matrix4 ipm, tipm;
+
+ tmp.x = n.wx;
+ tmp.y = n.wy;
+ tmp.z = n.wz;
+ tmp.w = n.ww;
+
+ if (!eina_matrix4_inverse(&ipm, &pm))
+ return EINA_FALSE;
+
+ eina_matrix4_transpose(&tipm, &ipm);
+
+ perspective->x = tipm.xx * tmp.x + tipm.yx * tmp.y + tipm.zx *
tmp.z + tipm.wx * tmp.w;
+ perspective->y = tipm.xy * tmp.x + tipm.yy * tmp.y + tipm.zy *
tmp.z + tipm.wy * tmp.w;
+ perspective->z = tipm.xz * tmp.x + tipm.yz * tmp.y + tipm.zz *
tmp.z + tipm.wz * tmp.w;
+ perspective->w = tipm.xw * tmp.x + tipm.yw * tmp.y + tipm.zw *
tmp.z + tipm.ww * tmp.w;
+ }
+ else
+ {
+ perspective->x = perspective->y = perspective->z = 0;
+ perspective->w = 1;
+ }
+ }
+
+ if (translation)
+ {
+ translation->x = n.xw;
+ translation->y = n.yw;
+ translation->z = n.zw;
+ }
+
+ if (skew || scale || rotation)
+ {
+ Eina_Point_3D tsc, tsk, row0, row1, row2, cross;
+ double tmp;
+
+ // Make sure all pointer are defined
+ if (!scale) scale = &tsc;
+ if (!skew) skew = &tsk;
+
+ row0.x = n.xx; row1.x = n.yx; row2.x = n.zx;
+ row0.y = n.xy; row1.y = n.yy; row2.y = n.zy;
+ row0.z = n.xz; row1.z = n.yz; row2.z = n.zz;
+
+ // Compute X scale factor and normalize first row.
+ scale->x = eina_point_3d_norm(&row0);
+ eina_point_3d_normalize(&row0, scale->x);
+
+ skew->x = eina_point_3d_dot(&row0, &row1);
+ eina_point_3d_combine(&row1, &row1, &row0, 1.0, -skew->x);
+
+ // Now, compute Y scale and normalize 2nd row.
+ scale->y = eina_point_3d_norm(&row1);
+ eina_point_3d_normalize(&row1, scale->y);
+ skew->x /= scale->y;
+
+ // Compute XZ and YZ shears, orthogonalize 3rd row
+ skew->y = eina_point_3d_dot(&row0, &row2);
+ eina_point_3d_combine(&row2, &row2, &row0, 1.0, -skew->y);
+ skew->z = eina_point_3d_dot(&row1, &row2);
+ eina_point_3d_combine(&row2, &row2, &row1, 1.0, -skew->z);
+
+ // Next, get Z scale and normalize 3rd row.
+ scale->z = eina_point_3d_norm(&row2);
+ eina_point_3d_normalize(&row2, scale->z);
+
+ tmp = 1 / scale->z;
+ skew->y *= tmp;
+ skew->z *= tmp;
+
+ // At this point, the matrix (in rows) is orthonormal.
+ // Check for a coordinate system flip. If the determinant
+ // is -1, then negate the matrix and the scaling factors.
+ eina_point3d_cross(&cross, &row1, &row2);
+ if (eina_point_3d_dot(&row0, &cross) < 0)
+ {
+ eina_point_3d_dot(scale, scale);
+ eina_point_3d_dot(&row0, &row0);
+ eina_point_3d_dot(&row1, &row1);
+ eina_point_3d_dot(&row2, &row2);
+ }
+
+ if (rotation)
+ {
+ // Now, get the rotations out
+ rotation->x = 0.5 * sqrt(_max(1 + row0.x - row1.y - row2.z, 0));
+ rotation->y = 0.5 * sqrt(_max(1 - row0.x + row1.y - row2.z, 0));
+ rotation->z = 0.5 * sqrt(_max(1 - row0.x - row1.y + row2.z, 0));
+ rotation->w = 0.5 * sqrt(_max(1 + row0.x + row1.y + row2.z, 0));
+
+ if (row2.y > row1.z) rotation->x = - rotation->x;
+ if (row0.z > row2.x) rotation->y = - rotation->y;
+ if (row1.x > row0.y) rotation->z = - rotation->z;
+ }
+ }
+
+ return EINA_TRUE;
+}
+
EAPI void
eina_matrix3_quaternion_get(Eina_Quaternion *q,
const Eina_Matrix3 *m)
@@ -674,3 +882,105 @@ eina_matrix3_quaternion_get(Eina_Quaternion *q,
q->y = y;
q->z = z;
}
+
+EAPI void
+eina_quaternion_matrix4_to(Eina_Matrix4 *m,
+ const Eina_Quaternion *rotation,
+ const Eina_Quaternion *perspective,
+ const Eina_Point_3D *translation,
+ const Eina_Point_3D *scale,
+ const Eina_Point_3D *skew)
+{
+ Eina_Matrix4 rm, tmp;
+ double x, y, z, w;
+
+ eina_matrix4_identity(m);
+
+ // apply perspective
+ m->wx = perspective->x;
+ m->wy = perspective->y;
+ m->wz = perspective->z;
+ m->ww = perspective->w;
+
+ // apply translation
+ m->xw = translation->x * m->xx + translation->y * m->xy + translation->z *
m->xz;
+ m->yw = translation->x * m->yx + translation->y * m->yy + translation->z *
m->yz;
+ m->zw = translation->x * m->zx + translation->y * m->zy + translation->z *
m->zz;
+
+ // apply rotation
+ x = rotation->x;
+ y = rotation->y;
+ z = rotation->z;
+ w = rotation->w;
+
+ // Construct a composite rotation matrix from the quaternion values
+ // rotationMatrix is a identity 4x4 matrix initially
+ eina_matrix4_identity(&rm);
+
+ rm.xx = 1 - 2 * (y * y + z * z);
+ rm.xy = 2 * (x * y - z * w);
+ rm.xz = 2 * (x * z + y * w);
+ rm.yx = 2 * (x * y + z * w);
+ rm.yy = 1 - 2 * (x * x + z * z);
+ rm.yz = 2 * (y * z - x * w);
+ rm.zx = 2 * (x * z - y * w);
+ rm.zy = 2 * (y * z + x * w);
+ rm.zz = 1 - 2 * (x * x + y * y);
+
+ eina_matrix4_multiply(&tmp, m, &rm);
+
+ // apply skew
+ // rm is a identity 4x4 matrix initially
+ if (skew->z)
+ {
+ Eina_Matrix4 cp;
+
+ eina_matrix4_identity(&rm);
+ rm.zx = skew->z;
+
+ eina_matrix4_multiply(&cp, &tmp, &rm);
+ tmp = cp;
+ }
+
+ if (skew->y)
+ {
+ Eina_Matrix4 cp;
+
+ eina_matrix4_identity(&rm);
+ rm.zy = 0;
+ rm.zx = skew->y;
+
+ eina_matrix4_multiply(&cp, &tmp, &rm);
+ tmp = cp;
+ }
+
+ if (skew->x)
+ {
+ Eina_Matrix4 cp;
+
+ eina_matrix4_identity(&rm);
+ rm.zx = 0;
+ rm.yx = skew->x;
+
+ eina_matrix4_multiply(&cp, &tmp, &rm);
+ tmp = cp;
+ }
+
+ // apply scale
+ m->xx = tmp.xx * scale->x;
+ m->xy = tmp.xy * scale->x;
+ m->xz = tmp.xz * scale->x;
+ m->xw = tmp.xw;
+ m->yx = tmp.yx * scale->y;
+ m->yy = tmp.yy * scale->y;
+ m->yz = tmp.yz * scale->y;
+ m->yw = tmp.yw;
+ m->zx = tmp.zx * scale->z;
+ m->zy = tmp.zy * scale->z;
+ m->zz = tmp.zz * scale->z;
+ m->zw = tmp.zw;
+ m->wx = tmp.wx;
+ m->wy = tmp.wy;
+ m->wz = tmp.wz;
+ m->ww = tmp.ww;
+}
diff --git a/src/lib/eina/eina_quaternion.h b/src/lib/eina/eina_quaternion.h
index 540c68f..9561ccf 100644
--- a/src/lib/eina/eina_quaternion.h
+++ b/src/lib/eina/eina_quaternion.h
@@ -129,5 +129,17 @@ EAPI void
eina_quaternion_rotation_matrix3_get(Eina_Matrix3 *m,
const Eina_Quaternion *q); /**<
@since 1.15 */
EAPI void eina_matrix3_quaternion_get(Eina_Quaternion *q,
const Eina_Matrix3 *m); /**< @since 1.15
*/
+EAPI Eina_Bool eina_matrix4_quaternion_to(Eina_Quaternion *rotation,
+ Eina_Quaternion *perspective,
+ Eina_Point_3D *translation,
+ Eina_Point_3D *scale,
+ Eina_Point_3D *skew,
+ const Eina_Matrix4 *m);
+EAPI void eina_quaternion_matrix4_to(Eina_Matrix4 *m,
+ const Eina_Quaternion *rotation,
+ const Eina_Quaternion *perspective,
+ const Eina_Point_3D *translation,
+ const Eina_Point_3D *scale,
+ const Eina_Point_3D *skew);
#endif
--
