153 lines
3.0 KiB
Plaintext
153 lines
3.0 KiB
Plaintext
vec4 quaternionFromMatrix( mat3 m )
|
|
{
|
|
vec4 qa = vec4( 0.0, 0.0, 0.0, 1.0 );
|
|
|
|
float trace = m[ 0 ][ 0 ] + m[ 1 ][ 1 ] + m[ 2 ][ 2 ];
|
|
|
|
if ( trace > 0.0 )
|
|
{
|
|
|
|
float s = 0.5 / sqrt( trace + 1.0 );
|
|
|
|
qa.w = 0.25 / s;
|
|
qa.x = ( m[ 1 ][ 2 ] - m[ 2 ][ 1 ] ) * s;
|
|
qa.y = ( m[ 2 ][ 0 ] - m[ 0 ][ 2 ] ) * s;
|
|
qa.z = ( m[ 0 ][ 1 ] - m[ 1 ][ 0 ] ) * 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 ] );
|
|
|
|
qa.w = ( m[ 1 ][ 2 ] - m[ 2 ][ 1 ] ) / s;
|
|
qa.x = 0.25 * s;
|
|
qa.y = ( m[ 1 ][ 0 ] + m[ 0 ][ 1 ] ) / s;
|
|
qa.z = ( m[ 2 ][ 0 ] + m[ 0 ][ 2 ] ) / 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 ] );
|
|
|
|
qa.w = ( m[ 2 ][ 0 ] - m[ 0 ][ 2 ] ) / s;
|
|
qa.x = ( m[ 1 ][ 0 ] + m[ 0 ][ 1 ] ) / s;
|
|
qa.y = 0.25 * s;
|
|
qa.z = ( m[ 2 ][ 1 ] + m[ 1 ][ 2 ] ) / s;
|
|
|
|
}
|
|
else
|
|
{
|
|
|
|
float s = 2.0 * sqrt( 1.0 + m[ 2 ][ 2 ] - m[ 0 ][ 0 ] - m[ 1 ][ 1 ] );
|
|
|
|
qa.w = ( m[ 0 ][ 1 ] - m[ 1 ][ 0 ] ) / s;
|
|
qa.x = ( m[ 2 ][ 0 ] + m[ 0 ][ 2 ] ) / s;
|
|
qa.y = ( m[ 2 ][ 1 ] + m[ 1 ][ 2 ] ) / s;
|
|
qa.z = 0.25 * s;
|
|
}
|
|
|
|
return qa;
|
|
}
|
|
|
|
vec4 quaternionMultiply( vec4 a, vec4 b )
|
|
{
|
|
|
|
// from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm
|
|
|
|
float qax = a.x;
|
|
float qay = a.y;
|
|
float qaz = a.z;
|
|
float qaw = a.w;
|
|
|
|
float qbx = b.x;
|
|
float qby = b.y;
|
|
float qbz = b.z;
|
|
float qbw = b.w;
|
|
|
|
vec4 q = vec4( 0.0, 0.0, 0.0, 0.0 );
|
|
|
|
q.x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;
|
|
q.y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz;
|
|
q.z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx;
|
|
q.w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;
|
|
|
|
return q;
|
|
}
|
|
|
|
vec4 quaternionSlerp( vec4 qa, vec4 qb, float t )
|
|
{
|
|
if ( t == 0.0 )
|
|
{
|
|
return qa;
|
|
}
|
|
|
|
if ( t == 1.0 )
|
|
{
|
|
return qb;
|
|
}
|
|
|
|
float x = qa.x;
|
|
float y = qa.y;
|
|
float z = qa.z;
|
|
float w = qa.w;
|
|
|
|
|
|
float cosHalfTheta = w * qb.w + x * qb.x + y * qb.y + z * qb.z;
|
|
|
|
if ( cosHalfTheta < 0.0 )
|
|
{
|
|
qa.w = - qb.w;
|
|
qa.x = - qb.x;
|
|
qa.y = - qb.y;
|
|
qa.z = - qb.z;
|
|
|
|
cosHalfTheta = - cosHalfTheta;
|
|
}
|
|
else
|
|
{
|
|
|
|
qa = qb;
|
|
}
|
|
|
|
if ( cosHalfTheta >= 1.0 )
|
|
{
|
|
|
|
qa.w = w;
|
|
qa.x = x;
|
|
qa.y = y;
|
|
qa.z = z;
|
|
|
|
return qa;
|
|
}
|
|
|
|
float sqrSinHalfTheta = 1.0 - cosHalfTheta * cosHalfTheta;
|
|
|
|
if ( sqrSinHalfTheta <= 0.000001 )
|
|
{
|
|
|
|
float s = 1.0 - t;
|
|
|
|
qa.w = s * w + t * qa.w;
|
|
qa.x = s * x + t * qa.x;
|
|
qa.y = s * y + t * qa.y;
|
|
qa.z = s * z + t * qa.z;
|
|
|
|
qa = normalize( qa );
|
|
|
|
return qa;
|
|
|
|
}
|
|
|
|
float sinHalfTheta = sqrt( sqrSinHalfTheta );
|
|
float halfTheta = atan( sinHalfTheta, cosHalfTheta );
|
|
float ratioA = sin( ( 1.0 - t ) * halfTheta ) / sinHalfTheta;
|
|
float ratioB = sin( t * halfTheta ) / sinHalfTheta;
|
|
|
|
qa.w = ( w * ratioA + qa.w * ratioB );
|
|
qa.x = ( x * ratioA + qa.x * ratioB );
|
|
qa.y = ( y * ratioA + qa.y * ratioB );
|
|
qa.z = ( z * ratioA + qa.z * ratioB );
|
|
|
|
return qa;
|
|
} |