vec4 quaternionFromAxisAngle (float angle, vec3 axis){
  angle /= 2.;
  float s = sin(angle);
  vec4 res;
  res.xyz = s * axis;
  res.w = cos(angle);
  return res;
}

vec4 quaternionFromAxis (vec3 xAxis,vec3 yAxis,vec3 zAxis){
  mat3 m = mat3(xAxis,yAxis,zAxis);
  float trace = m[0][0] + m[1][1] + m[2][2];
  vec4 quat;

  if (trace > 0.) {
    float s = 0.5 / sqrt(trace + 1.0);
    quat.w = 0.25 / s;
    quat.x = (m[2][1] - m[1][2]) * s;
    quat.y = (m[0][2] - m[2][0]) * s;
    quat.z = (m[1][0] - m[0][1]) * s;
  } else if ((m[0][0] > m[1][1]) && (m[0][0] > m[2][2])) {
    float s = 2.0 * sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]);
    quat.w = (m[2][1] - m[1][2]) / s;
    quat.x = 0.25 * s;
    quat.y = (m[0][1] + m[1][0]) / s;
    quat.z = (m[0][2] + m[2][0]) / s;
  } else if (m[1][1] > m[2][2]) {
    float s = 2.0 * sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]);
    quat.w = (m[0][2] - m[2][0]) / s;
    quat.x = (m[0][1] + m[1][0]) / s;
    quat.y = 0.25 * s;
    quat.z = (m[1][2] + m[2][1]) / s;
  } else {
    float s = 2.0 * sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]);
    quat.w = (m[1][0] - m[0][1]) / s;
    quat.x = (m[0][2] + m[2][0]) / s;
    quat.y = (m[1][2] + m[2][1]) / s;
    quat.z = 0.25 * s;
  }

  float len = quat.x * quat.x + quat.y * quat.y + quat.z * quat.z + quat.w * quat.w;
  if (len > 0.) {
    len = 1. / sqrt(len);
    quat.x = quat.x * len;
    quat.y = quat.y * len;
    quat.z = quat.z * len;
    quat.w = quat.w * len;
  }

  return quat;
}

vec4 quaternionFromEuler (vec3 angle){
  float x = angle.x / 2.;
  float y = angle.y / 2.;
  float z = angle.z / 2.;

  float sx = sin(x);
  float cx = cos(x);
  float sy = sin(y);
  float cy = cos(y);
  float sz = sin(z);
  float cz = cos(z);

  vec4 quat = vec4(0);

  quat.x = sx * cy * cz + cx * sy * sz;
  quat.y = cx * sy * cz + sx * cy * sz;
  quat.z = cx * cy * sz - sx * sy * cz;
  quat.w = cx * cy * cz - sx * sy * sz;

  return quat;
}

mat4 matrixFromRT (vec4 q, vec3 p){
  float x2 = q.x + q.x;
  float y2 = q.y + q.y;
  float z2 = q.z + q.z;
  float xx = q.x * x2;
  float xy = q.x * y2;
  float xz = q.x * z2;
  float yy = q.y * y2;
  float yz = q.y * z2;
  float zz = q.z * z2;
  float wx = q.w * x2;
  float wy = q.w * y2;
  float wz = q.w * z2;

  return mat4(
    1. - (yy + zz), xy + wz, xz - wy, 0,
    xy - wz, 1. - (xx + zz), yz + wx, 0,
    xz + wy, yz - wx, 1. - (xx + yy), 0,
    p.x, p.y, p.z, 1
  );
}

mat4 matFromRTS (vec4 q, vec3 t, vec3 s){
  float x = q.x, y = q.y, z = q.z, w = q.w;
  float x2 = x + x;
  float y2 = y + y;
  float z2 = z + z;

  float xx = x * x2;
  float xy = x * y2;
  float xz = x * z2;
  float yy = y * y2;
  float yz = y * z2;
  float zz = z * z2;
  float wx = w * x2;
  float wy = w * y2;
  float wz = w * z2;
  float sx = s.x;
  float sy = s.y;
  float sz = s.z;

  return mat4((1. - (yy + zz)) * sx, (xy + wz) * sx, (xz - wy) * sx, 0,
    (xy - wz) * sy, (1. - (xx + zz)) * sy, (yz + wx) * sy, 0,
    (xz + wy) * sz, (yz - wx) * sz, (1. - (xx + yy)) * sz, 0,
    t.x, t.y, t.z, 1);
}

