SHOW:
|
|
- or go back to the newest paste.
| 1 | //XYZ is a 3D vector class with components X, Y, and Z | |
| 2 | //Obvious thing but worth mentioning: if converting this to | |
| 3 | //use floats, make sure you convert all literal doubles | |
| 4 | //to end in f (1.0 becomes 1.0f, 0.5 becomes 0.5f, etc) | |
| 5 | ||
| 6 | //based on http://answers.unity3d.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html | |
| 7 | public static Quaternion LookRotation(XYZ forward, XYZ up) | |
| 8 | {
| |
| 9 | forward = forward.Normalize(); | |
| 10 | ||
| 11 | XYZ vector = forward.Normalize(); | |
| 12 | XYZ vector2 = up.CrossProduct(vector).Normalize(); | |
| 13 | XYZ vector3 = vector.CrossProduct(vector2); | |
| 14 | var m00 = vector2.X; | |
| 15 | var m01 = vector2.Y; | |
| 16 | var m02 = vector2.Z; | |
| 17 | var m10 = vector3.X; | |
| 18 | var m11 = vector3.Y; | |
| 19 | var m12 = vector3.Z; | |
| 20 | var m20 = vector.X; | |
| 21 | var m21 = vector.Y; | |
| 22 | var m22 = vector.Z; | |
| 23 | ||
| 24 | double num8 = (m00 + m11) + m22; | |
| 25 | var quaternion = new Quaternion(); | |
| 26 | if (num8 > 0.0) | |
| 27 | {
| |
| 28 | var num = (double)Math.Sqrt(num8 + 1.0); | |
| 29 | quaternion.w = num * 0.5; | |
| 30 | num = 0.5 / num; | |
| 31 | quaternion.x = (m12 - m21) * num; | |
| 32 | quaternion.y = (m20 - m02) * num; | |
| 33 | quaternion.z = (m01 - m10) * num; | |
| 34 | return quaternion; | |
| 35 | } | |
| 36 | if ((m00 >= m11) && (m00 >= m22)) | |
| 37 | {
| |
| 38 | var num7 = (double)Math.Sqrt(((1.0 + m00) - m11) - m22); | |
| 39 | var num4 = 0.5 / num7; | |
| 40 | quaternion.x = 0.5 * num7; | |
| 41 | quaternion.y = (m01 + m10) * num4; | |
| 42 | quaternion.z = (m02 + m20) * num4; | |
| 43 | quaternion.w = (m12 - m21) * num4; | |
| 44 | return quaternion; | |
| 45 | } | |
| 46 | if (m11 > m22) | |
| 47 | {
| |
| 48 | var num6 = (double)Math.Sqrt(((1.0 + m11) - m00) - m22); | |
| 49 | var num3 = 0.5 / num6; | |
| 50 | quaternion.x = (m10 + m01) * num3; | |
| 51 | quaternion.y = 0.5 * num6; | |
| 52 | quaternion.z = (m21 + m12) * num3; | |
| 53 | quaternion.w = (m20 - m02) * num3; | |
| 54 | return quaternion; | |
| 55 | } | |
| 56 | var num5 = (double)Math.Sqrt(((1.0 + m22) - m00) - m11); | |
| 57 | var num2 = 0.5 / num5; | |
| 58 | quaternion.x = (m20 + m02) * num2; | |
| 59 | quaternion.y = (m21 + m12) * num2; | |
| 60 | quaternion.z = 0.5 * num5; | |
| 61 | quaternion.w = (m01 - m10) * num2; | |
| 62 | return quaternion; | |
| 63 | } |