Post by DJLinux on Mar 12, 2012 16:45:29 GMT -5
QuternionFromEulerAngles
QuternionFromAxisAndAngle
QuternionFromMatrix
MatrixFromQuternion
AngleBetweenQuaternions
QuternionNormlize
QuternionSLerp
...
QuternionFromAxisAndAngle
QuternionFromMatrix
MatrixFromQuternion
AngleBetweenQuaternions
QuternionNormlize
QuternionSLerp
...
' Quaternion
const EPSILON# = 0.0000001
const DEG2RAD# = M_PI/180.0
const RAD2DEG# = 180.0/M_PI
const PI_HALF# = M_PI/2.0
const X=0,Y=1,Z=2,W=3
Function ACOS#(x#)
If x# = 0.0 Then
return Atn(1.0) * 2.0
ElseIf x# = -1 Then
return Atn(1.0) * 4.0
ElseIf x# = 1.0 Then
return 0.0
Else
return Atn(-x# / Sqr(-x# * x# + 1.0)) + Atn(1.0) * 2.0
End If
End Function
Function ASIN#(x#)
If x# = 0.0 Then
return 0.0
ElseIf x# = -1.0 Then
return 0.0
ElseIf x# = 1.0 Then
return Atn(1.0) * 2.0
Else
return Atn(x# / Sqr(-x# * x# + 1.0))
End If
End Function
function ATAN2#(dx#,dy#)
if (Abs(dx#) < EPSILON#) then
if (dy# <0.0) then
return -M_PI
else
return 0.0
end if
endif
if (Abs(dy#) < EPSILON#) then
if (dx#<0.0) then
return -PI_HALF#
else
return PI_HALF#
endif
endif
dim r# = Atn(dx# / dy#)
if (r#<0) land (dy#<0.0) then
r#=r# +M_PI
elseif (dx#<0) then
r#=r# -M_PI
endif
return r#
End Function
' return a identity Quaternion
function QIdentity#() ()
return vec4(0,0,0,1)
end function
function QNormalize#(p#()) ()
dim Q#(3)=p#
' make Q#(w) always +
if (Q#(w)<0.0) then
Q#=vec4(-Q#(x),-Q#(y),-Q#(z),-Q#(w))
endif
dim t2# = Q#(x)*Q#(x) + Q#(y)*Q#(y) + Q#(z)*Q#(z) + Q#(w)*Q#(w)
if (abs(t2#)<EPSILON#) then
return QIdentity#()
end if
dim t# = 1.0 / sqr(t2#)
return vec4(Q#(x)*t#,Q#(y)*t#,Q#(z)*t#,Q#(w)*t#)
end function
' Q = Qa * Qb
function QMult#(Qa#(),Qb#()) ()
dim Q#(3)
Q#(x) = Qa#(w)*Qb#(x) + Qa#(x)*Qb#(w) + Qa#(y)*Qb#(z) - Qa#(z)*Qb#(y)
Q#(y) = Qa#(w)*Qb#(y) - Qa#(x)*Qb#(z) + Qa#(y)*Qb#(w) + Qa#(z)*Qb#(x)
Q#(z) = Qa#(w)*Qb#(z) + Qa#(x)*Qb#(y) - Qa#(y)*Qb#(x) + Qa#(z)*Qb#(w)
Q#(w) = Qa#(w)*Qb#(w) - Qa#(x)*Qb#(x) - Qa#(y)*Qb#(y) - Qa#(z)*Qb#(z)
return Q#
end function
' angle between two Quaternions
function QAngle#(a#(),b#())
return ACOS#(a#(x)*b#(x) + a#(y)*b#(y) + a#(z)*b#(z) + a#(w)*b#(w) )
end function
' Quaternion from euler angles
function QFromE#(ex#,ey#,ez#) ()
dim Q#(3)
dim cx# = cos(ex#),sx# = sin(ex#)
dim cy# = cos(ey#),sy# = sin(ey#)
dim cz# = cos(ez#),sz# = sin(ez#)
dim m00# = cy#*cz# - sx#*sy#*sz#
dim m11# = cx#*cz#
dim m22# = cx#*cy#
dim t# = 0.25*(1.0 + m00# + m11# + m22#)
if (t# > EPSILON#) then
t# = sqr(t#)
Q#(w) = t#:t#=1.0/(t#*4.0)
dim m01# =-cx#*sz#
dim m02# = cz#*sy# + cy#*sx#*sz#
dim m10# = cz#*sx#*sy# + cy#*sz#
dim m12# =-cy#*cz#*sx# + sy#*sz#
dim m20# =-cx#*sy#
dim m21# = sx#
Q#(x) = (m12# - m21#)*t#
Q#(y) = (m20# - m02#)*t#
Q#(z) = (m01# - m10#)*t#
else
Q#(w) = 0
t# = -0.5*(m11# + m22#)
if (t# > EPSILON#) then
t# = sqr(t#)
Q#(x) = t#:t#=1.0/(t#*2.0)
dim m01# =-cx#*sz#
dim m02# = cz#*sy# + cy#*sx#*sz#
Q#(y) = m01#*t#
Q#(z) = m02#*t#
else
Q#(x) = 0.0
t# = 0.5*(1.0 - m22#)
if (t# > EPSILON#) then
t# = sqr(t#)
Q#(y) = t#:t#=1.0/(t#*2.0)
dim m12# =-cy#*cz#*sx# + sy#*sz#
Q#(z) = m12#*t#
else
Q#(x)=0:Q#(y)=0:Q#(z)=0:Q#(w)=1:
endif
endif
endif
return Q#
end function
' Euler angles from Quaternion
sub EFromQ(Q#(),&ex#, &ey#, &ez#)
dim m00#, m20#
dim m01#,m11#,m21#
dim m02#, m22#
' Q^2
dim qx2#=Q#(x)*Q#(x),qy2#=Q#(y)*Q#(y),qz2#=Q#(z)*Q#(z),qw2#=Q#(w)*Q#(w)
if sqr(qw2# + qx2# + qy2# + qz2#)<EPSILON# then
m00#=1.0:m11#=1.0:m22#=1.0
else
dim qwy#=Q#(w)*Q#(y)
dim qxz#=Q#(x)*Q#(z)
m00# = 1.0-2.0*(qy2# + qz2#)
m01# = 2.0*(Q#(x)*Q#(y) + Q#(w)*Q#(z))
m02# = 2.0*(qxz# - qwy#)
m11# = 1.0-2.0*(qx2# + qz2#)
m20# = 2.0*(qxz# + qwy#)
m21# = 2.0*(Q#(y)*Q#(z) - Q#(w)*Q#(x))
m22# = 1.0-2.0*(qx2# + qy2#)
endif
ex# = m21#
if (abs(ex#) > 1.0) then
if (ex# > 0.0) then
ex#= 1.0
else
ex#=-1.0
endif
endif
ex# = ASIN#(ex#)
if (ex# < PI_HALF#) then
if (ex# > -PI_HALF#) then
ez#=atan2#(-m01#, m11#)
ey#=atan2#(-m20#, m22#)
else
ez#=-atan2#(m02#, m00#)
ey#= 0.0
end if
else
ez#=atan2#(m02#, m00#)
ey#=0.0
end if
end sub
' Q from rotation about axis
function QFromAxisAndAngle#(axis#(),angle#) ()
dim Q#(3)
angle# = angle#*DEG2RAD#*0.5
dim s# = sin(angle#)
axis# = Normalize(axis#)
Q#(x) = axis#(x)*s#
Q#(y) = axis#(y)*s#
Q#(z) = axis#(z)*s#
Q#(w) = cos(angle#)
return Q#
end function
' rotation axis and angle from Quaternion
sub AxisAndAngleFromQ(Q#(),&axis#(),&angle#)
dim s# = 1.0/sqrt(Q#(x)*Q#(x) + Q#(y)*Q#(y) + Q#(z)*Q#(z))
axis# = vec3(Q#(x)*s#,Q#(y)*s#,Q#(z)*s#)
angle# = ACOS#(Q#(w))*RAD2DEG# * 2.0
end sub
' Matrix from Quaternion
function MFromQ#(Q#()) ()()
dim m#(3,3)
Q#=QNormalize#(Q#)
dim x2# = q#(x)*q#(x),y2# = q#(y)*q#(y),z2# = q#(z)*q#(z)
dim xy# = q#(x)*q#(y),wz# = q#(w)*q#(z),xz# = q#(x)*q#(z)
dim wy# = q#(w)*q#(y),yz# = q#(y)*q#(z),wx# = q#(w)*q#(x)
' compose the 4x4 matrix (without translation and scale)
m#(0)=vec4(1.0-2.0*(y2#+z2#), 2.0*(xy#-wz#), 2.0*(xz#+wy#),0)
m#(1)=vec4( 2.0*(xy#+wz#),1.0-2.0*(z2#+x2#), 2.0*(yz#-wx#),0)
m#(2)=vec4( 2.0*(xz#-wy#), 2.0*(yz#+wx#),1.0-2.0*(x2#+y2#),0)
m#(3)=vec4( 0, 0, 0,1)
return m#
end function
' Quaternion from Matrix
function QFromM#(m#()()) ()
dim Q#(3)
dim qw2# = 0.25 * (m#(0,0) + m#(1,1) + m#(2,2) + 1.0)
dim qx2# = qw2# - 0.5*(m#(1,1) + m#(2,2))
dim qy2# = qw2# - 0.5*(m#(2,2) + m#(0,0))
dim qz2# = qw2# - 0.5*(m#(0,0) + m#(1,1))
' maximum magnitude component
dim i
if (qw2#>qx2#) then
if (qw2#>qy2#) then
if (qw2#>qz2#) then
i=0
else
i=3
endif
else
if (qy2#>qz2#) then
i=2
else
i=3
endif
endif
else
if (qx2#>qy2#) then
if (qx2#>qz2#) then
i=1
else
i=3
endif
else
if (qy2#>qz2#) then
i=2
else
i=3
endif
endif
endif
' compute signed quaternion components using numerically stable method
dim t#
if (i=0) then
Q#(w) = sqr(qw2#)
t# = 0.25 / Q#(w)
Q#(x) = (m#(2,1) - m#(1,2))*t#
Q#(y) = (m#(0,2) - m#(2,0))*t#
Q#(z) = (m#(1,0) - m#(0,1))*t#
elseif (1=1) then
Q#(x) = sqr(qx2#)
t# = 0.25 / Q#(x)
Q#(y) = (m#(0,1) + m#(1,0))*t#
Q#(z) = (m#(2,0) + m#(0,2))*t#
Q#(w) = (m#(2,1) - m#(1,2))*t#
elseif (i=2) then
Q#(y) = sqr(qy2#)
t# = 0.25 / Q#(y)
Q#(x) = (m#(0,1) + m#(1,0))*t#
Q#(z) = (m#(1,2) + m#(2,1))*t#
Q#(w) = (m#(0,2) - m#(2,0))*t#
else
Q#(z) = sqr(qz2#)
t# = 0.25 / Q#(z)
Q#(x) = (m#(0,2) + m#(2,0))*t#
Q#(y) = (m#(1,2) + m#(2,1))*t#
Q#(w) = (m#(1,0) - m#(0,1))*t#
endif
' make Q#(w) always +
if (i>0 land Q#(w) < 0.0) then
Q#(w)=-Q#(w):Q#(x)=-Q#(x):Q#(y)=-Q#(y):Q#(z)=-Q#(z)
endif
' normalise it to be safe
t# = 1.0 / sqr(Q#(w)*Q#(w) + Q#(x)*Q#(x) + Q#(y)*Q#(y) + Q#(z)*Q#(z))
return vec4(Q#(x)*t#,Q#(y)*t#,Q#(z)*t#,Q#(w)*t#)
end function
' Quaternion = SLerp(Qa#,Qb#,t#)
function QSLerp#(Qa#(),Qb#(),t#) ()
dim Q#(3)
dim ang#, sina#, a#, b#
dim cosa# = Qa#(x)*Qb#(x) + Qa#(y)*Qb#(y) + Qa#(z)*Qb#(z) + Qa#(w)*Qb#(w)
if ((1.0 + cosa#) > EPSILON#) then
if ((1.0 - cosa#) > EPSILON#) then
ang# =acos#(cosa#)
sina#=sin(ang#)
a#=sin((1.0-t#)*ang#)/sina#
b#=sin( t# *ang#)/sina#
else
' Almost the same:
a#=1.0-t#
b#=t#
endif
return vec4(a#*Qa#(x)+b#*Qb#(x),a#*Qa#(y)+b#*Qb#(y),a#*Qa#(z)+b#*Qb#(z),a#*Qa#(w)+b#*Qb#(w))
endif
Q#(x)=-Qa#(y):Q#(y)=Qa#(x)
Q#(z)=-Qa#(w):Q#(w)=Qa#(z)
a#=sin((1.0-t#)*PI_HALF#):b#=sin(t#*PI_HALF#)
return vec4(a#*Qa#(x)+b#*Q#(x),a#*Qa#(y)+b#*Q#(y),a#*Qa#(z)+b#*Q#(z),a#*Qa#(w)+b#*Q#(w))
end function
function QConjugate#(Q#()) ()
return vec4(-Q#(x), -Q#(y), -Q#(z), Q#(w))
end function
function QRotateV#(Q#(),v3#()) ()
dim n#(2)=Normalize(v3#)
dim r#(3)=QMult#(Q#,QMult#(vec4(n#(x),n#(y),n#(z),0.0),vec4(-Q#(x),-Q#(y),-Q#(z),Q#(w))))
return Vec3(r#(x),r#(y),r#(z))
end function
function QMirroredX#(Q#()) ()
return vec4(Q#(x), -Q#(y), -Q#(z),Q#(w))
end function
function QMirroredY#(Q#()) ()
return vec4(-Q#(x), Q#(y), -Q#(z),Q#(w))
end function
function QMirroredZ#(Q#()) ()
return vec4(-Q#(x), -Q#(y), Q#(z),Q#(w))
end function