User:DavidJCobb/Rotation Library

I've created a rotation library that makes it easier to work with rotation math in Skyrim. Among other things, it can position and rotate one object relative to another for you. This tutorial takes you through creating a basic demo of the library, and assumes that you've compiled the (interdependent) scripts below.

Please use different script names if you choose to use these libraries, so as to avoid collisions with any edits or enhancements that other users may make in other mods.

The relative-position code was adapted from code originally written by Chesko.

Main code

Scriptname CobbLibraryRotations
{A library for working with rotations in Papyrus.

Skyrim uses extrinsic left-handed (clockwise) ZYX Euler rotations.}

Import CobbLibraryMisc
Import CobbLibraryVectors

Float Function MatrixTrace(Float[] afMatrix) Global
{Returns the trace of a 3x3 rotation matrix.}
   return afMatrix[0] + afMatrix[4] + afMatrix[8]
EndFunction

Float[] Function EulerToAxisAngle(float afX, float afY, float afZ) Global
{Converts a set of Euler angles to axis angle, returning [x, y, z, angle]. The angle is in degrees. Tailored for Skyrim (extrinsic left-handed ZYX Euler).}
   ;
   ; Source for the math: http://www.vectoralgebra.info/axisangle.html
   ; Source for the math: http://www.vectoralgebra.info/euleranglesvector.html
   ;
   Float[] fOutput = new Float[4]
   Float[] fMatrix = EulerToMatrix(afX, afY, afZ)
   return MatrixToAxisAngle(fMatrix)
EndFunction

Float[] Function EulerToMatrix(float afX, float afY, float afZ) Global
{Converts a set of Euler angles to a rotation matrix. Tailored for Skyrim (extrinsic left-handed ZYX Euler).

Matrix indices are:
0 1 2
3 4 5
6 7 8}
   ;
   ; Source for the math: http://www.vectoralgebra.info/eulermatrix.html
   ;
   Float[] fOutput = new Float[9]
   Float fSinX = Math.sin(afX)
   Float fSinY = Math.sin(afY)
   Float fSinZ = Math.sin(afZ)
   Float fCosX = Math.cos(afX)
   Float fCosY = Math.cos(afY)
   Float fCosZ = Math.cos(afZ)
   ;
   ; Build the matrix.
   ;
   fOutput[0] = fCosY * fCosZ				; 1,1
   fOutput[1] = fCosY * fSinZ				; 1,2
   fOutput[2] = -fSinY					; 1,3
   fOutput[3] = fSinX * fSinY * fCosZ - fCosX * fSinZ	; 2,1
   fOutput[4] = fSinX * fSinY * fSinZ + fCosX * fCosZ	; 2,2
   fOutput[5] = fSinX * fCosY				; 2,3
   fOutput[6] = fCosX * fSinY * fCosZ + fSinX * fSinZ	; 3,1
   fOutput[7] = fCosX * fSinY * fSinZ - fSinX * fCosZ	; 3,2
   fOutput[8] = fCosX * fCosY				; 3,3
   ;
   ; Done!
   ;
   return fOutput
EndFunction

Float[] Function EulerToQuaternion(float afX, float afY, float afZ) Global
{Converts a set of Euler angles to a quaternion (represented as [w, x, y, z]). Tailored for Skyrim (extrinsic left-handed ZYX Euler).}
   return AxisAngleToQuaternion(EulerToAxisAngle(afX, afY, afZ))
EndFunction

