Alberto Torres
cea7df6947
* Incomplete port of myou-engine-js to nimskull, after many months of work, and a few extra features that weren't exactly necessary for a "first commit" to work. Excuse the lack of commit history up to this point. * Bare bones structure of the documentation and the process to update it. * Restructure of the whole project to have a more sensible organization. * Making submodules of forks of larger libraries. * README, licenses, AUTHORS.md.
241 lines
7.5 KiB
Nim
241 lines
7.5 KiB
Nim
|
||
|
||
# TODO: make own vmath module with proper quats and more matrix types
|
||
|
||
import vmath except Quat
|
||
|
||
type RotationOrder* = enum
|
||
Quaternion
|
||
EulerXYZ
|
||
EulerXZY
|
||
EulerYXZ
|
||
EulerYZX
|
||
EulerZXY
|
||
EulerZYX
|
||
AxisAngle
|
||
|
||
|
||
type
|
||
GQuat*[T] = GVec4[T]
|
||
Quat* = GQuat[float32]
|
||
DQuat* = GQuat[float64]
|
||
|
||
template gquat*[T](x,y,z,w: T): GQuat[T] =
|
||
gvec4[T](x,y,z,w)
|
||
|
||
proc `@`*[T](a,b: GQuat[T]): GQuat[T] =
|
||
return gquat[T](
|
||
a.x * b.w + a.w * b.x + a.y * b.z - a.z * b.y,
|
||
a.y * b.w + a.w * b.y + a.z * b.x - a.x * b.z,
|
||
a.z * b.w + a.w * b.z + a.x * b.y - a.y * b.x,
|
||
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
|
||
)
|
||
|
||
# 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]): 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)
|
||
|
||
func to_mat3*[T](q: GQuat[T]): GMat3[T] =
|
||
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
|
||
|
||
|
||
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
|
||
|
||
|
||
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 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)
|
||
|