summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorCedric BAIL <cedric@osg.samsung.com>2015-06-24 19:33:44 +0200
committerCedric BAIL <cedric@osg.samsung.com>2015-08-21 16:40:31 +0200
commit8228145ca923ed4fbf5333f409e25a1f88bd9b38 (patch)
tree64c1f8c4b06ecac48b2028dc5613a08dcf25ee46
parentabfe65e05d6ef483432def7a4554f401e33657ca (diff)
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
-rw-r--r--src/lib/eina/eina_quaternion.c310
-rw-r--r--src/lib/eina/eina_quaternion.h12
2 files changed, 322 insertions, 0 deletions
diff --git a/src/lib/eina/eina_quaternion.c b/src/lib/eina/eina_quaternion.c
index 0334d57944..55105bee2f 100644
--- a/src/lib/eina/eina_quaternion.c
+++ b/src/lib/eina/eina_quaternion.c
@@ -23,6 +23,7 @@
23#include "eina_private.h" 23#include "eina_private.h"
24 24
25#include <math.h> 25#include <math.h>
26#include <float.h>
26 27
27#include "eina_fp.h" 28#include "eina_fp.h"
28#include "eina_matrix.h" 29#include "eina_matrix.h"
@@ -623,6 +624,213 @@ eina_quaternion_rotation_matrix3_get(Eina_Matrix3 *m,
623 m->zz = 1.0 - xx - yy; 624 m->zz = 1.0 - xx - yy;
624} 625}
625 626
627static inline double
628_max(double a, double b)
629{
630 return a > b ? a : b;
631}
632
633static inline double
634eina_point_3d_norm(Eina_Point_3D *p)
635{
636 return sqrt(p->x * p->x + p->y * p->y + p->z * p->z);
637}
638
639static inline void
640eina_point_3d_normalize(Eina_Point_3D *p, double norm)
641{
642 double tmp = 1 / norm;
643
644 p->x *= tmp;
645 p->y *= tmp;
646 p->z *= tmp;
647}
648
649static inline double
650eina_point_3d_dot(const Eina_Point_3D *a, const Eina_Point_3D *b)
651{
652 return a->x * b->x + a->y * b->y + a->z * b->z;
653}
654
655static inline void
656eina_point_3d_combine(Eina_Point_3D *out,
657 const Eina_Point_3D *a, const Eina_Point_3D *b,
658 double scale1, double scale2)
659{
660 out->x = a->x * scale1 + b->x * scale2;
661 out->y = a->y * scale1 + b->y * scale2;
662 out->z = a->z * scale1 + b->z * scale2;
663}
664
665static inline void
666eina_point3d_cross(Eina_Point_3D *out,
667 const Eina_Point_3D *a, const Eina_Point_3D *b)
668{
669 out->x = a->y * b->z - a->z * b->y;
670 out->y = a->z * b->x - a->x * b->z;
671 out->z = a->x * b->y - a->y * b->x;
672}
673
674static inline void
675eina_point3d_neg(Eina_Point_3D *out, const Eina_Point_3D *in)
676{
677 out->x = - in->x;
678 out->y = - in->y;
679 out->z = - in->z;
680}
681
682/* http://www.w3.org/TR/css3-transforms/#decomposing-a-3d-matrix */
683EAPI Eina_Bool
684eina_matrix4_quaternion_to(Eina_Quaternion *rotation,
685 Eina_Quaternion *perspective,
686 Eina_Point_3D *translation,
687 Eina_Point_3D *scale,
688 Eina_Point_3D *skew,
689 const Eina_Matrix4 *m)
690{
691 Eina_Matrix4 n, pm;
692 double det, factor;
693
694 if (m->ww == 0) return EINA_FALSE;
695
696 // Normalize the matrix.
697 factor = 1 / m->ww;
698
699 n.xx = m->xx * factor;
700 n.xy = m->xy * factor;
701 n.xz = m->xz * factor;
702 n.xw = m->xw * factor;
703 n.yx = m->yx * factor;
704 n.yy = m->yy * factor;
705 n.yz = m->yz * factor;
706 n.yw = m->yw * factor;
707 n.zx = m->zx * factor;
708 n.zy = m->zy * factor;
709 n.zz = m->zz * factor;
710 n.zw = m->zw * factor;
711 n.wx = m->wx * factor;
712 n.wy = m->wy * factor;
713 n.wz = m->wz * factor;
714 n.ww = m->ww * factor;
715
716 pm = n;
717 pm.xw = 0;
718 pm.yw = 0;
719 pm.zw = 0;
720 pm.ww = 1;
721
722 // If the perspective matrix is not invertible, we are also unable to
723 // decompose, so we'll bail early.
724 det = eina_matrix4_determinant(&pm);
725 if (fabs(det) < DBL_EPSILON) return EINA_FALSE;
726
727 // First, isolate perspective.
728 if (perspective)
729 {
730 if (fabs(n.xw) < DBL_EPSILON ||
731 fabs(n.yw) < DBL_EPSILON ||
732 fabs(n.zw) < DBL_EPSILON)
733 {
734 Eina_Quaternion tmp;
735 Eina_Matrix4 ipm, tipm;
736
737 tmp.x = n.wx;
738 tmp.y = n.wy;
739 tmp.z = n.wz;
740 tmp.w = n.ww;
741
742 if (!eina_matrix4_inverse(&ipm, &pm))
743 return EINA_FALSE;
744
745 eina_matrix4_transpose(&tipm, &ipm);
746
747 perspective->x = tipm.xx * tmp.x + tipm.yx * tmp.y + tipm.zx * tmp.z + tipm.wx * tmp.w;
748 perspective->y = tipm.xy * tmp.x + tipm.yy * tmp.y + tipm.zy * tmp.z + tipm.wy * tmp.w;
749 perspective->z = tipm.xz * tmp.x + tipm.yz * tmp.y + tipm.zz * tmp.z + tipm.wz * tmp.w;
750 perspective->w = tipm.xw * tmp.x + tipm.yw * tmp.y + tipm.zw * tmp.z + tipm.ww * tmp.w;
751 }
752 else
753 {
754 perspective->x = perspective->y = perspective->z = 0;
755 perspective->w = 1;
756 }
757 }
758
759 if (translation)
760 {
761 translation->x = n.xw;
762 translation->y = n.yw;
763 translation->z = n.zw;
764 }
765
766 if (skew || scale || rotation)
767 {
768 Eina_Point_3D tsc, tsk, row0, row1, row2, cross;
769 double tmp;
770
771 // Make sure all pointer are defined
772 if (!scale) scale = &tsc;
773 if (!skew) skew = &tsk;
774
775 row0.x = n.xx; row1.x = n.yx; row2.x = n.zx;
776 row0.y = n.xy; row1.y = n.yy; row2.y = n.zy;
777 row0.z = n.xz; row1.z = n.yz; row2.z = n.zz;
778
779 // Compute X scale factor and normalize first row.
780 scale->x = eina_point_3d_norm(&row0);
781 eina_point_3d_normalize(&row0, scale->x);
782
783 skew->x = eina_point_3d_dot(&row0, &row1);
784 eina_point_3d_combine(&row1, &row1, &row0, 1.0, -skew->x);
785
786 // Now, compute Y scale and normalize 2nd row.
787 scale->y = eina_point_3d_norm(&row1);
788 eina_point_3d_normalize(&row1, scale->y);
789 skew->x /= scale->y;
790
791 // Compute XZ and YZ shears, orthogonalize 3rd row
792 skew->y = eina_point_3d_dot(&row0, &row2);
793 eina_point_3d_combine(&row2, &row2, &row0, 1.0, -skew->y);
794 skew->z = eina_point_3d_dot(&row1, &row2);
795 eina_point_3d_combine(&row2, &row2, &row1, 1.0, -skew->z);
796
797 // Next, get Z scale and normalize 3rd row.
798 scale->z = eina_point_3d_norm(&row2);
799 eina_point_3d_normalize(&row2, scale->z);
800
801 tmp = 1 / scale->z;
802 skew->y *= tmp;
803 skew->z *= tmp;
804
805 // At this point, the matrix (in rows) is orthonormal.
806 // Check for a coordinate system flip. If the determinant
807 // is -1, then negate the matrix and the scaling factors.
808 eina_point3d_cross(&cross, &row1, &row2);
809 if (eina_point_3d_dot(&row0, &cross) < 0)
810 {
811 eina_point_3d_dot(scale, scale);
812 eina_point_3d_dot(&row0, &row0);
813 eina_point_3d_dot(&row1, &row1);
814 eina_point_3d_dot(&row2, &row2);
815 }
816
817 if (rotation)
818 {
819 // Now, get the rotations out
820 rotation->x = 0.5 * sqrt(_max(1 + row0.x - row1.y - row2.z, 0));
821 rotation->y = 0.5 * sqrt(_max(1 - row0.x + row1.y - row2.z, 0));
822 rotation->z = 0.5 * sqrt(_max(1 - row0.x - row1.y + row2.z, 0));
823 rotation->w = 0.5 * sqrt(_max(1 + row0.x + row1.y + row2.z, 0));
824
825 if (row2.y > row1.z) rotation->x = - rotation->x;
826 if (row0.z > row2.x) rotation->y = - rotation->y;
827 if (row1.x > row0.y) rotation->z = - rotation->z;
828 }
829 }
830
831 return EINA_TRUE;
832}
833
626EAPI void 834EAPI void
627eina_matrix3_quaternion_get(Eina_Quaternion *q, 835eina_matrix3_quaternion_get(Eina_Quaternion *q,
628 const Eina_Matrix3 *m) 836 const Eina_Matrix3 *m)
@@ -674,3 +882,105 @@ eina_matrix3_quaternion_get(Eina_Quaternion *q,
674 q->y = y; 882 q->y = y;
675 q->z = z; 883 q->z = z;
676} 884}
885
886EAPI void
887eina_quaternion_matrix4_to(Eina_Matrix4 *m,
888 const Eina_Quaternion *rotation,
889 const Eina_Quaternion *perspective,
890 const Eina_Point_3D *translation,
891 const Eina_Point_3D *scale,
892 const Eina_Point_3D *skew)
893{
894 Eina_Matrix4 rm, tmp;
895 double x, y, z, w;
896
897 eina_matrix4_identity(m);
898
899 // apply perspective
900 m->wx = perspective->x;
901 m->wy = perspective->y;
902 m->wz = perspective->z;
903 m->ww = perspective->w;
904
905 // apply translation
906 m->xw = translation->x * m->xx + translation->y * m->xy + translation->z * m->xz;
907 m->yw = translation->x * m->yx + translation->y * m->yy + translation->z * m->yz;
908 m->zw = translation->x * m->zx + translation->y * m->zy + translation->z * m->zz;
909
910 // apply rotation
911 x = rotation->x;
912 y = rotation->y;
913 z = rotation->z;
914 w = rotation->w;
915
916 // Construct a composite rotation matrix from the quaternion values
917 // rotationMatrix is a identity 4x4 matrix initially
918 eina_matrix4_identity(&rm);
919
920 rm.xx = 1 - 2 * (y * y + z * z);
921 rm.xy = 2 * (x * y - z * w);
922 rm.xz = 2 * (x * z + y * w);
923 rm.yx = 2 * (x * y + z * w);
924 rm.yy = 1 - 2 * (x * x + z * z);
925 rm.yz = 2 * (y * z - x * w);
926 rm.zx = 2 * (x * z - y * w);
927 rm.zy = 2 * (y * z + x * w);
928 rm.zz = 1 - 2 * (x * x + y * y);
929
930 eina_matrix4_multiply(&tmp, m, &rm);
931
932 // apply skew
933 // rm is a identity 4x4 matrix initially
934 if (skew->z)
935 {
936 Eina_Matrix4 cp;
937
938 eina_matrix4_identity(&rm);
939 rm.zx = skew->z;
940
941 eina_matrix4_multiply(&cp, &tmp, &rm);
942 tmp = cp;
943 }
944
945 if (skew->y)
946 {
947 Eina_Matrix4 cp;
948
949 eina_matrix4_identity(&rm);
950 rm.zy = 0;
951 rm.zx = skew->y;
952
953 eina_matrix4_multiply(&cp, &tmp, &rm);
954 tmp = cp;
955 }
956
957 if (skew->x)
958 {
959 Eina_Matrix4 cp;
960
961 eina_matrix4_identity(&rm);
962 rm.zx = 0;
963 rm.yx = skew->x;
964
965 eina_matrix4_multiply(&cp, &tmp, &rm);
966 tmp = cp;
967 }
968
969 // apply scale
970 m->xx = tmp.xx * scale->x;
971 m->xy = tmp.xy * scale->x;
972 m->xz = tmp.xz * scale->x;
973 m->xw = tmp.xw;
974 m->yx = tmp.yx * scale->y;
975 m->yy = tmp.yy * scale->y;
976 m->yz = tmp.yz * scale->y;
977 m->yw = tmp.yw;
978 m->zx = tmp.zx * scale->z;
979 m->zy = tmp.zy * scale->z;
980 m->zz = tmp.zz * scale->z;
981 m->zw = tmp.zw;
982 m->wx = tmp.wx;
983 m->wy = tmp.wy;
984 m->wz = tmp.wz;
985 m->ww = tmp.ww;
986}
diff --git a/src/lib/eina/eina_quaternion.h b/src/lib/eina/eina_quaternion.h
index 540c68fcc1..9561ccff2c 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,
129 const Eina_Quaternion *q); /**< @since 1.15 */ 129 const Eina_Quaternion *q); /**< @since 1.15 */
130EAPI void eina_matrix3_quaternion_get(Eina_Quaternion *q, 130EAPI void eina_matrix3_quaternion_get(Eina_Quaternion *q,
131 const Eina_Matrix3 *m); /**< @since 1.15 */ 131 const Eina_Matrix3 *m); /**< @since 1.15 */
132EAPI Eina_Bool eina_matrix4_quaternion_to(Eina_Quaternion *rotation,
133 Eina_Quaternion *perspective,
134 Eina_Point_3D *translation,
135 Eina_Point_3D *scale,
136 Eina_Point_3D *skew,
137 const Eina_Matrix4 *m);
138EAPI void eina_quaternion_matrix4_to(Eina_Matrix4 *m,
139 const Eina_Quaternion *rotation,
140 const Eina_Quaternion *perspective,
141 const Eina_Point_3D *translation,
142 const Eina_Point_3D *scale,
143 const Eina_Point_3D *skew);
132 144
133#endif 145#endif