Float[] Function MatrixToAxisAngle(Float[] afMatrix) Global
{Converts a rotation matrix to axis angle, returning [x, y, z, angle]. The angle is in degrees. Tailored for Skyrim (extrinsic left-handed ZYX Euler).}
   Float[] fOutput = new Float[4]
   ;
   ; Determine the axis.
   ;
   fOutput[0] = afMatrix[7] - afMatrix[5]
   fOutput[1] = afMatrix[2] - afMatrix[6]
   fOutput[2] = afMatrix[3] - afMatrix[1]
   ;
   ; Normalize the axis.
   ;
   If VectorLength(fOutput) != 0
      OverwriteFloatArrayWith(fOutput, VectorNormalize(fOutput), 0)
   Else
      ;
      ; Edge-case caused a zero vector! Try using the Z-axis instead.
      ;
      ; (This is known to happen when dealing with rotation matrices that 
      ; describe the Euler rotations (0, 0, 0) or (0, 0, 180). In those 
      ; cases, the angle computed below is 0 and 180 respectively, so...)
      ;
      fOutput[0] = 0
      fOutput[1] = 0
      fOutput[2] = 1
   EndIf
   ;
   ; Determine the angle.
   ;
   Float fTrace = MatrixTrace(afMatrix)
   fOutput[3] = Math.acos((fTrace - 1) / 2)
   ;
   ; Done!
   ;
   return fOutput
EndFunction

Float[] Function MatrixToEuler(Float[] afMatrix) Global
{Converts a rotation matrix to Euler angles. Tailored for Skyrim (extrinsic left-handed ZYX Euler).}
   ;
   ; Source for the math: https://web.archive.org/web/20051124013711/http://skal.planet-d.net/demo/matrixfaq.htm#Q37
   ;
   ; The math there is righthanded, but it's easy to tailor it to 
   ; lefthanded if you have a handy-dandy reference like the one 
   ; at <http://www.vectoralgebra.info/eulermatrix.html>.
   ;
   Float[] fEuler = new Float[3]
   ; 
   ; We can immediately solve for Y, but we must round it to account 
   ; for imprecision that is sometimes introduced when we have 
   ; converted through other forms (e.g. axis-angle). fCYTest exists 
   ; solely as part of that accounting.
   ; 
   Float fY = Math.asin( (((-afMatrix[2] * 1000000) as int) as float) / 1000000 )
   Float fCY = Math.cos(fY)
   Float fCYTest = (((fCY * 100) as int) as float) / 100
   Float fTX
   Float fTY
   If fCY && fCY >= 0.00000011920929 && fCYTest
      Debug.Trace("MatrixToEuler: Y == " + fY + "; cos(Y) == " + fCY)
      fTX = afMatrix[8] / fCY
      fTY = afMatrix[5] / fCY
      fEuler[0] = atan2(fTY, fTX)   ; = atan(sinXcosY / cosXcosY) = atan(sin X / cos X)
      fTX = afMatrix[0] / fCY
      fTY = afMatrix[1] / fCY
      fEuler[2] = atan2(fTY, fTX)   ; = atan(cosYcosZ / cosYsinZ) = atan(sin Z / cos Z)
   Else
      Debug.Trace("MatrixToEuler: cos(Y) == 0. Taking another path...")
      ;
      ; We can't compute X and Z by using Y, because cos(Y) is zero. Therefore, 
      ; we have to compromise.
      ;
      ; We'll assume X to be zero, and dump the rest into Z.
      ;
      fEuler[0] = 0
      fTX = afMatrix[4]             ; Setting X to zero simplifies this element to: 0*sinY*sinZ + 1*cosZ
      fTY = afMatrix[3]             ; Setting X to zero simplifies this element to: 0*sinY*cosZ - 1*sinZ
      ;
      ; NOTE: Negating the result APPEARS to be necessary to account for our use of a 
      ; left-handed system versus the source's use of a right-handed system. However, 
      ; I arrived at that conclusion deductively, and I am not 100% certain of it.
      ;
      fEuler[2] = -atan2(fTY, fTX)   ; = atan(sin Z / cos Z)
   EndIf
   fEuler[1] = fY
   Return fEuler
EndFunction

Float[] Function MatrixToQuaternion(Float[] afMatrix) Global
   return AxisAngleToQuaternion(MatrixToAxisAngle(afMatrix)) ; TODO: Find a more direct method, if possible.
EndFunction

Float[] Function AxisAngleToEuler(Float[] afAxisAngle) Global
{Converts an axis-angle orientation to Euler angles in degrees. Tailored for Skyrim (extrinsic left-handed ZYX Euler).}
   return MatrixToEuler(AxisAngleToMatrix(afAxisAngle)) ; TODO: Find a more direct method, if possible.
EndFunction

