quat
This commit is contained in:
Ryan Oldenburg
parent
7a9bca4236
commit
a307f89589
1 changed files with 107 additions and 97 deletions
204
src/vmath.nim
204
src/vmath.nim
|
|
@ -796,66 +796,66 @@ proc `*`*(a, b: Mat4): Mat4 =
|
|||
result[15] = b30*a03 + b31*a13 + b32*a23 + b33*a33
|
||||
|
||||
proc `*`*(a: Mat4, b: Vec3): Vec3 =
|
||||
result.x = a[0]*b.x + a[4]*b.y + a[8]*b.z + a[12]
|
||||
result.y = a[1]*b.x + a[5]*b.y + a[9]*b.z + a[13]
|
||||
result.z = a[2]*b.x + a[6]*b.y + a[10]*b.z + a[14]
|
||||
result.x = a[0] * b.x + a[4] * b.y + a[8] * b.z + a[12]
|
||||
result.y = a[1] * b.x + a[5] * b.y + a[9] * b.z + a[13]
|
||||
result.z = a[2] * b.x + a[6] * b.y + a[10] * b.z + a[14]
|
||||
|
||||
proc `*`*(a: Mat4, b: Vec4): Vec4 =
|
||||
result.x = a[0]*b.x + a[4]*b.y + a[8]*b.z + a[12]*b.w
|
||||
result.y = a[1]*b.x + a[5]*b.y + a[9]*b.z + a[13]*b.w
|
||||
result.z = a[2]*b.x + a[6]*b.y + a[10]*b.z + a[14]*b.w
|
||||
result.w = a[3]*b.x + a[7]*b.y + a[11]*b.z + a[15]*b.w
|
||||
result.x = a[0] * b.x + a[4] * b.y + a[8] * b.z + a[12] * b.w
|
||||
result.y = a[1] * b.x + a[5] * b.y + a[9] * b.z + a[13] * b.w
|
||||
result.z = a[2] * b.x + a[6] * b.y + a[10] * b.z + a[14] * b.w
|
||||
result.w = a[3] * b.x + a[7] * b.y + a[11] * b.z + a[15] * b.w
|
||||
|
||||
proc right*(a: Mat4): Vec3 =
|
||||
proc right*(a: Mat4): Vec3 {.inline.} =
|
||||
result.x = a[0]
|
||||
result.y = a[1]
|
||||
result.z = a[2]
|
||||
|
||||
proc `right=`*(a: var Mat4, b: Vec3) =
|
||||
proc `right=`*(a: var Mat4, b: Vec3) {.inline.} =
|
||||
a[0] = b.x
|
||||
a[1] = b.y
|
||||
a[2] = b.z
|
||||
|
||||
proc up*(a: Mat4): Vec3 =
|
||||
proc up*(a: Mat4): Vec3 {.inline.} =
|
||||
result.x = a[4]
|
||||
result.y = a[5]
|
||||
result.z = a[6]
|
||||
|
||||
proc `up=`*(a: var Mat4, b: Vec3) =
|
||||
proc `up=`*(a: var Mat4, b: Vec3) {.inline.} =
|
||||
a[4] = b.x
|
||||
a[5] = b.y
|
||||
a[6] = b.z
|
||||
|
||||
proc forward*(a: Mat4): Vec3 =
|
||||
proc forward*(a: Mat4): Vec3 {.inline.} =
|
||||
result.x = a[8]
|
||||
result.y = a[9]
|
||||
result.z = a[10]
|
||||
|
||||
proc `forward=`*(a: var Mat4, b: Vec3) =
|
||||
proc `forward=`*(a: var Mat4, b: Vec3) {.inline.} =
|
||||
a[8] = b.x
|
||||
a[9] = b.y
|
||||
a[10] = b.z
|
||||
|
||||
proc pos*(a: Mat4): Vec3 =
|
||||
proc pos*(a: Mat4): Vec3 {.inline.} =
|
||||
result.x = a[12]
|
||||
result.y = a[13]
|
||||
result.z = a[14]
|
||||
|
||||
proc `pos=`*(a: var Mat4, b: Vec3) =
|
||||
proc `pos=`*(a: var Mat4, b: Vec3) {.inline.} =
|
||||
a[12] = b.x
|
||||
a[13] = b.y
|
||||
a[14] = b.z
|
||||
|
||||
proc rotationOnly*(a: Mat4): Mat4 =
|
||||
proc rotationOnly*(a: Mat4): Mat4 {.inline.} =
|
||||
result = a
|
||||
result.pos = vec3(0, 0, 0)
|
||||
|
||||
proc dist*(a, b: Mat4): float32 =
|
||||
var
|
||||
let
|
||||
x = a[12] - b[12]
|
||||
y = a[13] - b[13]
|
||||
z = a[14] - b[14]
|
||||
sqrt(x*x + y*y + z*z)
|
||||
sqrt(x * x + y * y + z * z)
|
||||
|
||||
#[
|
||||
proc translate*(a: Mat4, v: Vec3): Mat4 =
|
||||
|
|
@ -932,16 +932,16 @@ proc hrp*(m: Mat4): Vec3 =
|
|||
result.z = roll
|
||||
|
||||
proc frustum*(left, right, bottom, top, near, far: float32): Mat4 =
|
||||
var
|
||||
let
|
||||
rl = (right - left)
|
||||
tb = (top - bottom)
|
||||
fn = (far - near)
|
||||
result[0] = (near*2) / rl
|
||||
result[0] = (near * 2) / rl
|
||||
result[1] = 0
|
||||
result[2] = 0
|
||||
result[3] = 0
|
||||
result[4] = 0
|
||||
result[5] = (near*2) / tb
|
||||
result[5] = (near * 2) / tb
|
||||
result[6] = 0
|
||||
result[7] = 0
|
||||
result[8] = (right + left) / rl
|
||||
|
|
@ -950,17 +950,17 @@ proc frustum*(left, right, bottom, top, near, far: float32): Mat4 =
|
|||
result[11] = -1
|
||||
result[12] = 0
|
||||
result[13] = 0
|
||||
result[14] = -(far*near*2) / fn
|
||||
result[14] = -(far * near * 2) / fn
|
||||
result[15] = 0
|
||||
|
||||
proc perspective*(fovy, aspect, near, far: float32): Mat4 =
|
||||
var
|
||||
top = near * tan(fovy*PI / 360.0)
|
||||
let
|
||||
top = near * tan(fovy * PI / 360.0)
|
||||
right = top * aspect
|
||||
frustum(-right, right, -top, top, near, far)
|
||||
|
||||
proc ortho*(left, right, bottom, top, near, far: float32): Mat4 =
|
||||
var
|
||||
let
|
||||
rl = (right - left)
|
||||
tb = (top - bottom)
|
||||
fn = (far - near)
|
||||
|
|
@ -982,7 +982,7 @@ proc ortho*(left, right, bottom, top, near, far: float32): Mat4 =
|
|||
result[15] = 1
|
||||
|
||||
proc lookAt*(eye, center, up: Vec3): Mat4 =
|
||||
var
|
||||
let
|
||||
eyex = eye[0]
|
||||
eyey = eye[1]
|
||||
eyez = eye[2]
|
||||
|
|
@ -1003,34 +1003,34 @@ proc lookAt*(eye, center, up: Vec3): Mat4 =
|
|||
z2 = eyez - center[2]
|
||||
|
||||
# normalize (no check needed for 0 because of early return)
|
||||
var len = 1/sqrt(z0*z0 + z1*z1 + z2*z2)
|
||||
var len = 1 / sqrt(z0 * z0 + z1 * z1 + z2 * z2)
|
||||
z0 *= len
|
||||
z1 *= len
|
||||
z2 *= len
|
||||
|
||||
var
|
||||
# vec3.normalize(vec3.cross(up, z, x))
|
||||
x0 = upy*z2 - upz*z1
|
||||
x1 = upz*z0 - upx*z2
|
||||
x2 = upx*z1 - upy*z0
|
||||
len = sqrt(x0*x0 + x1*x1 + x2*x2)
|
||||
x0 = upy * z2 - upz * z1
|
||||
x1 = upz * z0 - upx * z2
|
||||
x2 = upx * z1 - upy * z0
|
||||
len = sqrt(x0 * x0 + x1 * x1 + x2 * x2)
|
||||
if len == 0:
|
||||
x0 = 0
|
||||
x1 = 0
|
||||
x2 = 0
|
||||
else:
|
||||
len = 1/len
|
||||
len = 1 / len
|
||||
x0 *= len
|
||||
x1 *= len
|
||||
x2 *= len
|
||||
|
||||
var
|
||||
# vec3.normalize(vec3.cross(z, x, y))
|
||||
y0 = z1*x2 - z2*x1
|
||||
y1 = z2*x0 - z0*x2
|
||||
y2 = z0*x1 - z1*x0
|
||||
y0 = z1 * x2 - z2 * x1
|
||||
y1 = z2 * x0 - z0 * x2
|
||||
y2 = z0 * x1 - z1 * x0
|
||||
|
||||
len = sqrt(y0*y0 + y1*y1 + y2*y2)
|
||||
len = sqrt(y0 * y0 + y1 * y1 + y2 * y2)
|
||||
if len == 0:
|
||||
y0 = 0
|
||||
y1 = 0
|
||||
|
|
@ -1053,9 +1053,9 @@ proc lookAt*(eye, center, up: Vec3): Mat4 =
|
|||
result[9] = y2
|
||||
result[10] = z2
|
||||
result[11] = 0
|
||||
result[12] = -(x0*eyex + x1*eyey + x2*eyez)
|
||||
result[13] = -(y0*eyex + y1*eyey + y2*eyez)
|
||||
result[14] = -(z0*eyex + z1*eyey + z2*eyez)
|
||||
result[12] = -(x0 * eyex + x1 * eyey + x2 * eyez)
|
||||
result[13] = -(y0 * eyex + y1 * eyey + y2 * eyez)
|
||||
result[14] = -(z0 * eyex + z1 * eyey + z2 * eyez)
|
||||
result[15] = 1
|
||||
|
||||
proc mat3*(m: Mat4): Mat3 =
|
||||
|
|
@ -1128,19 +1128,19 @@ type Quat* = object
|
|||
z*: float32
|
||||
w*: float32
|
||||
|
||||
proc quat*(x, y, z, w: float32): Quat =
|
||||
proc quat*(x, y, z, w: float32): Quat {.inline.} =
|
||||
result.x = x
|
||||
result.y = y
|
||||
result.z = z
|
||||
result.w = w
|
||||
|
||||
proc conjugate*(q: Quat): Quat =
|
||||
proc conjugate*(q: Quat): Quat {.inline.} =
|
||||
result.w = +q.w
|
||||
result.x = -q.x
|
||||
result.y = -q.y
|
||||
result.z = -q.z
|
||||
|
||||
proc length*(q: Quat): float32 =
|
||||
proc length*(q: Quat): float32 {.inline.} =
|
||||
sqrt(
|
||||
q.w * q.w +
|
||||
q.x * q.x +
|
||||
|
|
@ -1155,23 +1155,23 @@ proc normalize*(q: Quat): Quat =
|
|||
result.z = q.z / m
|
||||
result.w = q.w / m
|
||||
|
||||
proc xyz*(q: Quat): Vec3 =
|
||||
proc xyz*(q: Quat): Vec3 {.inline.} =
|
||||
result.x = q.x
|
||||
result.y = q.y
|
||||
result.z = q.z
|
||||
|
||||
proc `xyz=`*(q: var Quat, v: Vec3) =
|
||||
proc `xyz=`*(q: var Quat, v: Vec3) {.inline.} =
|
||||
q.x = v.x
|
||||
q.y = v.y
|
||||
q.z = v.z
|
||||
|
||||
proc `-`*(a: var Quat): Quat =
|
||||
proc `-`*(a: var Quat): Quat {.inline.} =
|
||||
result.x = -a.x
|
||||
result.y = -a.y
|
||||
result.z = -a.z
|
||||
result.w = -a.w
|
||||
|
||||
proc `+`*(a: Quat, b: Quat): Quat =
|
||||
proc `+`*(a: Quat, b: Quat): Quat {.inline.} =
|
||||
result.x = a.x + b.x
|
||||
result.y = a.y + b.y
|
||||
result.z = a.z + b.z
|
||||
|
|
@ -1195,7 +1195,7 @@ proc `*`*(a, b: Quat): Quat =
|
|||
result.z = a.z * b.w + a.w * b.z + a.x * b.y - a.y * b.x
|
||||
result.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
|
||||
|
||||
proc `*`*(q: Quat, v: float32): Quat =
|
||||
proc `*`*(q: Quat, v: float32): Quat {.inline.} =
|
||||
## Multiply the quaternion by a float32.
|
||||
result.x = q.x * v
|
||||
result.y = q.y * v
|
||||
|
|
@ -1204,7 +1204,7 @@ proc `*`*(q: Quat, v: float32): Quat =
|
|||
|
||||
proc `*`*(q: Quat, v: Vec3): Vec3 =
|
||||
## Multiply the quaternion by a vector.
|
||||
var
|
||||
let
|
||||
x = v.x
|
||||
y = v.y
|
||||
z = v.z
|
||||
|
|
@ -1223,29 +1223,35 @@ proc `*`*(q: Quat, v: Vec3): Vec3 =
|
|||
result.y = iy * qw + iw * -qy + iz * -qx - ix * -qz
|
||||
result.z = iz * qw + iw * -qz + ix * -qy - iy * -qx
|
||||
|
||||
proc `[]`*(a: var Quat, i: int, b: float32) =
|
||||
case i
|
||||
of 0: a.x
|
||||
of 1: a.y
|
||||
of 2: a.z
|
||||
of 3: a.w
|
||||
else: raise newException(IndexDefect, "Index not in 0 .. 3")
|
||||
|
||||
proc `[]=`*(a: var Quat, i: int, b: float32) =
|
||||
assert(i == 0 or i == 1 or i == 2 or i == 3)
|
||||
if i == 0:
|
||||
a.x = b
|
||||
elif i == 1:
|
||||
a.y = b
|
||||
elif i == 2:
|
||||
a.z = b
|
||||
elif i == 3:
|
||||
a.w = b
|
||||
case i
|
||||
of 0: a.x = b
|
||||
of 1: a.y = b
|
||||
of 2: a.z = b
|
||||
of 3: a.w = b
|
||||
else: raise newException(IndexDefect, "Index not in 0 .. 3")
|
||||
|
||||
proc mat3*(q: Quat): Mat3 =
|
||||
var xx = q.x * q.x
|
||||
var xy = q.x * q.y
|
||||
var xz = q.x * q.z
|
||||
var xw = q.x * q.w
|
||||
let
|
||||
xx = q.x * q.x
|
||||
xy = q.x * q.y
|
||||
xz = q.x * q.z
|
||||
xw = q.x * q.w
|
||||
|
||||
var yy = q.y * q.y
|
||||
var yz = q.y * q.z
|
||||
var yw = q.y * q.w
|
||||
yy = q.y * q.y
|
||||
yz = q.y * q.z
|
||||
yw = q.y * q.w
|
||||
|
||||
var zz = q.z * q.z
|
||||
var zw = q.z * q.w
|
||||
zz = q.z * q.z
|
||||
zw = q.z * q.w
|
||||
|
||||
result[0] = 1 - 2 * (yy + zz)
|
||||
result[1] = 0 + 2 * (xy - zw)
|
||||
|
|
@ -1258,17 +1264,18 @@ proc mat3*(q: Quat): Mat3 =
|
|||
result[8] = 1 - 2 * (xx + yy)
|
||||
|
||||
proc mat4*(q: Quat): Mat4 =
|
||||
var xx = q.x * q.x
|
||||
var xy = q.x * q.y
|
||||
var xz = q.x * q.z
|
||||
var xw = q.x * q.w
|
||||
let
|
||||
xx = q.x * q.x
|
||||
xy = q.x * q.y
|
||||
xz = q.x * q.z
|
||||
xw = q.x * q.w
|
||||
|
||||
var yy = q.y * q.y
|
||||
var yz = q.y * q.z
|
||||
var yw = q.y * q.w
|
||||
yy = q.y * q.y
|
||||
yz = q.y * q.z
|
||||
yw = q.y * q.w
|
||||
|
||||
var zz = q.z * q.z
|
||||
var zw = q.z * q.w
|
||||
zz = q.z * q.z
|
||||
zw = q.z * q.w
|
||||
|
||||
result[00] = 1 - 2 * (yy + zz)
|
||||
result[01] = 0 + 2 * (xy - zw)
|
||||
|
|
@ -1288,11 +1295,11 @@ proc mat4*(q: Quat): Mat4 =
|
|||
result[14] = 0
|
||||
result[15] = 1.0
|
||||
|
||||
proc recifuncalSqrt*(x: float32): float32 =
|
||||
1.0/sqrt(x)
|
||||
proc recifuncalSqrt*(x: float32): float32 {.inline.} =
|
||||
1.0 / sqrt(x)
|
||||
|
||||
proc quat*(m: Mat4): Quat =
|
||||
var
|
||||
let
|
||||
m00 = m[0]
|
||||
m01 = m[4]
|
||||
m02 = m[8]
|
||||
|
|
@ -1305,8 +1312,9 @@ proc quat*(m: Mat4): Quat =
|
|||
m21 = m[6]
|
||||
m22 = m[10]
|
||||
|
||||
var q: Quat
|
||||
var t: float32
|
||||
var
|
||||
q: Quat
|
||||
t: float32
|
||||
|
||||
if m22 < 0:
|
||||
if m00 > m11:
|
||||
|
|
@ -1328,8 +1336,9 @@ proc quat*(m: Mat4): Quat =
|
|||
q
|
||||
|
||||
proc fromAxisAngle*(axis: Vec3, angle: float32): Quat =
|
||||
var a = axis.normalize()
|
||||
var s = sin(angle / 2)
|
||||
let
|
||||
a = axis.normalize()
|
||||
s = sin(angle / 2)
|
||||
result.x = a.x * s
|
||||
result.y = a.y * s
|
||||
result.z = a.z * s
|
||||
|
|
@ -1348,18 +1357,19 @@ proc toAxisAngle*(q: Quat, axis: var Vec3, angle: var float32) =
|
|||
axis.z = q.z / sinAngle
|
||||
|
||||
proc quat*(heading, pitch, roll: float32): Quat =
|
||||
var t0 = cos(heading * 0.5)
|
||||
var t1 = sin(heading * 0.5)
|
||||
var t2 = cos(roll * 0.5)
|
||||
var t3 = sin(roll * 0.5)
|
||||
var t4 = cos(pitch * 0.5)
|
||||
var t5 = sin(pitch * 0.5)
|
||||
let
|
||||
t0 = cos(heading * 0.5)
|
||||
t1 = sin(heading * 0.5)
|
||||
t2 = cos(roll * 0.5)
|
||||
t3 = sin(roll * 0.5)
|
||||
t4 = cos(pitch * 0.5)
|
||||
t5 = sin(pitch * 0.5)
|
||||
result.w = t0 * t2 * t4 + t1 * t3 * t5
|
||||
result.x = t0 * t3 * t4 - t1 * t2 * t5
|
||||
result.y = t0 * t2 * t5 + t1 * t3 * t4
|
||||
result.z = t1 * t2 * t4 - t0 * t3 * t5
|
||||
|
||||
proc quat*(hpr: Vec3): Quat =
|
||||
proc quat*(hpr: Vec3): Quat {.inline.} =
|
||||
quat(hpr.x, hpr.y, hpr.z)
|
||||
|
||||
proc hrp*(q: Quat): Vec3 =
|
||||
|
|
@ -1380,8 +1390,8 @@ proc hrp*(q: Quat): Vec3 =
|
|||
var t4 = +1.0 - 2.0 * (ysqr + q.z * q.z)
|
||||
result.x = arctan2(t3, t4)
|
||||
|
||||
proc dot*(a: Quat, b: Quat): float32 =
|
||||
a.x*b.x + a.y*b.y + a.z*b.z + a.w*b.w
|
||||
proc dot*(a: Quat, b: Quat): float32 {.inline.} =
|
||||
a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w
|
||||
|
||||
proc nlerp*(a: Quat, b: Quat, v: float32): Quat =
|
||||
if dot(a, b) < 0:
|
||||
|
|
@ -1393,25 +1403,25 @@ proc nlerp*(a: Quat, b: Quat, v: float32): Quat =
|
|||
proc `$`*(a: Quat): string =
|
||||
&"q({a.x:.8f}, {a.y:.8f}, {a.z:.8f}, {a.w:.8f})"
|
||||
|
||||
proc rotate*(angle: float32, axis: Vec3): Mat4 =
|
||||
proc rotate*(angle: float32, axis: Vec3): Mat4 {.inline.} =
|
||||
fromAxisAngle(axis, angle).mat4()
|
||||
|
||||
proc rotateX*(angle: float32): Mat4 =
|
||||
proc rotateX*(angle: float32): Mat4 {.inline.} =
|
||||
rotate(angle, vec3(1, 0, 0))
|
||||
|
||||
proc rotateY*(angle: float32): Mat4 =
|
||||
proc rotateY*(angle: float32): Mat4 {.inline.} =
|
||||
rotate(angle, vec3(0, 1, 0))
|
||||
|
||||
proc rotateZ*(angle: float32): Mat4 =
|
||||
proc rotateZ*(angle: float32): Mat4 {.inline.} =
|
||||
rotate(angle, vec3(0, 0, 1))
|
||||
|
||||
proc scaleMat*(scale: Vec3): Mat4 =
|
||||
proc scaleMat*(scale: Vec3): Mat4 {.inline.} =
|
||||
result[0] = scale.x
|
||||
result[5] = scale.y
|
||||
result[10] = scale.z
|
||||
result[15] = 1.0
|
||||
|
||||
proc scaleMat*(scale: float32): Mat4 =
|
||||
proc scaleMat*(scale: float32): Mat4 {.inline.} =
|
||||
scaleMat(vec3(scale, scale, scale))
|
||||
|
||||
type Rect* = object
|
||||
|
|
|
|||
Loading…
Reference in a new issue