277 lines
8.7 KiB
Nim
277 lines
8.7 KiB
Nim
|
||
|
||
# TODO: make own vmath module with proper quats and more matrix types
|
||
|
||
import vmath except Quat, quat
|
||
|
||
type RotationOrder* = enum
|
||
Quaternion
|
||
EulerXYZ
|
||
EulerXZY
|
||
EulerYXZ
|
||
EulerYZX
|
||
EulerZXY
|
||
EulerZYX
|
||
AxisAngle
|
||
|
||
|
||
type
|
||
GQuat*[T] = object
|
||
x*, y*, z*, w*: float32
|
||
Quat* = GQuat[float32]
|
||
DQuat* = GQuat[float64]
|
||
|
||
proc gquat*[T](x,y,z,w: T): GQuat[T] {.inline.} =
|
||
GQuat[T](x:x, y:y, z:z, w:w)
|
||
|
||
proc quat*(x,y,z,w: float32): Quat {.inline.} =
|
||
Quat(x:x, y:y, z:z, w:w)
|
||
|
||
proc dquat*(x,y,z,w: float64): DQuat {.inline.} =
|
||
DQuat(x:x, y:y, z:z, w:w)
|
||
|
||
template quat*(): Quat = quat(0,0,0,1)
|
||
template dquat*(): DQuat = dquat(0,0,0,1)
|
||
template quat*(v: Vec4): Quat = cast[Quat](v)
|
||
template dquat*(v: DVec4): DQuat = cast[DQuat](v)
|
||
|
||
template `[]`*[T](q: GQuat[T], i: int): T = cast[array[4, T]](q)[i]
|
||
template `[]=`*[T](q: var GQuat[T], i: int, v: T) =
|
||
case i:
|
||
of 0: q.x = v
|
||
of 1: q.y = v
|
||
of 2: q.z = v
|
||
of 3: q.w = v
|
||
else: raise RangeDefect.newException "Index out of range"
|
||
|
||
# https://stackoverflow.com/questions/28673777/convert-quaternion-from-right-handed-to-left-handed-coordinate-system
|
||
# S=s, B=−b, C=−d and D=−c.
|
||
func swap_quat_handness*[T](q: GQuat[T]): GQuat[T] =
|
||
# this swaps from right handed Z-up to left handed Y-up
|
||
# and vice versa
|
||
# TODO: rotate 90 to not change up-ness?
|
||
GQuat[T](x: -q.x, y: -q.z, z: -q.y, w: q.w)
|
||
|
||
func to_quat*[T](m: GMat3[T]|GMat4[T]): GQuat[T] =
|
||
# http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
|
||
let trace = m[0,0] + m[1,1] + m[2,2]
|
||
if trace > 0:
|
||
let s = 0.5 / sqrt(trace + 1.0)
|
||
result.w = 0.25 / s
|
||
result.x = (m[1,2] - m[2,1]) * s
|
||
result.y = (m[2,0] - m[0,2]) * s
|
||
result.z = (m[0,1] - m[1,0]) * s
|
||
elif (m[0,0] > m[1,1]) and (m[0,0] > m[2,2]):
|
||
let s = 2.0 * sqrt(1.0 + m[0,0] - m[1,1] - m[2,2])
|
||
result.w = (m[1,2] - m[2,1]) / s
|
||
result.x = 0.25 * s
|
||
result.y = (m[1,0] + m[0,1]) / s
|
||
result.z = (m[2,0] + m[0,2]) / s
|
||
elif m[1,1] > m[2,2]:
|
||
let s = 2.0 * sqrt(1.0 + m[1,1] - m[0,0] - m[2,2])
|
||
result.w = (m[2,0] - m[0,2]) / s
|
||
result.x = (m[1,0] + m[0,1]) / s
|
||
result.y = 0.25 * s
|
||
result.z = (m[2,1] + m[1,2]) / s
|
||
else:
|
||
let s = 2.0 * sqrt(1.0 + m[2,2] - m[0,0] - m[1,1])
|
||
result.w = (m[0,1] - m[1,0]) / s
|
||
result.x = (m[2,0] + m[0,2]) / s
|
||
result.y = (m[2,1] + m[1,2]) / s
|
||
result.z = 0.25 * s
|
||
|
||
func inverse*[T](a: GQuat[T]): GQuat[T] =
|
||
let dot = a.x * a.x + a.y * a.y + a.z * a.z + a.w * a.w
|
||
if dot == 0:
|
||
return
|
||
return gquat[T](-a.x, -a.y, -a.z, a.w) * (1/dot)
|
||
|
||
proc normalize*[T](q: GQuat[T]): GQuat[T] =
|
||
let length = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w)
|
||
if length == 0:
|
||
return q
|
||
return gquat[T](q.x / length , q.y / length, q.z / length, q.w / length)
|
||
|
||
func to_mat[T,U](q: GQuat[T]): U =
|
||
let xx = q.x * q.x * 2
|
||
let yx = q.y * q.x * 2
|
||
let yy = q.y * q.y * 2
|
||
let zx = q.z * q.x * 2
|
||
let zy = q.z * q.y * 2
|
||
let zz = q.z * q.z * 2
|
||
let wx = q.w * q.x * 2
|
||
let wy = q.w * q.y * 2
|
||
let wz = q.w * q.z * 2
|
||
|
||
result[0,0] = 1 - yy - zz
|
||
result[1,0] = yx - wz
|
||
result[2,0] = zx + wy
|
||
|
||
result[0,1] = yx + wz
|
||
result[1,1] = 1 - xx - zz
|
||
result[2,1] = zy - wx
|
||
|
||
result[0,2] = zx - wy
|
||
result[1,2] = zy + wx
|
||
result[2,2] = 1 - xx - yy
|
||
when U is GMat4[T]:
|
||
result[3,3] = 1
|
||
|
||
template to_mat3*[T](q: GQuat[T]): GMat3[T] =
|
||
to_mat[T,GMat3[T]](q)
|
||
|
||
template to_mat4*[T](q: GQuat[T]): GMat4[T] =
|
||
to_mat[T,GMat4[T]](q)
|
||
|
||
|
||
func `*`*[T](q: GQuat[T], a: GVec3[T]): GVec3[T] =
|
||
# calculate quat * vec
|
||
let ix = q.w * a.x + q.y * a.z - q.z * a.y
|
||
let iy = q.w * a.y + q.z * a.x - q.x * a.z
|
||
let iz = q.w * a.z + q.x * a.y - q.y * a.x
|
||
let iw = -q.x * a.x - q.y * a.y - q.z * a.z
|
||
|
||
# calculate result * inverse quat
|
||
result.x = ix * q.w + iw * -q.x + iy * -q.z - iz * -q.y
|
||
result.y = iy * q.w + iw * -q.y + iz * -q.x - ix * -q.z
|
||
result.z = iz * q.w + iw * -q.z + ix * -q.y - iy * -q.x
|
||
|
||
proc `*`*[T](a, b: GQuat[T]): GQuat[T] =
|
||
GQuat[T](
|
||
x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
|
||
y: a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
|
||
z: a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
|
||
w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
|
||
)
|
||
|
||
proc `*`*[T](q: GQuat[T], s: T): GQuat[T] =
|
||
GQuat[T](x: q.x*s, y: q.y*s, z: q.z*s, w: q.w*s)
|
||
|
||
template to_tuple*[T](v: GQuat[T]): (T, T, T, T) =
|
||
# (v.x, v.y, v.z, v.w)
|
||
cast[(T,T,T,T)](v)
|
||
|
||
|
||
# http://stackoverflow.com/questions/1031005/is-there-an-algorithm-for-converting-quaternion-rotations-to-euler-angle-rotatio
|
||
|
||
proc threeaxisrot[T](r11, r12, r21, r31, r32: T): GVec3[T] =
|
||
gvec3[T](arctan2(r31, r32), arcsin (r21), arctan2(r11, r12))
|
||
|
||
# NOTE: It uses Blender's convention for euler rotations:
|
||
# XYZ means that to convert back to quat you must rotate Z, then Y, then X
|
||
|
||
proc to_euler_XYZ*[T](q: GQuat[T]): GVec3[T] =
|
||
threeaxisrot(2*(q.x*q.y + q.w*q.z),
|
||
q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
|
||
-2*(q.x*q.z - q.w*q.y),
|
||
2*(q.y*q.z + q.w*q.x),
|
||
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z)
|
||
|
||
proc to_euler_XZY*[T](q: GQuat[T]): GVec3[T] =
|
||
threeaxisrot(-2*(q.x*q.z - q.w*q.y),
|
||
q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
|
||
2*(q.x*q.y + q.w*q.z),
|
||
-2*(q.y*q.z - q.w*q.x),
|
||
q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z).xzy
|
||
|
||
proc to_euler_YXZ*[T](q: GQuat[T]): GVec3[T] =
|
||
threeaxisrot(-2*(q.x*q.y - q.w*q.z),
|
||
q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
|
||
2*(q.y*q.z + q.w*q.x),
|
||
-2*(q.x*q.z - q.w*q.y),
|
||
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z).yxz
|
||
|
||
proc to_euler_YZX*[T](q: GQuat[T]): GVec3[T] =
|
||
threeaxisrot(2*(q.y*q.z + q.w*q.x),
|
||
q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
|
||
-2*(q.x*q.y - q.w*q.z),
|
||
2*(q.x*q.z + q.w*q.y),
|
||
q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z).yzx
|
||
|
||
proc to_euler_ZXY*[T](q: GQuat[T]): GVec3[T] =
|
||
threeaxisrot(2*(q.x*q.z + q.w*q.y),
|
||
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
|
||
-2*(q.y*q.z - q.w*q.x),
|
||
2*(q.x*q.y + q.w*q.z),
|
||
q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z).zxy
|
||
|
||
proc to_euler_ZYX*[T](q: GQuat[T]): GVec3[T] =
|
||
threeaxisrot(-2*(q.y*q.z - q.w*q.x),
|
||
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
|
||
2*(q.x*q.z + q.w*q.y),
|
||
-2*(q.x*q.y - q.w*q.z),
|
||
q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z).zyx
|
||
|
||
proc rotateX*[T](q: GQuat[T], rad: T): GQuat[T] =
|
||
let rad = rad * 0.5
|
||
let x = sin(rad)
|
||
let w = cos(rad)
|
||
result.x = q.x * w + q.w * x
|
||
result.y = q.y * w + q.z * x
|
||
result.z = q.z * w - q.y * x
|
||
result.w = q.w * w - q.x * x
|
||
|
||
proc rotateY*[T](q: GQuat[T], rad: T): GQuat[T] =
|
||
let rad = rad * 0.5
|
||
let y = sin(rad)
|
||
let w = cos(rad)
|
||
result.x = q.x * w - q.z * y
|
||
result.y = q.y * w + q.w * y
|
||
result.z = q.z * w + q.x * y
|
||
result.w = q.w * w - q.y * y
|
||
|
||
proc rotateZ*[T](q: GQuat[T], rad: T): GQuat[T] =
|
||
let rad = rad * 0.5
|
||
let z = sin(rad)
|
||
let w = cos(rad)
|
||
result.x = q.x * w + q.y * z
|
||
result.y = q.y * w - q.x * z
|
||
result.z = q.z * w + q.w * z
|
||
result.w = q.w * w - q.z * z
|
||
|
||
|
||
proc to_quat*[T](euler: GVec3[T], order: RotationOrder): GQuat[T] =
|
||
var q = quat()
|
||
case order:
|
||
of Quaternion:
|
||
raise newException(ValueError, "invalid euler order")
|
||
of EulerXYZ:
|
||
q = q.rotateZ euler.z
|
||
q = q.rotateY euler.y
|
||
q = q.rotateX euler.x
|
||
of EulerXZY:
|
||
q = q.rotateY euler.y
|
||
q = q.rotateZ euler.z
|
||
q = q.rotateX euler.x
|
||
of EulerYXZ:
|
||
q = q.rotateZ euler.z
|
||
q = q.rotateX euler.x
|
||
q = q.rotateY euler.y
|
||
of EulerYZX:
|
||
q = q.rotateX euler.x
|
||
q = q.rotateZ euler.z
|
||
q = q.rotateY euler.y
|
||
of EulerZXY:
|
||
q = q.rotateY euler.y
|
||
q = q.rotateX euler.x
|
||
q = q.rotateZ euler.z
|
||
of EulerZYX:
|
||
q = q.rotateX euler.x
|
||
q = q.rotateY euler.y
|
||
q = q.rotateZ euler.z
|
||
of AxisAngle:
|
||
raise newException(ValueError, "invalid euler order")
|
||
return q
|
||
|
||
proc rotationTo*(a,b: Vec3): Quat =
|
||
let dot = dot(a, b)
|
||
if dot < -0.999999:
|
||
var v = cross(vec3(1,0,0), a)
|
||
if v.length < 0.000001:
|
||
v = cross(vec3(0,1,0), a)
|
||
return cast[Quat](fromAxisAngle(v.normalize, PI))
|
||
elif dot > 0.999999:
|
||
return quat(0,0,0,1)
|
||
else:
|
||
let v = cross(a, b)
|
||
return quat(v.x, v.y, v.z, 1.0+dot).normalize
|