Float[] Function AxisAngleToMatrix(Float[] afAxisAngle) Global
{Converts an axis-angle orientation to a rotation matrix. Tailored for Skyrim (extrinsic left-handed ZYX Euler).}
   ;
   ; Based on the math at: https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
   ;
   Float[] fMatrix = new Float[9]
   Float fOneMinusCos = (1 - Math.cos(afAxisAngle[3]))
   fMatrix[0] = Math.cos(afAxisAngle[3]) + Math.pow(afAxisAngle[0], 2) * fOneMinusCos
   fMatrix[1] = afAxisAngle[0] * afAxisAngle[1] * fOneMinusCos - afAxisAngle[2] * Math.sin(afAxisAngle[3])
   fMatrix[2] = afAxisAngle[0] * afAxisAngle[2] * fOneMinusCos + afAxisAngle[1] * Math.sin(afAxisAngle[3])
   fMatrix[3] = afAxisAngle[1] * afAxisAngle[0] * fOneMinusCos + afAxisAngle[2] * Math.sin(afAxisAngle[3])
   fMatrix[4] = Math.cos(afAxisAngle[3]) + Math.pow(afAxisAngle[1], 2) * fOneMinusCos
   fMatrix[5] = afAxisAngle[1] * afAxisAngle[2] * fOneMinusCos - afAxisAngle[0] * Math.sin(afAxisAngle[3])
   fMatrix[6] = afAxisAngle[2] * afAxisAngle[0] * fOneMinusCos - afAxisAngle[1] * Math.sin(afAxisAngle[3])
   fMatrix[7] = afAxisAngle[2] * afAxisAngle[1] * fOneMinusCos + afAxisAngle[0] * Math.sin(afAxisAngle[3])
   fMatrix[8] = Math.cos(afAxisAngle[3]) + Math.pow(afAxisAngle[2], 2) * fOneMinusCos
   return fMatrix
EndFunction

Float[] Function AxisAngleToQuaternion(Float[] afAxisAngle) Global
{Converts an axis-angle orientation to a unit quaternion (versor), returning [w, x, y, z].}
   ;
   ; Source for the math: https://en.wikipedia.org/w/index.php?title=Axis%E2%80%93angle_representation&oldid=608157500#Unit_quaternions
   ;
   Float[] qOutput = new Float[4]
   Float fHalfAngle = afAxisAngle[3] / 2
   qOutput[0] = Math.cos(fHalfAngle) 			; w
   qOutput[1] = Math.sin(fHalfAngle) * afAxisAngle[0] 	; x
   qOutput[2] = Math.sin(fHalfAngle) * afAxisAngle[1] 	; y
   qOutput[3] = Math.sin(fHalfAngle) * afAxisAngle[2] 	; z
   return qOutput
EndFunction

Float[] Function QuaternionToAxisAngle(Float[] aqQuat) Global
{Converts a unit quaternion (versor) to an axis-angle representation, returning [x, y, z, angle].}
   return MatrixToAxisAngle(QuaternionToMatrix(aqQuat))
EndFunction

Float[] Function QuaternionToEuler(Float[] aqQuat) Global
   return MatrixToEuler(QuaternionToMatrix(aqQuat))
EndFunction

Float[] Function QuaternionToMatrix(Float[] aqQuat) Global
{Converts a quaternion (as [w, x, y, z]) to a rotation matrix.

NOTE: I have not tested to see whether using a unit quaternion or a non-normalized quaternion makes any difference.}
   ;
   ; Source for the math: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
   ;
   int W = 0
   int X = 1
   int Y = 2
   int Z = 3
   Float[] mOutput = new Float[9]
   mOutput[0] = 1 - 2*Math.pow(aqQuat[Y],2) - 2*Math.pow(aqQuat[Z],2)
   mOutput[1] = 2*aqQuat[X]*aqQuat[Y] - 2*aqQuat[Z]*aqQuat[W]
   mOutput[2] = 2*aqQuat[X]*aqQuat[Z] + 2*aqQuat[Y]*aqQuat[W]
   mOutput[3] = 2*aqQuat[X]*aqQuat[Y] + 2*aqQuat[Z]*aqQuat[W]
   mOutput[4] = 1 - 2*Math.pow(aqQuat[X],2) - 2*Math.pow(aqQuat[Z],2)
   mOutput[5] = 2*aqQuat[Y]*aqQuat[Z] - 2*aqQuat[X]*aqQuat[W]
   mOutput[6] = 2*aqQuat[X]*aqQuat[Z] - 2*aqQuat[Y]*aqQuat[W]
   mOutput[7] = 2*aqQuat[Y]*aqQuat[Z] + 2*aqQuat[X]*aqQuat[W]
   mOutput[8] = 1 - 2*Math.pow(aqQuat[X],2) - 2*Math.pow(aqQuat[Y],2)
   return mOutput
