// Quaternion extensions for Unity by Vegard Myklebust. | |
// Made available under Creative Commons license CC0. License details can be found here: | |
// https://creativecommons.org/publicdomain/zero/1.0/legalcode.txt | |
[Extension] | |
public def Log(ref a as Quaternion): | |
a0 as single = a.w | |
a.w = 0.0 | |
if(Mathf.Abs(a0) < 1.0): | |
angle as single = Mathf.Acos(a0) | |
sinAngle as single = Mathf.Sin(angle) | |
if(Mathf.Abs(sinAngle) >= 1.0e-15): | |
coeff as single = angle/sinAngle | |
a.x *= coeff | |
a.y *= coeff | |
a.z *= coeff | |
[Extension] | |
public def Loged(a as Quaternion): | |
result as Quaternion = a | |
a0 as single = result.w | |
result.w = 0.0 | |
if(Mathf.Abs(a0) < 1.0): | |
angle as single = Mathf.Acos(a0) | |
sinAngle as single = Mathf.Sin(angle) | |
if(Mathf.Abs(sinAngle) >= 1.0e-15): | |
coeff as single = angle/sinAngle | |
result.x *= coeff | |
result.y *= coeff | |
result.z *= coeff | |
return result | |
[Extension] | |
public def Conjugate(ref a as Quaternion): | |
a.x *= -1 | |
a.y *= -1 | |
a.z *= -1 | |
[Extension] | |
public def Conjugated(a as Quaternion): | |
result as Quaternion = a | |
result.x *= -1 | |
result.y *= -1 | |
result.z *= -1 | |
return result | |
[Extension] | |
public def Scale(ref a as Quaternion, s as single): | |
a.w *= s | |
a.x *= s | |
a.y *= s | |
a.z *= s | |
[Extension] | |
public def Scaled(a as Quaternion, s as single): | |
result as Quaternion = a | |
result.w *= s | |
result.x *= s | |
result.y *= s | |
result.z *= s | |
return result | |
[Extension] | |
public def Exp(ref a as Quaternion): | |
angle as single = Mathf.Sqrt(a.x*a.x + a.y*a.y + a.z*a.z) | |
sinAngle as single = Mathf.Sin(angle) | |
a.w = Mathf.Cos(angle) | |
if(Mathf.Abs(sinAngle) >= 1.0e-15): | |
coeff as single = sinAngle/angle | |
a.x *= coeff | |
a.y *= coeff | |
a.z *= coeff | |
[Extension] | |
public def Exped(a as Quaternion): | |
result as Quaternion = a | |
angle as single = Mathf.Sqrt(result.x*result.x + result.y*result.y + result.z*result.z) | |
sinAngle as single = Mathf.Sin(angle) | |
result.w = Mathf.Cos(angle) | |
if(Mathf.Abs(sinAngle) >= 1.0e-15): | |
coeff as single = sinAngle/angle | |
result.x *= coeff | |
result.y *= coeff | |
result.z *= coeff | |
return result | |
[Extension] | |
public def Normalize(ref a as Quaternion): | |
length as single = a.Length() | |
if(length > 1.0e-15): | |
invlen as single = 1.0 / length | |
a.w *= invlen | |
a.x *= invlen | |
a.y *= invlen | |
a.z *= invlen | |
else: | |
length = 0.0 | |
a.w = 0.0 | |
a.x = 0.0 | |
a.y = 0.0 | |
a.z = 0.0 | |
return length | |
[Extension] | |
public def Normalized(a as Quaternion): | |
result as Quaternion | |
length as single = result.Length() | |
if(length > 1.0e-15): | |
invlen as single = 1.0 / length | |
result.w *= invlen | |
result.x *= invlen | |
result.y *= invlen | |
result.z *= invlen | |
else: | |
length = 0.0 | |
result.w = 0.0 | |
result.x = 0.0 | |
result.y = 0.0 | |
result.z = 0.0 | |
return result | |
[Extension] | |
public def Length(a as Quaternion): | |
return Mathf.Sqrt(a.w*a.w + a.x*a.x + a.y*a.y + a.z*a.z) | |
[Extension] | |
static def op_Addition(a as Quaternion, b as Quaternion): | |
return QuaternionExtensions.Add(a, b) | |
[Extension] | |
static def op_Subtraction(a as Quaternion, b as Quaternion): | |
return QuaternionExtensions.Sub(a, b) | |
static class QuaternionExtensions(): | |
def Add(a as Quaternion, b as Quaternion): | |
r as Quaternion | |
r.w = a.w+b.w | |
r.x = a.x+b.x | |
r.y = a.y+b.y | |
r.z = a.z+b.z | |
return r | |
def Sub(a as Quaternion, b as Quaternion): | |
r as Quaternion | |
r.w = a.w-b.w | |
r.x = a.x-b.x | |
r.y = a.y-b.y | |
r.z = a.z-b.z | |
return r | |
def SlerpNoInvert(fro as Quaternion, to as Quaternion, factor as single): | |
dot as single = Quaternion.Dot(fro,to); | |
if (Mathf.Abs(dot) > 0.9999f): | |
return fro; | |
theta as single = Mathf.Acos(dot) | |
sinT = 1.0f / Mathf.Sin(theta) | |
newFactor = Mathf.Sin(factor * theta) * sinT | |
invFactor = Mathf.Sin((1.0f - factor) * theta) * sinT; | |
return Quaternion( invFactor * fro.x + newFactor * to.x,invFactor * fro.y + newFactor * to.y,invFactor * fro.z + newFactor * to.z, invFactor * fro.w + newFactor * to.w ); |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment