Continue quat

make.sh

@@ -1,4 +1,4 @@
 #!/bin/sh
 
 rm test_sdl
-g++ -x c -std=c99 -Wall -Wextra -pedantic -O3 -Wall -Wextra -Wno-unused-parameter -Wno-missing-field-initializers -lSDL2 -o test_sdl test_sdl.c && ./test_sdl
+g++ -x c -std=c99 -Wall -Wextra -pedantic -O3 -Wall -Wextra -Wno-unused-parameter -Wno-missing-field-initializers -lSDL2 -o test test.c && ./test

test.c

@@ -1,42 +1,70 @@
 #include "tinyphysicsengine.h"
 #include <stdio.h>
 
-int main(void)
+#define F TPE_FRACTIONS_PER_UNIT
+
+int testRotToQuat(
+  TPE_Unit x, TPE_Unit y, TPE_Unit z, TPE_Unit angle,
+  TPE_Unit expX, TPE_Unit expY, TPE_Unit expZ, TPE_Unit expW)
 {
+  printf("testing axis + rot -> quaternion ([%d,%d,%d] %d -> %d %d %d): ",
+    x,y,z,angle,expW,expX,expY,expZ);
 
+  TPE_Vec4 q, axis;
 
+  TPE_setVec4(&axis,x,y,z,0);
+  TPE_rotationToQuaternion(axis,angle,&q);
 
-TPE_Unit 
-  v1 = 6000,
-  v2 = 0,
-  m1 = 2000,
-  m2 = 2000;
+  #define TOLERANCE 10
 
+  if (q.x > expX + TOLERANCE || q.x < expX - TOLERANCE ||
+      q.y > expY + TOLERANCE || q.y < expY - TOLERANCE ||
+      q.z > expZ + TOLERANCE || q.z < expZ - TOLERANCE ||
+      q.w > expW + TOLERANCE || q.w < expW - TOLERANCE)
+  {
+    printf("%d %d %d %d, ERROR",q.x,q.y,q.z,q.w);
+    return;
+  }
+  
+  puts("OK");
+  return 1;
+
+  #undef TOLERANCE
+}
 
-TPE_getVelocitiesAfterCollision(&v1,&v2,m1,m2,512);
+int testQuatToEuler(TPE_Unit yaw, TPE_Unit pitch, TPE_Unit roll)
+{
 
+  TPE_Vec4 q, q2, axis;
 
-TPE_Vec3 a, b;
 
-a[0] = 1024;
-a[1] = 0;
-a[2] = 0;
 
-b[0] = 512;
-b[1] = 512;
-b[2] = 0;
+  TPE_setVec4(&axis,0,0,F,0);
 
+  TPE_rotationToQuaternion(axis,pitch,&q);
 
-TPE_vec3Normalize(b);
+  
 
+  TPE_Unit y,p,r;
 
-TPE_vec3Project(a,b,a);
+  TPE_quaternionToEulerAngles(q,&y,&p,&r);
 
+  printf("%d %d %d\n",y,p,r);
+  
+}
 
-TPE_PRINTF_VEC3(a);
+int main(void)
+{
+  TPE_Vec4 q1, q2, q3, axis;
 
+  testRotToQuat(F,0,0,    0,    0,0,0,F);
+  testRotToQuat(F,0,0,    F/4,  361,0,0,361);
+  testRotToQuat(0,F,0,    F/4,  0,361,0,361);
+  testRotToQuat(0,0,F,    F/2,  0,0,F,0);
+  testRotToQuat(-F,F,F,   -F/8, 195,-195,-195,472);
 
 
+  testQuatToEuler(0,F/2,0);
 
   return 0;
 }

test_sdl.c

@@ -27,12 +27,13 @@ S3L_Index cubeTriangles[] = { S3L_CUBE_TRIANGLES };
 int main()
 {
 
+
 TPE_Vec4 a, b, r, r2, r3, axis;
 TPE_Unit yaw, pitch, roll;
 
-TPE_setVec4(&axis,0,0,512,0);
+TPE_setVec4(&axis,0,512,0,0);
 
-TPE_rotationToQuaternion(axis,TPE_FRACTIONS_PER_UNIT / 2,&r);
+TPE_rotationToQuaternion(axis,TPE_FRACTIONS_PER_UNIT / 4,&r);
 
 TPE_PRINTF_VEC4(r)
 

tinyphysicsengine.h

@@ -26,6 +26,8 @@
     units, e.g. a unit of velocity (e.g. 1 m/s) is also equal to 1
     TPE_FRACTIONS_PER_UNIT. A full angle is also split into
     TPE_FRACTIONS_PER_UNIT parts (instead of 2 * PI or degrees).
+
+  - Quaternions are represented as vec4 where x ~ i, y ~ j, z ~ k, w ~ real.
 */
 
 #include <stdint.h>
@@ -140,70 +142,70 @@ static const TPE_Unit TPE_sinTable[TPE_SIN_TABLE_LENGTH] =
   /* 511 was chosen here as a highest number that doesn't overflow during
      compilation for TPE_FRACTIONS_PER_UNIT == 1024 */
 
-  (0*S3L_FRACTIONS_PER_UNIT)/511, (6*S3L_FRACTIONS_PER_UNIT)/511, 
-  (12*S3L_FRACTIONS_PER_UNIT)/511, (18*S3L_FRACTIONS_PER_UNIT)/511, 
-  (25*S3L_FRACTIONS_PER_UNIT)/511, (31*S3L_FRACTIONS_PER_UNIT)/511, 
-  (37*S3L_FRACTIONS_PER_UNIT)/511, (43*S3L_FRACTIONS_PER_UNIT)/511, 
-  (50*S3L_FRACTIONS_PER_UNIT)/511, (56*S3L_FRACTIONS_PER_UNIT)/511, 
-  (62*S3L_FRACTIONS_PER_UNIT)/511, (68*S3L_FRACTIONS_PER_UNIT)/511, 
-  (74*S3L_FRACTIONS_PER_UNIT)/511, (81*S3L_FRACTIONS_PER_UNIT)/511, 
-  (87*S3L_FRACTIONS_PER_UNIT)/511, (93*S3L_FRACTIONS_PER_UNIT)/511, 
-  (99*S3L_FRACTIONS_PER_UNIT)/511, (105*S3L_FRACTIONS_PER_UNIT)/511, 
-  (111*S3L_FRACTIONS_PER_UNIT)/511, (118*S3L_FRACTIONS_PER_UNIT)/511, 
-  (124*S3L_FRACTIONS_PER_UNIT)/511, (130*S3L_FRACTIONS_PER_UNIT)/511, 
-  (136*S3L_FRACTIONS_PER_UNIT)/511, (142*S3L_FRACTIONS_PER_UNIT)/511, 
-  (148*S3L_FRACTIONS_PER_UNIT)/511, (154*S3L_FRACTIONS_PER_UNIT)/511, 
-  (160*S3L_FRACTIONS_PER_UNIT)/511, (166*S3L_FRACTIONS_PER_UNIT)/511, 
-  (172*S3L_FRACTIONS_PER_UNIT)/511, (178*S3L_FRACTIONS_PER_UNIT)/511, 
-  (183*S3L_FRACTIONS_PER_UNIT)/511, (189*S3L_FRACTIONS_PER_UNIT)/511, 
-  (195*S3L_FRACTIONS_PER_UNIT)/511, (201*S3L_FRACTIONS_PER_UNIT)/511, 
-  (207*S3L_FRACTIONS_PER_UNIT)/511, (212*S3L_FRACTIONS_PER_UNIT)/511, 
-  (218*S3L_FRACTIONS_PER_UNIT)/511, (224*S3L_FRACTIONS_PER_UNIT)/511, 
-  (229*S3L_FRACTIONS_PER_UNIT)/511, (235*S3L_FRACTIONS_PER_UNIT)/511, 
-  (240*S3L_FRACTIONS_PER_UNIT)/511, (246*S3L_FRACTIONS_PER_UNIT)/511, 
-  (251*S3L_FRACTIONS_PER_UNIT)/511, (257*S3L_FRACTIONS_PER_UNIT)/511, 
-  (262*S3L_FRACTIONS_PER_UNIT)/511, (268*S3L_FRACTIONS_PER_UNIT)/511, 
-  (273*S3L_FRACTIONS_PER_UNIT)/511, (278*S3L_FRACTIONS_PER_UNIT)/511, 
-  (283*S3L_FRACTIONS_PER_UNIT)/511, (289*S3L_FRACTIONS_PER_UNIT)/511, 
-  (294*S3L_FRACTIONS_PER_UNIT)/511, (299*S3L_FRACTIONS_PER_UNIT)/511, 
-  (304*S3L_FRACTIONS_PER_UNIT)/511, (309*S3L_FRACTIONS_PER_UNIT)/511, 
-  (314*S3L_FRACTIONS_PER_UNIT)/511, (319*S3L_FRACTIONS_PER_UNIT)/511, 
-  (324*S3L_FRACTIONS_PER_UNIT)/511, (328*S3L_FRACTIONS_PER_UNIT)/511, 
-  (333*S3L_FRACTIONS_PER_UNIT)/511, (338*S3L_FRACTIONS_PER_UNIT)/511, 
-  (343*S3L_FRACTIONS_PER_UNIT)/511, (347*S3L_FRACTIONS_PER_UNIT)/511, 
-  (352*S3L_FRACTIONS_PER_UNIT)/511, (356*S3L_FRACTIONS_PER_UNIT)/511, 
-  (361*S3L_FRACTIONS_PER_UNIT)/511, (365*S3L_FRACTIONS_PER_UNIT)/511, 
-  (370*S3L_FRACTIONS_PER_UNIT)/511, (374*S3L_FRACTIONS_PER_UNIT)/511, 
-  (378*S3L_FRACTIONS_PER_UNIT)/511, (382*S3L_FRACTIONS_PER_UNIT)/511, 
-  (386*S3L_FRACTIONS_PER_UNIT)/511, (391*S3L_FRACTIONS_PER_UNIT)/511, 
-  (395*S3L_FRACTIONS_PER_UNIT)/511, (398*S3L_FRACTIONS_PER_UNIT)/511, 
-  (402*S3L_FRACTIONS_PER_UNIT)/511, (406*S3L_FRACTIONS_PER_UNIT)/511, 
-  (410*S3L_FRACTIONS_PER_UNIT)/511, (414*S3L_FRACTIONS_PER_UNIT)/511, 
-  (417*S3L_FRACTIONS_PER_UNIT)/511, (421*S3L_FRACTIONS_PER_UNIT)/511, 
-  (424*S3L_FRACTIONS_PER_UNIT)/511, (428*S3L_FRACTIONS_PER_UNIT)/511, 
-  (431*S3L_FRACTIONS_PER_UNIT)/511, (435*S3L_FRACTIONS_PER_UNIT)/511, 
-  (438*S3L_FRACTIONS_PER_UNIT)/511, (441*S3L_FRACTIONS_PER_UNIT)/511, 
-  (444*S3L_FRACTIONS_PER_UNIT)/511, (447*S3L_FRACTIONS_PER_UNIT)/511, 
-  (450*S3L_FRACTIONS_PER_UNIT)/511, (453*S3L_FRACTIONS_PER_UNIT)/511, 
-  (456*S3L_FRACTIONS_PER_UNIT)/511, (459*S3L_FRACTIONS_PER_UNIT)/511, 
-  (461*S3L_FRACTIONS_PER_UNIT)/511, (464*S3L_FRACTIONS_PER_UNIT)/511, 
-  (467*S3L_FRACTIONS_PER_UNIT)/511, (469*S3L_FRACTIONS_PER_UNIT)/511, 
-  (472*S3L_FRACTIONS_PER_UNIT)/511, (474*S3L_FRACTIONS_PER_UNIT)/511, 
-  (476*S3L_FRACTIONS_PER_UNIT)/511, (478*S3L_FRACTIONS_PER_UNIT)/511, 
-  (481*S3L_FRACTIONS_PER_UNIT)/511, (483*S3L_FRACTIONS_PER_UNIT)/511, 
-  (485*S3L_FRACTIONS_PER_UNIT)/511, (487*S3L_FRACTIONS_PER_UNIT)/511, 
-  (488*S3L_FRACTIONS_PER_UNIT)/511, (490*S3L_FRACTIONS_PER_UNIT)/511, 
-  (492*S3L_FRACTIONS_PER_UNIT)/511, (494*S3L_FRACTIONS_PER_UNIT)/511, 
-  (495*S3L_FRACTIONS_PER_UNIT)/511, (497*S3L_FRACTIONS_PER_UNIT)/511, 
-  (498*S3L_FRACTIONS_PER_UNIT)/511, (499*S3L_FRACTIONS_PER_UNIT)/511, 
-  (501*S3L_FRACTIONS_PER_UNIT)/511, (502*S3L_FRACTIONS_PER_UNIT)/511, 
-  (503*S3L_FRACTIONS_PER_UNIT)/511, (504*S3L_FRACTIONS_PER_UNIT)/511, 
-  (505*S3L_FRACTIONS_PER_UNIT)/511, (506*S3L_FRACTIONS_PER_UNIT)/511, 
-  (507*S3L_FRACTIONS_PER_UNIT)/511, (507*S3L_FRACTIONS_PER_UNIT)/511, 
-  (508*S3L_FRACTIONS_PER_UNIT)/511, (509*S3L_FRACTIONS_PER_UNIT)/511, 
-  (509*S3L_FRACTIONS_PER_UNIT)/511, (510*S3L_FRACTIONS_PER_UNIT)/511, 
-  (510*S3L_FRACTIONS_PER_UNIT)/511, (510*S3L_FRACTIONS_PER_UNIT)/511, 
-  (510*S3L_FRACTIONS_PER_UNIT)/511, (510*S3L_FRACTIONS_PER_UNIT)/511
+  (0*TPE_FRACTIONS_PER_UNIT)/511, (6*TPE_FRACTIONS_PER_UNIT)/511, 
+  (12*TPE_FRACTIONS_PER_UNIT)/511, (18*TPE_FRACTIONS_PER_UNIT)/511, 
+  (25*TPE_FRACTIONS_PER_UNIT)/511, (31*TPE_FRACTIONS_PER_UNIT)/511, 
+  (37*TPE_FRACTIONS_PER_UNIT)/511, (43*TPE_FRACTIONS_PER_UNIT)/511, 
+  (50*TPE_FRACTIONS_PER_UNIT)/511, (56*TPE_FRACTIONS_PER_UNIT)/511, 
+  (62*TPE_FRACTIONS_PER_UNIT)/511, (68*TPE_FRACTIONS_PER_UNIT)/511, 
+  (74*TPE_FRACTIONS_PER_UNIT)/511, (81*TPE_FRACTIONS_PER_UNIT)/511, 
+  (87*TPE_FRACTIONS_PER_UNIT)/511, (93*TPE_FRACTIONS_PER_UNIT)/511, 
+  (99*TPE_FRACTIONS_PER_UNIT)/511, (105*TPE_FRACTIONS_PER_UNIT)/511, 
+  (111*TPE_FRACTIONS_PER_UNIT)/511, (118*TPE_FRACTIONS_PER_UNIT)/511, 
+  (124*TPE_FRACTIONS_PER_UNIT)/511, (130*TPE_FRACTIONS_PER_UNIT)/511, 
+  (136*TPE_FRACTIONS_PER_UNIT)/511, (142*TPE_FRACTIONS_PER_UNIT)/511, 
+  (148*TPE_FRACTIONS_PER_UNIT)/511, (154*TPE_FRACTIONS_PER_UNIT)/511, 
+  (160*TPE_FRACTIONS_PER_UNIT)/511, (166*TPE_FRACTIONS_PER_UNIT)/511, 
+  (172*TPE_FRACTIONS_PER_UNIT)/511, (178*TPE_FRACTIONS_PER_UNIT)/511, 
+  (183*TPE_FRACTIONS_PER_UNIT)/511, (189*TPE_FRACTIONS_PER_UNIT)/511, 
+  (195*TPE_FRACTIONS_PER_UNIT)/511, (201*TPE_FRACTIONS_PER_UNIT)/511, 
+  (207*TPE_FRACTIONS_PER_UNIT)/511, (212*TPE_FRACTIONS_PER_UNIT)/511, 
+  (218*TPE_FRACTIONS_PER_UNIT)/511, (224*TPE_FRACTIONS_PER_UNIT)/511, 
+  (229*TPE_FRACTIONS_PER_UNIT)/511, (235*TPE_FRACTIONS_PER_UNIT)/511, 
+  (240*TPE_FRACTIONS_PER_UNIT)/511, (246*TPE_FRACTIONS_PER_UNIT)/511, 
+  (251*TPE_FRACTIONS_PER_UNIT)/511, (257*TPE_FRACTIONS_PER_UNIT)/511, 
+  (262*TPE_FRACTIONS_PER_UNIT)/511, (268*TPE_FRACTIONS_PER_UNIT)/511, 
+  (273*TPE_FRACTIONS_PER_UNIT)/511, (278*TPE_FRACTIONS_PER_UNIT)/511, 
+  (283*TPE_FRACTIONS_PER_UNIT)/511, (289*TPE_FRACTIONS_PER_UNIT)/511, 
+  (294*TPE_FRACTIONS_PER_UNIT)/511, (299*TPE_FRACTIONS_PER_UNIT)/511, 
+  (304*TPE_FRACTIONS_PER_UNIT)/511, (309*TPE_FRACTIONS_PER_UNIT)/511, 
+  (314*TPE_FRACTIONS_PER_UNIT)/511, (319*TPE_FRACTIONS_PER_UNIT)/511, 
+  (324*TPE_FRACTIONS_PER_UNIT)/511, (328*TPE_FRACTIONS_PER_UNIT)/511, 
+  (333*TPE_FRACTIONS_PER_UNIT)/511, (338*TPE_FRACTIONS_PER_UNIT)/511, 
+  (343*TPE_FRACTIONS_PER_UNIT)/511, (347*TPE_FRACTIONS_PER_UNIT)/511, 
+  (352*TPE_FRACTIONS_PER_UNIT)/511, (356*TPE_FRACTIONS_PER_UNIT)/511, 
+  (361*TPE_FRACTIONS_PER_UNIT)/511, (365*TPE_FRACTIONS_PER_UNIT)/511, 
+  (370*TPE_FRACTIONS_PER_UNIT)/511, (374*TPE_FRACTIONS_PER_UNIT)/511, 
+  (378*TPE_FRACTIONS_PER_UNIT)/511, (382*TPE_FRACTIONS_PER_UNIT)/511, 
+  (386*TPE_FRACTIONS_PER_UNIT)/511, (391*TPE_FRACTIONS_PER_UNIT)/511, 
+  (395*TPE_FRACTIONS_PER_UNIT)/511, (398*TPE_FRACTIONS_PER_UNIT)/511, 
+  (402*TPE_FRACTIONS_PER_UNIT)/511, (406*TPE_FRACTIONS_PER_UNIT)/511, 
+  (410*TPE_FRACTIONS_PER_UNIT)/511, (414*TPE_FRACTIONS_PER_UNIT)/511, 
+  (417*TPE_FRACTIONS_PER_UNIT)/511, (421*TPE_FRACTIONS_PER_UNIT)/511, 
+  (424*TPE_FRACTIONS_PER_UNIT)/511, (428*TPE_FRACTIONS_PER_UNIT)/511, 
+  (431*TPE_FRACTIONS_PER_UNIT)/511, (435*TPE_FRACTIONS_PER_UNIT)/511, 
+  (438*TPE_FRACTIONS_PER_UNIT)/511, (441*TPE_FRACTIONS_PER_UNIT)/511, 
+  (444*TPE_FRACTIONS_PER_UNIT)/511, (447*TPE_FRACTIONS_PER_UNIT)/511, 
+  (450*TPE_FRACTIONS_PER_UNIT)/511, (453*TPE_FRACTIONS_PER_UNIT)/511, 
+  (456*TPE_FRACTIONS_PER_UNIT)/511, (459*TPE_FRACTIONS_PER_UNIT)/511, 
+  (461*TPE_FRACTIONS_PER_UNIT)/511, (464*TPE_FRACTIONS_PER_UNIT)/511, 
+  (467*TPE_FRACTIONS_PER_UNIT)/511, (469*TPE_FRACTIONS_PER_UNIT)/511, 
+  (472*TPE_FRACTIONS_PER_UNIT)/511, (474*TPE_FRACTIONS_PER_UNIT)/511, 
+  (476*TPE_FRACTIONS_PER_UNIT)/511, (478*TPE_FRACTIONS_PER_UNIT)/511, 
+  (481*TPE_FRACTIONS_PER_UNIT)/511, (483*TPE_FRACTIONS_PER_UNIT)/511, 
+  (485*TPE_FRACTIONS_PER_UNIT)/511, (487*TPE_FRACTIONS_PER_UNIT)/511, 
+  (488*TPE_FRACTIONS_PER_UNIT)/511, (490*TPE_FRACTIONS_PER_UNIT)/511, 
+  (492*TPE_FRACTIONS_PER_UNIT)/511, (494*TPE_FRACTIONS_PER_UNIT)/511, 
+  (495*TPE_FRACTIONS_PER_UNIT)/511, (497*TPE_FRACTIONS_PER_UNIT)/511, 
+  (498*TPE_FRACTIONS_PER_UNIT)/511, (499*TPE_FRACTIONS_PER_UNIT)/511, 
+  (501*TPE_FRACTIONS_PER_UNIT)/511, (502*TPE_FRACTIONS_PER_UNIT)/511, 
+  (503*TPE_FRACTIONS_PER_UNIT)/511, (504*TPE_FRACTIONS_PER_UNIT)/511, 
+  (505*TPE_FRACTIONS_PER_UNIT)/511, (506*TPE_FRACTIONS_PER_UNIT)/511, 
+  (507*TPE_FRACTIONS_PER_UNIT)/511, (507*TPE_FRACTIONS_PER_UNIT)/511, 
+  (508*TPE_FRACTIONS_PER_UNIT)/511, (509*TPE_FRACTIONS_PER_UNIT)/511, 
+  (509*TPE_FRACTIONS_PER_UNIT)/511, (510*TPE_FRACTIONS_PER_UNIT)/511, 
+  (510*TPE_FRACTIONS_PER_UNIT)/511, (510*TPE_FRACTIONS_PER_UNIT)/511, 
+  (510*TPE_FRACTIONS_PER_UNIT)/511, (510*TPE_FRACTIONS_PER_UNIT)/511
 };
 
 #define TPE_SIN_TABLE_UNIT_STEP\
@@ -274,7 +276,7 @@ TPE_Unit TPE_cos(TPE_Unit x)
 
 TPE_Unit TPE_asin(TPE_Unit x)
 {
-  x = TPE_clamp(x,-S3L_FRACTIONS_PER_UNIT,S3L_FRACTIONS_PER_UNIT);
+  x = TPE_clamp(x,-TPE_FRACTIONS_PER_UNIT,TPE_FRACTIONS_PER_UNIT);
 
   int8_t sign = 1;
 
@@ -285,14 +287,14 @@ TPE_Unit TPE_asin(TPE_Unit x)
   }
 
   int16_t low = 0;
-  int16_t high = S3L_SIN_TABLE_LENGTH -1;
+  int16_t high = TPE_SIN_TABLE_LENGTH -1;
   int16_t middle;
 
   while (low <= high) // binary search
   {
     middle = (low + high) / 2;
 
-    S3L_Unit v = S3L_sinTable[middle];
+    TPE_Unit v = TPE_sinTable[middle];
 
     if (v > x)
       high = middle - 1;
@@ -329,7 +331,7 @@ TPE_Unit TPE_atan2(TPE_Unit x, TPE_Unit y)
       (TPE_FRACTIONS_PER_UNIT / -4);
   }
 
-  TPE_Unit at = TPE_atan((x * TPE_FRACTIONS_PER_UNIT) / y);
+  TPE_Unit at = TPE_atan((y * TPE_FRACTIONS_PER_UNIT) / TPE_nonZero(x));
 
   if (x > 0)
     return at;
@@ -347,10 +349,10 @@ void TPE_initBody(TPE_Body *body)
 
   // init orientation to identity unit quaternion (1,0,0,0):
 
-  body->orientation.x = TPE_FRACTIONS_PER_UNIT;
+  body->orientation.x = 0;
   body->orientation.y = 0;
   body->orientation.z = 0;
-  body->orientation.w = 0;
+  body->orientation.w = TPE_FRACTIONS_PER_UNIT;
 }
 
 void TPE_quaternionMultiply(TPE_Vec4 a, TPE_Vec4 b, TPE_Vec4 *result)
@@ -386,13 +388,13 @@ void TPE_rotationToQuaternion(TPE_Vec4 axis, TPE_Unit angle, TPE_Vec4 *quaternio
 
   angle /= 2;
 
-  quaternion->x = TPE_cos(angle);
-
   TPE_Unit s = TPE_sin(angle);
 
-  quaternion->y = (s * axis.x) / TPE_FRACTIONS_PER_UNIT;
-  quaternion->z = (s * axis.y) / TPE_FRACTIONS_PER_UNIT;
-  quaternion->w = (s * axis.z) / TPE_FRACTIONS_PER_UNIT;
+  quaternion->x = (s * axis.x) / TPE_FRACTIONS_PER_UNIT;
+  quaternion->y = (s * axis.y) / TPE_FRACTIONS_PER_UNIT;
+  quaternion->z = (s * axis.z) / TPE_FRACTIONS_PER_UNIT;
+
+  quaternion->w = TPE_cos(angle);
 }
 
 void TPE_quaternionToRotation(TPE_Vec4 quaternion, TPE_Vec4 *axis, TPE_Unit *angle)
