Sorry your browser is not supported!

type Quaternion
    w as float
    x as float
    y as float
    z as float
endtype

type Euler
    x as float
    y as float
    z as float
endtype

global ReturnQuat as Quaternion
global ReturnEuler as Euler

function PointLimb(obj, limb, px#, py#, pz#)
   if object exist(obj)=0 then exitfunction
   if limb exist(obj,limb)=0 then exitfunction

   t_ax#=object angle x(obj) : t_ay#=object angle y(obj) : t_az#=object angle z(obj) : point object obj,px#,py#,pz#
   ax#=object angle x(obj)   : ay#=object angle y(obj)   : az#=object angle z(obj)
   rotate object obj,t_ax#,t_ay#,t_az#
   Limb_SetDirection(obj,limb,ax#,ay#,az#)
endfunction

function Limb_SetDirection(obj, limb, ax#, ay#, az#)
   if object exist(obj)=0 then exitfunction
   if limb exist(obj,limb)=0 then exitfunction
   if ax#=0 then inc ax#,0.0000001
   if ay#=0 then inc ay#,0.0000001
   if az#=0 then inc az#,0.0000001

   e as euler
   q1 as Quaternion
   q2 as Quaternion
   q3 as Quaternion
   e.x=object angle x(obj)
   e.y=object angle y(obj)
   e.z=object angle z(obj)
   EulerToQuat(e)    : q1=ReturnQuat
   QuatConjugate(q1) : q1=ReturnQuat

   e.x=ax#           : e.y=ay# : e.z=az#
   EulerToQuat(e)    : q2=ReturnQuat

   QuatMult(q2,q1)   : q3=ReturnQuat
   QuatToEuler(q3)
   rotate limb obj,limb,ReturnEuler.x,ReturnEuler.y,ReturnEuler.z
endfunction

function QuatConjugate(q as quaternion)
   ReturnQuat.w=q.w
   ReturnQuat.x=q.x*-1
   ReturnQuat.y=q.y*-1
   ReturnQuat.z=q.z*-1
endfunction

function EulerToQuat(Rotation as Euler)
    SRX as float
    CRX as float
    SRY as float
    CRY as float
    SRZ as float
    CRZ as float
    Rotation.x = Rotation.x/2
    Rotation.y = Rotation.y/2
    Rotation.z = Rotation.z/2
    SRX = sin(Rotation.x)
    CRX = cos(Rotation.x)
    SRY = sin(Rotation.y)
    CRY = cos(Rotation.y)
    SRZ = sin(Rotation.z)
    CRZ = cos(Rotation.z)
    ReturnQuat.w = CRX*CRY*CRZ +  SRX*SRY*SRZ
    ReturnQuat.x = SRX*CRY*CRZ -  CRX*SRY*SRZ
    ReturnQuat.y = CRX*SRY*CRZ +  SRX*CRY*SRZ
    ReturnQuat.z = CRX*CRY*SRZ -  SRX*SRY*CRZ
endfunction

function QuatToEuler(Rotation as Quaternion)
   remstart
    test as float
   test = Rotation.w*Rotation.y - Rotation.z*Rotation.x
   if test > 0.499
      ReturnEuler.z = 2 * atanfull(Rotation.w,Rotation.x)
      ReturnEuler.y = 90
      ReturnEuler.x = 0
      exitfunction
   endif
   if test < -0.499
      ReturnEuler.z = -2 * atanfull(Rotation.w,Rotation.x)
      ReturnEuler.y = -90
      ReturnEuler.x = 0
      exitfunction
   endif
    remend
    ReturnEuler.x = atanfull((2*(Rotation.w*Rotation.x + Rotation.y*Rotation.z)),(1 - 2*(Rotation.x*Rotation.x + Rotation.y*Rotation.y)))
    ReturnEuler.y = asin(2*(Rotation.w*Rotation.y - Rotation.z*Rotation.x))
    ReturnEuler.z = atanfull((2*(Rotation.w*Rotation.z + Rotation.x*Rotation.y)),(1 - 2*(Rotation.y*Rotation.y + Rotation.z*Rotation.z)))
endfunction

function QuatMult(Angle as Quaternion, Rotation as Quaternion)
    ReturnQuat.x =  Rotation.x * Angle.w + Rotation.y * Angle.z - Rotation.z * Angle.y + Rotation.w * Angle.x
    ReturnQuat.y = (0-Rotation.x) * Angle.z + Rotation.y * Angle.w + Rotation.z * Angle.x + Rotation.w * Angle.y
    ReturnQuat.z =  Rotation.x * Angle.y - Rotation.y * Angle.x + Rotation.z * Angle.w + Rotation.w * Angle.z
    ReturnQuat.w = (0-Rotation.x) * Angle.x - Rotation.y * Angle.y - Rotation.z * Angle.z + Rotation.w * Angle.w
endfunction