void scaleMatrix (inout mat4 m, float s){
  m[0].xyz *= s;
  m[1].xyz *= s;
  m[2].xyz *= s;
}

vec4 quatMultiply (vec4 a, vec4 b){
  vec4 quat;
  quat.x = a.x * b.w + a.w * b.x + a.y * b.z - a.z * b.y;
  quat.y = a.y * b.w + a.w * b.y + a.z * b.x - a.x * b.z;
  quat.z = a.z * b.w + a.w * b.z + a.x * b.y - a.y * b.x;
  quat.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
  return quat;
}

void rotateVecFromQuat (inout vec3 v, vec4 q){
  float ix = q.w * v.x + q.y * v.z - q.z * v.y;
  float iy = q.w * v.y + q.z * v.x - q.x * v.z;
  float iz = q.w * v.z + q.x * v.y - q.y * v.x;
  float iw = -q.x * v.x - q.y * v.y - q.z * v.z;

  // calculate result * inverse quat
  v.x = ix * q.w + iw * -q.x + iy * -q.z - iz * -q.y;
  v.y = iy * q.w + iw * -q.y + iz * -q.x - ix * -q.z;
  v.z = iz * q.w + iw * -q.z + ix * -q.y - iy * -q.x;
}

vec3 rotateVecFromAxis (vec3 v, vec3 axis, float theta){
  return cos(theta) * v + sin(theta) * cross(v, axis) + (1. - cos(theta)) * dot(v, axis) * axis;
}

vec3 rotateInLocalSpace (vec3 pos, vec3 xAxis, vec3 yAxis, vec3 zAxis, vec4 q){
  vec4 viewQuat = quaternionFromAxis(xAxis, yAxis, zAxis);
  vec4 rotQuat = quatMultiply(viewQuat, q);
  rotateVecFromQuat(pos, rotQuat);
  return pos;
}

void rotateCorner (inout vec2 corner, float angle){
  float xOS = cos(angle) * corner.x - sin(angle) * corner.y;
  float yOS = sin(angle) * corner.x + cos(angle) * corner.y;
  corner.x = xOS;
  corner.y = yOS;
}

mat3 quatToMat3(vec4 q) {
  vec3 m0 = vec3(
    1.0 - 2.0 * q.y * q.y - 2.0 * q.z * q.z,
    2.0 * q.x * q.y + 2.0 * q.w * q.z,
    2.0 * q.x * q.z - 2.0 * q.w * q.y);
	vec3 m1 = vec3(
    2.0 * q.x * q.y - 2.0 * q.w * q.z,
    1.0 - 2.0 * q.x * q.x - 2.0 * q.z * q.z,
    2.0 * q.y * q.z + 2.0 * q.w * q.x);
	vec3 m2 = vec3(
    2.0 * q.x * q.z + 2.0 * q.w * q.y,
    2.0 * q.y * q.z - 2.0 * q.w * q.x,
    1.0 - 2.0 * q.x * q.x - 2.0 * q.y * q.y);
  return mat3(m0, m1, m2);
}

mat4 quatToMat4(vec4 q) {
  mat3 m3 = quatToMat3(q);
  return mat4(vec4(m3[0], 0.0), vec4(m3[1], 0.0), vec4(m3[2], 0.0), vec4(0.0, 0.0, 0.0, 1.0));
}