@@ -413,33 +415,32 @@ void TPE_quaternionToRotation(TPE_Vec4 quaternion, TPE_Vec4 *axis, TPE_Unit *ang
 void TPE_quaternionToEulerAngles(TPE_Vec4 quaternion, TPE_Unit *yaw, 
   TPE_Unit *pitch, TPE_Unit *roll)
 {
-  *yaw = 
+  *roll = 
      TPE_atan2(
-     (  (2 * (quaternion.x * quaternion.y + quaternion.z * quaternion.w)) 
-        / TPE_FRACTIONS_PER_UNIT    
+     (  (2 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z)) 
+        / TPE_FRACTIONS_PER_UNIT 
      ),
-     (  
+     (
        TPE_FRACTIONS_PER_UNIT - 
-        (2 * (quaternion.y * quaternion.y + 
-          quaternion.z * quaternion.z)  / TPE_FRACTIONS_PER_UNIT))
+       ((2 * (quaternion.x * quaternion.x + 
+         quaternion.y * quaternion.y))  / TPE_FRACTIONS_PER_UNIT))
      );
 
   *pitch =
      TPE_asin(
-       (2 * quaternion.x * quaternion.z - quaternion.w * quaternion.y) / TPE_FRACTIONS_PER_UNIT
+       (2 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x)) / TPE_FRACTIONS_PER_UNIT
     );
 
-  *roll =
+  *yaw =
      TPE_atan2(
-     (  (2 * (quaternion.x * quaternion.w + quaternion.y * quaternion.z))
-        / TPE_FRACTIONS_PER_UNIT   
+     (  (2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y))
+        / TPE_FRACTIONS_PER_UNIT
      ),
      (  
        TPE_FRACTIONS_PER_UNIT - 
-        (2 * (quaternion.z * quaternion.z + 
-          quaternion.w * quaternion.w)  / TPE_FRACTIONS_PER_UNIT))
+        ((2 * (quaternion.y * quaternion.y + 
+          quaternion.z * quaternion.z))  / TPE_FRACTIONS_PER_UNIT))
      );
-
 }
 
 void TPE_quaternionToRotationMatrix(TPE_Vec4 quaternion, TPE_Unit matrix[16])