EndFunction

Float[] Function QuaternionAdd(Float[] aqA, Float[] aqB) Global
{Adds two quaternions, returning the result as a new quaternion.}
   Float[] qOut = new Float[4]
   qOut[0] = aqA[0] + aqB[0]
   qOut[1] = aqA[1] + aqB[1]
   qOut[2] = aqA[2] + aqB[2]
   qOut[3] = aqA[3] + aqB[3]
   return qOut
EndFunction

Float[] Function QuaternionMultiply(Float[] aqA, Float[] aqB) Global
{Returns as a new quaternion the Hamilton product of two quaternions (of the form [w, x, y, z]).}
   ;
   ; Source for the math: https://en.wikipedia.org/w/index.php?title=Quaternion&oldid=618007927#Hamilton_product
   ;
   Float[] qOut = new Float[4]
   qOut[0] = aqA[0]*aqB[0] - aqA[1]*aqB[1] - aqA[2]*aqB[2] - aqA[3]*aqB[3]
   qOut[1] = aqA[0]*aqB[1] + aqA[1]*aqB[0] + aqA[2]*aqB[3] - aqA[3]*aqB[2]
   qOut[2] = aqA[0]*aqB[2] - aqA[1]*aqB[3] + aqA[2]*aqB[0] + aqA[3]*aqB[1]
   qOut[3] = aqA[0]*aqB[3] + aqA[1]*aqB[2] - aqA[2]*aqB[1] + aqA[3]*aqB[0]
   return qOut
EndFunction

Float[] Function QuaternionConjugate(Float[] aq) Global
{UNTESTED. Returns as a new quaternion the conjugate of the given quaternion (of the form [w, x, y, z]).}
   Float[] qOut = new Float[4]
   Float[] v = OverwriteFloatArrayWith(new Float[3], aq, 1)
   OverwriteFloatArrayWith(qOut, VectorNegate(v))
   qOut[0] = aq[0]
   return qOut
EndFunction

