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. You should probably strip out all of the Debug.Trace() statements as well.
The relative-position code was adapted from code originally written by Chesko.
Other editors: feel free to add this to whatever Papyrus categories you feel are relevant. :)
Changelog
- 8/26/2014 7
- 30pm EST
- I appear to have successfully fixed issues that arose when converting rotation matrices to axis angles, when the resulting axis-angle had an angle of exactly 180.
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 angle. ; Float fTrace = MatrixTrace(afMatrix) fOutput[3] = Math.acos((fTrace - 1) / 2) ; ; Determine the axis. ; fOutput[0] = afMatrix[7] - afMatrix[5] fOutput[1] = afMatrix[2] - afMatrix[6] fOutput[2] = afMatrix[3] - afMatrix[1] If fOutput[3] == 180 ; ; A 180-degree angle tends to lead to a zero vector as our axis. ; There seems to be a way to correct that... ; ; Source for the math: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm ; Source for the math: http://sourceforge.net/p/mjbworld/discussion/122133/thread/912b44f7 ; fOutput[0] = Math.sqrt((afMatrix[0] + 1) / 2) fOutput[1] = Math.sqrt((afMatrix[4] + 1) / 2) fOutput[2] = Math.sqrt((afMatrix[8] + 1) / 2) ; ; We don't know the signs of the above terms. Per our second ; source, we can start to figure that out by finding the largest ; term... ; Int iLargestIndex = 0 Float fTemporary = fOutput[0] If fTemporary < fOutput[1] fTemporary = fOutput[1] iLargestIndex = 1 EndIf If fTemporary < fOutput[2] fTemporary = fOutput[2] iLargestIndex = 2 EndIf Int iIterator = 0 While iIterator < 3 Int iIndex = iLargestIndex * 3 + iIterator If iIterator != iLargestIndex fOutput[iIterator] = fOutput[iIterator] * Sign(afMatrix[iIndex]) EndIf iIterator += 1 EndWhile EndIf ; ; Normalize the axis. ; If VectorLength(fOutput) != 0 OverwriteFloatArrayWith(fOutput, VectorNormalize(fOutput), 0) Else ; ; Edge-case caused a zero vector! Dumb fallback to the Z-axis. ; fOutput[0] = 0 fOutput[1] = 0 fOutput[2] = 1 EndIf ; ; 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.