vec4 mat3ToQuat(mat3 mat) {
  float tr = mat[0][0] + mat[1][1] + mat[2][2];
	float qw, qx, qy, qz;
  if (tr > 0.0) {
    float S = sqrt(tr + 1.0) * 2.0; // S=4*qw
	  float invS = 1.0 / S;
	  qw = 0.25 * S;
	  qx = (mat[1][2] - mat[2][1]) * invS;
	  qy = (mat[2][0] - mat[0][2]) * invS;
	  qz = (mat[0][1] - mat[1][0]) * invS;
  } else if ((mat[0][0] > mat[1][1])&&(mat[0][0] > mat[2][2])) {
    float S = sqrt(1.0 + mat[0][0] - mat[1][1] - mat[2][2]) * 2.0; // S=4*qx
	  float invS = 1.0 / S;
	  qw = (mat[1][2] - mat[2][1]) * invS;
	  qx = 0.25 * S;
	  qy = (mat[1][0] + mat[0][1]) * invS;
	  qz = (mat[2][0] + mat[0][2]) * invS;
  } else if (mat[1][1] > mat[2][2]) {
	  float S = sqrt(1.0 + mat[1][1] - mat[0][0] - mat[2][2]) * 2.0; // S=4*qy
	  float invS = 1.0 / S;
	  qw = (mat[2][0] - mat[0][2]) * invS;
	  qx = (mat[1][0] + mat[0][1]) * invS;
	  qy = 0.25 * S;
	  qz = (mat[2][1] + mat[1][2]) * invS;
  } else {
	  float S = sqrt(1.0 + mat[2][2] - mat[0][0] - mat[1][1]) * 2.0; // S=4*qz
	  float invS = 1.0 / S;
	  qw = (mat[0][1] - mat[1][0]) * invS;
	  qx = (mat[2][0] + mat[0][2]) * invS;
	  qy = (mat[2][1] + mat[1][2]) * invS;
	  qz = 0.25 * S;
  }
  return vec4(qx, qy, qz, qw);
}

vec4 eulerToQuat(vec3 euler) {
  vec3 er = euler * 0.5;
  float x = er.x, y = er.y, z = er.z;

  float sx = sin(x);
  float cx = cos(x);
  float sy = sin(y);
  float cy = cos(y);
  float sz = sin(z);
  float cz = cos(z);

  vec4 quat;
  quat.x = sx * cy * cz + cx * sy * sz;
  quat.y = cx * sy * cz + sx * cy * sz;
  quat.z = cx * cy * sz - sx * sy * cz;
  quat.w = cx * cy * cz - sx * sy * sz;
  return quat;
}

float atan2(float y, float x) {
  return x == 0.0 ? sign(y) * 3.1416 * 0.5 : atan(y, x);
}

vec3 quatToEuler(vec4 q) {
  float x = q.x, y = q.y, z = q.z, w = q.w;
  float bank = 0.0, heading = 0.0, attitude = 0.0;
  float test = x * y + z * w;
  if (test > 0.499999) {
    bank = 0.0; // default to zero
    heading = (2.0 * atan2(x, w));
    attitude = 3.1416 * 0.5;
  } else if (test < -0.499999) {
    bank = 0.0; // default to zero
    heading = -(2.0 * atan2(x, w));
    attitude = -3.1416 * 0.5;
  } else {
    float sqx = x * x;
    float sqy = y * y;
    float sqz = z * z;
    bank = (atan2(2.0 * x * w - 2.0 * y * z, 1.0 - 2.0 * sqx - 2.0 * sqz));
    heading = (atan2(2.0 * y * w - 2.0 * x * z, 1.0 - 2.0 * sqy - 2.0 * sqz));
    attitude = (asin(2.0 * test));
  }
  return vec3(bank, heading, attitude);
}

void departWorldMatrix(out vec3 translation, out vec3 scale, mat4 matWorld)
{
  scale = vec3(length(matWorld[0].xyz), length(matWorld[1].xyz), length(matWorld[2].xyz));
  translation = vec3(matWorld[0].w, matWorld[1].w, matWorld[2].w);
}

void departWorldMatrix(out vec3 translation, out vec3 scale, out mat3 matRotation, mat4 matWorld)
{
  departWorldMatrix(translation, scale, matWorld);
  matRotation[0] = matWorld[0].xyz / scale.x;
  matRotation[1] = matWorld[1].xyz / scale.y;
  matRotation[2] = matWorld[2].xyz / scale.z;
}