Function MoveObjectRelativeToObject(ObjectReference akChild, ObjectReference akParent, Float[] afPositionOffset, Float[] afRotationOffset) Global
{Positions and rotates one object relative to another. Position code is based on GetPosXYZRotateAroundRef, a function authored by Chesko that can be found on the Creation Kit wiki.}
   If !afPositionOffset || !afRotationOffset || afPositionOffset.length < 3 || afRotationOffset.length < 3
      return
   EndIf
   ;
   ; CONSTRUCT POSITION USING CHESKO'S METHOD.
   ;
   Float[] Angles = new Float[3]
   Float[] Origin = new Float[3]
   Float[] Target = new Float[3]
   Float[] Output = new Float[3]

   Angles[0] = -akParent.GetAngleX()
   Angles[1] = -akParent.GetAngleY()
   Angles[2] = -akParent.GetAngleZ()

   Origin[0] = akParent.GetPositionX()
   Origin[1] = akParent.GetPositionY()
   Origin[2] = akParent.GetPositionZ()
   ;
   ; Grab the parent-relative coordinates that we want to convert to 
   ; world-relative.
   ;
   Target[0] = afPositionOffset[0]
   Target[1] = afPositionOffset[1]
   Target[2] = afPositionOffset[2]
   Float[] Vector = new Float[3]
   Vector[0] = Target[0]
   Vector[1] = Target[1]
   Vector[2] = Target[2]
   Output[0] = (Vector[0] * Math.cos(Angles[2])) + (Vector[1] * Math.sin(-Angles[2])) + (Vector[2] * 0)
   Output[1] = (Vector[0] * Math.sin(Angles[2])) + (Vector[1] * Math.cos( Angles[2])) + (Vector[2] * 0)
   Output[2] = (Vector[0] * 0) + (Vector[1] * 0) + (Vector[2] * 1)
   ;
   ; From the original: Y-axis rotation matrix.
   ;
   Vector[0] = Output[0]
   Vector[1] = Output[1]
   Vector[2] = Output[2]
   Output[0] = (Vector[0] * Math.cos( Angles[1])) + (Vector[1] * 0) + (Vector[2] * Math.sin(Angles[1]))
   Output[1] = (Vector[0] * 0) + (Vector[1] * 1) + (Vector[2] * 0)
   Output[2] = (Vector[0] * Math.sin(-Angles[1])) + (Vector[1] * 0) + (Vector[2] * Math.cos(Angles[1]))
   ;
   ; From the original: X-axis rotation matrix.
   ;
   Vector[0] = Output[0]
   Vector[1] = Output[1]
   Vector[2] = Output[2]
   Output[0] = (Vector[0] * 1) + (Vector[1] * 0) + (Vector[2] * 0)
   Output[1] = (Vector[0] * 0) + (Vector[1] * Math.cos(Angles[0])) + (Vector[2] * Math.sin(-Angles[0]))
   Output[2] = (Vector[0] * 0) + (Vector[1] * Math.sin(Angles[0])) + (Vector[2] * Math.cos( Angles[0]))
   ;
   ; Finalize coordinates.
   ;
   Output[0] = Output[0] + Origin[0]
   Output[1] = Output[1] + Origin[1]
   Output[2] = Output[2] + Origin[2]
   ;
   ; CONSTRUCT ROTATION USING THIS LIBRARY.
   ;
   Float[] qShelf = EulerToQuaternion(akParent.GetAngleX(), akParent.GetAngleY(), akParent.GetAngleZ())
   Float[] eBook = new Float[3]
   eBook[0] = afRotationOffset[0]
   eBook[1] = afRotationOffset[1]
   eBook[2] = afRotationOffset[2]
   Float[] qBook = eBook
   qBook = EulerToQuaternion(qBook[0], qBook[1], qBook[2])
   Float[] qDone = QuaternionMultiply(qShelf, qBook)
   Float[] eDone = QuaternionToEuler(qDone)
   ;
   ; Spawn and return marker.
   ;
   akChild.SetPosition(Output[0], Output[1], Output[2])
   akChild.SetAngle(eDone[0], eDone[1], eDone[2])
EndFunction

Dependencies

Misc library

Scriptname CobbLibraryMisc
{Library for miscellaneous resource functions, including generic math stuff.}

Float Property FLT_EPSILON = 0.0000001192092896 AutoReadOnly

Float[] Function OverwriteFloatArrayWith(Float[] target, Float[] source, int offset = 0) Global
{Overwrites the elements of one array with the elements of another, starting at the given offset (in the array to be overwritten).}
   int iterator = 0
   While iterator < source.length
      If iterator + offset < target.length
         target[iterator + offset] = source[iterator]
      EndIf
      iterator += 1
   EndWhile
   return target
EndFunction
Int Function Sign(float a) Global
{Returns, as an integer, the sign of a float: -1, 0, or 1.}
   If a < 0
      Return -1
   ElseIf a > 0
      Return 1
   Else
      Return 0
   EndIf
EndFunction
Float Function atan2(float y, float x) Global
   Float out = 0
   If y != 0
      out = Math.sqrt(x * x + y * y) - x
      out /= y
      out = Math.atan(out) * 2
   Else
      If x == 0
         return 0
      EndIf
      out = Math.atan(y / x)
      If x < 0
         out += 180
      EndIf
   EndIf
   return out
EndFunction

Vector library

Scriptname CobbLibraryVectors
{Library for working with 3D vectors.}

