GLSL QTangent and Quaternion code stuff

qtangent_quaternion.glsl

// Copyright (C) 2017, Benjamin 'BeRo' Rosseaux - [email protected]
// License: CC0

vec4 matrixToQTangent(mat3 m){  
  float f = 1.0;
  if(((((((m[0][0]*m[1][1]*m[2][2])+
         (m[0][1]*m[1][2]*m[2][0])
        )+
        (m[0][2]*m[1][0]*m[2][1])
       )-
       (m[0][2]*m[1][1]*m[2][0])
      )-
      (m[0][1]*m[1][0]*m[2][2])
     )-
     (m[0][0]*m[1][2]*m[2][1]))<0.0){
    f = -1.0;
    m[2] = -m[2];
  }
  float t = m[0][0] + (m[1][1] + m[2][2]);
  vec4 r;
  if(t > 2.9999999){
    r = vec4(0.0, 0.0, 0.0, 1.0);
  }else if(t > 0.0000001){
    float s = sqrt(1.0 + t) * 2.0;
    r = vec4(vec3(m[1][2] - m[2][1], m[2][0] - m[0][2], m[0][1] - m[1][0]) / s, s * 0.25);
  }else if((m[0][0] > m[1][1]) && (m[0][0] > m[2][2])){
    float s = sqrt(1.0 + (m[0][0] - (m[1][1] + m[2][2]))) * 2.0;
    r = vec4(s * 0.25, vec3(m[1][0] - m[0][1], m[2][0] - m[0][2], m[0][2] - m[2][1]) / s);
  }else if(m[1][1] > m[2][2]){
    float s = sqrt(1.0 + (m[1][1] - (m[0][0] + m[2][2]))) * 2.0;
    r = vec4(vec3(m[1][0] + m[0][1], m[2][1] + m[1][2], m[2][0] - m[0][2]) / s, s * 0.25).xwyz;     
  }else{
    float s = sqrt(1.0 + (m[2][2] - (m[0][0] + m[1][1]))) * 2.0;
    r = vec4(vec3(m[2][0] + m[0][2], m[2][1] + m[1][2], m[0][1] - m[1][0]) / s, s * 0.25).xywz;     
  }
  r = normalize(r);
  const float threshold = 1.0 / 32767.0;
  if(r.w <= threshold){
    r = vec4(r.xyz * sqrt(1.0 - (threshold * threshold)), (r.w > 0.0) ? threshold : -threshold);
  }
  if(((f < 0.0) && (r.w >= 0.0)) || ((f >= 0.0) && (r.w < 0.0))){
    r = -r;
  }
  return r; 
}

mat3 QTangentToMatrix(vec4 q){  
  q = normalize(q);
  float qx2 = q.x + q.x,
        qy2 = q.y + q.y,
        qz2 = q.z + q.z,
        qxqx2 = q.x * qx2,
        qxqy2 = q.x * qy2,
        qxqz2 = q.x * qz2,
        qxqw2 = q.w * qx2,
        qyqy2 = q.y * qy2,
        qyqz2 = q.y * qz2,
        qyqw2 = q.w * qy2,
        qzqz2 = q.z * qz2,
        qzqw2 = q.w * qz2;
  mat3 m = mat3(1.0 - (qyqy2 + qzqz2), qxqy2 + qzqw2, qxqz2 - qyqw2,
                qxqy2 - qzqw2, 1.0 - (qxqx2 + qzqz2), qyqz2 + qxqw2,
                qxqz2 + qyqw2, qyqz2 - qxqw2, 1.0 - (qxqx2 + qyqy2));
  m[2] = normalize(cross(m[0], m[1])) * ((q.w < 0.0) ? -1.0 : 1.0);
  return m;
}

vec4 QTangentSlerp(vec4 q0, vec4 q1, float t){
  float co = dot(q0, q1), so, s0, s1, s2 = 1.0, Omega;
  if(co < 0.0){
    co = -co;
    s2 = -s2;
  };
  if((1.0 - co) > 1e-8){
    Omega = acos(co);
    so = sin(Omega);
    s0 = sin((1.0 - t) * Omega) / so;
    s1 = sin(t * Omega) / so;
  }else{
    s0 = 1.0 - t;
    s1 = t;
  }
  vec4 r = ((q0 * s0) + (q1 * (s1 * s2)));
  return r * ((((q0.w < 0.0) || (q1.w < 0.0)) != (r.w < 0.0)) ? -1.0 : 1.0);
}                     

struct Transform {
  vec4 orientation;
  vec4 position;
};
                