Float[] Function VectorAdd(Float[] avA, Float[] avB) Global
{Adds two vectors together and returns the sum as a new vector.}
   Float[] vOut = new Float[3]
   vOut[0] = avA[0] + avB[0]
   vOut[1] = avA[1] + avB[1]
   vOut[2] = avA[2] + avB[2]
   return vOut
EndFunction

Float[] Function VectorSubtract(Float[] avA, Float[] avB) Global
{Subtracts one vector from another and returns the difference as a new vector.}
   Float[] vOut = new Float[3]
   vOut[0] = avA[0] - avB[0]
   vOut[1] = avA[1] - avB[1]
   vOut[2] = avA[2] - avB[2]
   return vOut
EndFunction

Float[] Function VectorMultiply(Float[] avA, Float afB) Global
{Multiplies a vector by a scalar and returns the result as a new vector.}
   Float[] vOut = new Float[3]
   vOut[0] = avA[0] * afB
   vOut[1] = avA[1] * afB
   vOut[2] = avA[2] * afB
   return vOut
EndFunction

Float[] Function VectorDivide(Float[] avA, Float afB) Global
{Divides a vector by a scalar and returns the result as a new vector.}
   Float[] vOut = new Float[3]
   If afB == 0
      Debug.Trace("VectorDivide: A script asked me to divide a vector by zero. I just returned a null vector instead.")
      return vOut
   EndIf
   vOut[0] = avA[0] / afB
   vOut[1] = avA[1] / afB
   vOut[2] = avA[2] / afB
   return vOut
EndFunction

Float[] Function VectorProject(Float[] avA, Float[] avB) Global
{Projects one vector onto another, returning the result as a new vector.}
   Float[] vOut = new Float[3]
   vOut[0] = avB[0]
   vOut[1] = avB[1]
   vOut[2] = avB[2]
   Float scalar = VectorDotProduct(avA, avB) / VectorDotProduct(avB, avB)
   return VectorMultiply(vOut, scalar)
EndFunction

Float[] Function VectorCrossProduct(Float[] avA, Float[] avB) Global
{Takes the cross product of two vectors and returns the result as a new vector.}
   Float[] vOut = new Float[3]
   vOut[0] = avA[1] * avB[2] - avA[2] * avB[1]
   vOut[1] = avA[2] * avB[0] - avA[0] * avB[2]
   vOut[2] = avA[0] * avB[1] - avA[1] * avB[0]
   return vOut
EndFunction

Float Function VectorDotProduct(Float[] avA, Float[] avB) Global
{Returns the dot product of two vectors.}
   Float fOut = 0
   fOut += avA[0] * avB[0]
   fOut += avA[1] * avB[1]
   fOut += avA[2] * avB[2]
   return fOut
EndFunction

Float[] Function VectorNegate(Float[] av) Global
{Multiplies a vector by -1 and returns the result as a new vector.}
   Return VectorMultiply(av, -1)
EndFunction

Float Function VectorLength(Float[] av) Global
{Returns the length of a vector.}
   return Math.sqrt(av[0]*av[0] + av[1]*av[1] + av[2]*av[2])
EndFunction

Float[] Function VectorNormalize(Float[] av) Global
{Normalizes a vector and returns the result as a new vector.}
   Return VectorDivide(av, VectorLength(av))
EndFunction

Sources for mathematical formulae

This page generates forumlae to convert from Euler angles (in any convention) to rotation matrices.

This page and this page together offer the information needed to convert from Euler angles (in any convention) to an axis-angle representation by way of rotation matrices. The axis you get won't be normalized.

This page describes how to pull right-handed Euler ZYX from a rotation matrix. It's easy to convert the math to left-handed if you use the matrix formula generator linked earlier and if you understand what atan2 does and why.

The process of converting from axis angle to quaternion (and vice versa) is one of the few rotation-related operations that Wikipedia explains in plain English. Ten bucks says a pack of PhDs will eventually come along and rewrite the article into gibberish, so that's a link to an archived version of the article as it existed when I found it.

In another rare instance of clarity, Wikipedia describes how to convert from axis-angle back to a rotation matrix. The article doesn't state whether it's working with extrinsic rotations, or what the handedness of the system is, but it seems to line up with the math at one of the previously-linked pages.