vec4 matrixToQuaternion(mat3 m){  
  float t = m[0][0] + (m[1][1] + m[2][2]);
  vec4 r;
  if(t>2.9999999){
    r = vec4(0.0, 0.0, 0.0, 1.0);
  }else if(t > 0.0000001){
    float s = sqrt(1.0 + t) * 2.0;
    r = vec4(vec3(m[1][2] - m[2][1], m[2][0] - m[0][2], m[0][1] - m[1][0]) / s, s * 0.25);
  }else if((m[0][0] > m[1][1]) && (m[0][0] > m[2][2])){
    float s = sqrt(1.0 + (m[0][0] - (m[1][1] + m[2][2]))) * 2.0;
    r = vec4(s * 0.25, vec3(m[1][0] - m[0][1], m[2][0] - m[0][2], m[0][2] - m[2][1]) / s);
  }else if(m[1][1] > m[2][2]){
    float s = sqrt(1.0 + (m[1][1] - (m[0][0] + m[2][2]))) * 2.0;
    r = vec4(vec3(m[1][0] + m[0][1], m[2][1] + m[1][2], m[2][0] - m[0][2]) / s, s * 0.25).xwyz;     
  }else{
    float s = sqrt(1.0 + (m[2][2] - (m[0][0] + m[1][1]))) * 2.0;
    r = vec4(vec3(m[2][0] + m[0][2], m[2][1] + m[1][2], m[0][1] - m[1][0]) / s, s * 0.25).xywz;     
  }
  return normalize(r); 
}

mat3 quaternionToMatrix(vec4 q){  
  q = normalize(q);
  float qx2 = q.x + q.x,
        qy2 = q.y + q.y,
        qz2 = q.z + q.z,
        qxqx2 = q.x * qx2,
        qxqy2 = q.x * qy2,
        qxqz2 = q.x * qz2,
        qxqw2 = q.w * qx2,
        qyqy2 = q.y * qy2,
        qyqz2 = q.y * qz2,
        qyqw2 = q.w * qy2,
        qzqz2 = q.z * qz2,
        qzqw2 = q.w * qz2;
  return mat3(1.0 - (qyqy2 + qzqz2), qxqy2 + qzqw2, qxqz2 - qyqw2,
              qxqy2 - qzqw2, 1.0 - (qxqx2 + qzqz2), qyqz2 + qxqw2,
              qxqz2 + qyqw2, qyqz2 - qxqw2, 1.0 - (qxqx2 + qyqy2));
}

vec4 quaternionSlerp(vec4 q0, vec4 q1, float t){
  float co = dot(q0, q1), so, s0, s1, s2 = 1.0, Omega;
  if(co < 0.0){
    co = -co;
    s2 = -s2;
  };
  if((1.0 - co) > 1e-8){
    Omega = acos(co);
    so = sin(Omega);
    s0 = sin((1.0 - t) * Omega) / so;
    s1 = sin(t * Omega) / so;
  }else{
    s0 = 1.0 - t;
    s1 = t;
  }
  return (q0 * s0) + (q1 * (s1 * s2));
}

mat4 transformToMatrix(const in Transform t){
  mat3 m = quaternionToMatrix(t.orientation);
  return mat4(vec4(m[0], 0.0), vec4(m[1], 0.0), vec4(m[2], 0.0), vec4(t.position.xyz, 1.0));
}

Transform matrixToTransform(const in mat4 m){
  return Transform(matrixToQuaternion(mat3(m)), vec4(m[3].xyz, 1.0));
}

Transform transformLerp(const in Transform a, const in Transform b, const in float x){
  Transform t;
  t.orientation = quaternionSlerp(a.orientation, b.orientation, x);
  t.position = mix(a.position, b.position, x);
  return t; 
}

vec4 quaternionInverse(vec4 q){
 return vec4(-q.xyz, q.w) / length(q);
}

vec4 quaternionMul(vec4 a, vec4 b){
 return vec4(cross(a.xyz, b.xyz) + (a.xyz * b.w) + (b.xyz * a.w), (a.w * b.w) - dot(a.xyz, b.xyz));
}

vec4 quaternionRotateByAngle(vec4 q, vec3 angle){
  float angleScalar = length(angle);
  return (angleScalar < 1e-5) ? q : quaternionMul(q, vec4(angle * (sin(angleScalar * 0.5) / angleScalar), cos(angleScalar * 0.5)));
}

vec3 transformVectorByQuaternion(vec3 v, vec4 q){
  return v + (2.0 * cross(q.xyz, cross(q.xyz, v) + (q.w * v)));
}

vec4 transformVectorByQuaternion(vec4 v, vec4 q){
  return vec4(transformVectorByQuaternion(v.xyz, q), v.w);
}

vec4 quaternionFromToRotation(vec3 from, vec3 to){
  return vec4(cross(normalize(from), normalize(to)), sqrt(dot(from, from) * dot(to, to)) + dot(from, to));
}

vec4 quaternionToAxisAngle(vec4 q){
  q = normalize(q);
  float sinAngle = sqrt(1.0 - (q.w * q.w));
  return vec4(q.xyz / ((abs(sinAngle) < 1e-8) ? 1.0 : sinAngle), 2.0 * acos(q.w));
}

// Simple physics integration example:
// sps.transform.position.xyz += sps.linearVelocity * deltaTime;
// sps.transform.orientation = normalize(sps.transform.orientation + quaternionMul(vec4(sps.angularVelocity, 0.0) * deltaTime * 0.5, sps.transform.orientation));