Skip to content

Commit 0f30452

Browse files
elect86elect86
committed
- DualQuat
- fixed "axisAngle" + coverage - spline - yawPitchRoll with lambda
1 parent 679bc23 commit 0f30452

21 files changed

+1577
-66
lines changed

‎src/main/kotlin/glm_/buffer/buffer.kt‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -84,4 +84,12 @@ val IntBuffer.cap get() = capacity()
8484
val LongBuffer.cap get() = capacity()
8585
val FloatBuffer.cap get() = capacity()
8686
val DoubleBuffer.cap get() = capacity()
87-
val CharBuffer.cap get() = capacity()
87+
val CharBuffer.cap get() = capacity()
88+
89+
val ByteBuffer.rem get() = remaining()
90+
val ShortBuffer.rem get() = remaining()
91+
val IntBuffer.rem get() = remaining()
92+
val LongBuffer.rem get() = remaining()
93+
val FloatBuffer.rem get() = remaining()
94+
val DoubleBuffer.rem get() = remaining()
95+
val CharBuffer.rem get() = remaining()

‎src/main/kotlin/glm_/dualQuat/DualQuat.kt‎

Lines changed: 273 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,273 @@
1+
package glm_.dualQuat
2+
3+
import glm_.mat2x4.Mat2x4
4+
import glm_.mat3x4.Mat3x4
5+
import glm_.quat.Quat
6+
import glm_.vec3.Vec3
7+
import glm_.vec4.Vec4
8+
import kotlin.math.sqrt
9+
10+
11+
class DualQuat(var real: Quat, var dual: Quat) {
12+
13+
// -- Component accesses --
14+
15+
/** Return the count of components of a dual quaternion */
16+
val length = 2
17+
18+
operator fun get(index: Int) = when (index) {
19+
0 -> real
20+
1 -> dual
21+
else -> throw Error()
22+
}
23+
24+
operator fun set(index: Int, quat: Quat) = when (index) {
25+
0 -> real put quat
26+
1 -> dual put quat
27+
else -> throw Error()
28+
}
29+
30+
// -- Implicit basic constructors --
31+
32+
constructor() : this(Quat(), Quat(0f))
33+
34+
constructor(dualQuat: DualQuat) : this(Quat(dualQuat.real), Quat(dualQuat.dual))
35+
36+
constructor(realW: Float, realX: Float, realY: Float, realZ: Float,
37+
dualW: Float, dualX: Float, dualY: Float, dualZ: Float) : this(
38+
Quat(realW, realX, realY, realZ), Quat(dualW, dualX, dualY, dualZ))
39+
40+
41+
// -- Explicit basic constructors --
42+
43+
constructor(real: Quat) : this(real, Quat(0f))
44+
45+
constructor(orientation: Quat, translation: Vec3) : this(orientation, Quat(
46+
-0.5f * (translation.x * orientation.x + translation.y * orientation.y + translation.z * orientation.z),
47+
+0.5f * (translation.x * orientation.w + translation.y * orientation.z - translation.z * orientation.y),
48+
+0.5f * (-translation.x * orientation.z + translation.y * orientation.w + translation.z * orientation.x),
49+
+0.5f * (translation.x * orientation.y - translation.y * orientation.x + translation.z * orientation.w)))
50+
51+
// -- Conversion constructors --
52+
53+
constructor(m: Mat2x4) : this() {
54+
dualquat_cast(m, this)
55+
}
56+
57+
constructor(m: Mat3x4) : this() {
58+
dualquat_cast(m, this)
59+
}
60+
61+
// -- Lambda constructors --
62+
63+
constructor(block: (Int) -> Float) : this(
64+
block(0), block(1), block(2), block(3),
65+
block(4), block(5), block(6), block(7))
66+
67+
// -- Unary bit operators --
68+
69+
operator fun unaryPlus() = this
70+
operator fun unaryMinus() = DualQuat(-real, -dual)
71+
72+
73+
fun put(real: Quat, dual: Quat) {
74+
real.put(real.w, real.x, real.y, real.z)
75+
dual.put(dual.w, dual.x, dual.y, dual.z)
76+
}
77+
78+
fun put(realW: Float, realX: Float, realY: Float, realZ: Float,
79+
dualW: Float, dualX: Float, dualY: Float, dualZ: Float) {
80+
real.put(realW, realX, realY, realZ)
81+
dual.put(dualW, dualX, dualY, dualZ)
82+
}
83+
84+
operator fun invoke(realW: Float, realX: Float, realY: Float, realZ: Float,
85+
dualW: Float, dualX: Float, dualY: Float, dualZ: Float): DualQuat {
86+
put(realW, realX, realY, realZ, dualW, dualX, dualY, dualZ)
87+
return this
88+
}
89+
90+
operator fun invoke(real: Quat, dual: Quat): DualQuat {
91+
put(real, dual)
92+
return this
93+
}
94+
95+
// -- Specific binary arithmetic operators --
96+
97+
infix operator fun plus(b: DualQuat) = plus(DualQuat(), this, b)
98+
fun plus(b: DualQuat, res: DualQuat) = plus(res, this, b)
99+
infix operator fun plusAssign(b: DualQuat) {
100+
plus(this, this, b)
101+
}
102+
103+
infix operator fun times(b: DualQuat) = times(DualQuat(), this, b)
104+
infix operator fun times(b: Float) = times(DualQuat(), this, b)
105+
infix operator fun times(b: Vec3) = times(Vec3(), this, b)
106+
infix operator fun times(b: Vec4) = times(Vec4(), this, b)
107+
fun times(b: DualQuat, res: DualQuat) = times(res, this, b)
108+
fun times(b: Float, res: DualQuat) = times(res, this, b)
109+
fun times(b: Vec3, res: Vec3) = times(res, this, b)
110+
fun times(b: Vec4, res: Vec4) = times(res, this, b)
111+
infix operator fun timesAssign(b: DualQuat) {
112+
times(this, this, b)
113+
}
114+
115+
infix operator fun timesAssign(b: Float) {
116+
times(this, this, b)
117+
}
118+
119+
infix operator fun timesAssign(b: Vec3) {
120+
times(b, this, b)
121+
}
122+
123+
infix operator fun timesAssign(b: Vec4) {
124+
times(b, this, b)
125+
}
126+
127+
128+
infix operator fun div(b: Float) = div(DualQuat(), this, b)
129+
fun div(b: Float, res: DualQuat) = div(res, this, b)
130+
infix operator fun divAssign(b: Float) {
131+
div(this, this, b)
132+
}
133+
134+
135+
// -- Operations --
136+
137+
/** Set the dual quaternion as identity.
138+
* @see gtx_dual_quaternion */
139+
fun identity() {
140+
real.put(1f, 0f, 0f, 0f)
141+
dual.put(0f, 0f, 0f, 0f)
142+
}
143+
144+
/** Returns a new normalized quaternion.
145+
* @see gtx_dual_quaternion */
146+
fun normalize(): DualQuat = div(real.length(), DualQuat())
147+
148+
/** Normalize the quaternion itself.
149+
* @see gtx_dual_quaternion */
150+
fun normalizeAssign(): DualQuat {
151+
divAssign(real.length())
152+
return this
153+
}
154+
155+
/** Returns the linear interpolation of two dual quaternion.
156+
* @see gtc_dual_quaternion */
157+
fun lerp(b: DualQuat, a: Float, res: DualQuat = DualQuat()): DualQuat {
158+
// Dual Quaternion Linear blend aka DLB:
159+
// Lerp is only defined in [0, 1]
160+
assert(a in 0f..1f)
161+
val k = if (real dot b.real < 0f) -a else a
162+
val j = 1f - a
163+
return res(
164+
real.w * j + b.real.w * k,
165+
real.x * j + b.real.x * k,
166+
real.y * j + b.real.y * k,
167+
real.z * j + b.real.z * k,
168+
dual.z * j + b.dual.z * k,
169+
dual.z * j + b.dual.z * k,
170+
dual.z * j + b.dual.z * k,
171+
dual.z * j + b.dual.z * k)
172+
}
173+
174+
/** Returns the q inverse.
175+
* @see gtx_dual_quaternion */
176+
fun inverse(res: DualQuat = DualQuat()): DualQuat {
177+
// conjugate
178+
val realW = real.w
179+
val realX = -real.x
180+
val realY = -real.y
181+
val realZ = -real.z
182+
// conjugate
183+
val dualW = dual.w
184+
val dualX = -dual.x
185+
val dualY = -dual.y
186+
val dualZ = -dual.z
187+
188+
val dot = realX * dualX + realY * dualY + realZ * dualZ + realW * dualW
189+
val w = dualW + realW * (-2f * dot)
190+
val x = dualX + realX * (-2f * dot)
191+
val y = dualY + realY * (-2f * dot)
192+
val z = dualZ + realZ * (-2f * dot)
193+
return res(realW, realX, realY, realZ, w, x, y, z)
194+
}
195+
196+
/// Converts a quaternion to a 2 * 4 matrix.
197+
///
198+
/// @see gtx_dual_quaternion
199+
// template<typename T, qualifier Q>
200+
// GLM_FUNC_DECL mat<2, 4, T, Q> mat2x4_cast(tdualquat<T, Q> const& x);
201+
202+
/// Converts a quaternion to a 3 * 4 matrix.
203+
///
204+
/// @see gtx_dual_quaternion
205+
// template<typename T, qualifier Q>
206+
// GLM_FUNC_DECL mat<3, 4, T, Q> mat3x4_cast(tdualquat<T, Q> const& x);
207+
208+
/** Converts a 2 * 4 matrix (matrix which holds real and dual parts) to a quaternion.
209+
* @see gtx_dual_quaternion */
210+
fun dualquat_cast(x: Mat2x4, res: DualQuat = DualQuat()): DualQuat = res(x[0].w, x[0].x, x[0].y, x[0].z, x[1].w, x[1].x, x[1].y, x[1].z)
211+
212+
/** Converts a 3 * 4 matrix (augmented matrix rotation + translation) to a quaternion.
213+
* @see gtx_dual_quaternion */
214+
fun dualquat_cast(m: Mat3x4, res: DualQuat = DualQuat()): DualQuat {
215+
216+
val real = Quat()
217+
218+
val trace = m[0, 0] + m[1, 1] + m[2, 2]
219+
when {
220+
trace > 0f -> {
221+
val r = sqrt(1f + trace)
222+
val invR = 0.5f / r
223+
real.w = 0.5f * r
224+
real.x = (m[2, 1] - m[1, 2]) * invR
225+
real.y = (m[0, 2] - m[2, 0]) * invR
226+
real.z = (m[1, 0] - m[0, 1]) * invR
227+
}
228+
m[0, 0] > m[1, 1] && m[0, 0] > m[2, 2] -> {
229+
val r = sqrt(1f + m[0, 0] - m[1, 1] - m[2, 2])
230+
val invR = 0.5f / r
231+
real.x = 0.5f * r
232+
real.y = (m[1, 0] + m[0, 1]) * invR
233+
real.z = (m[0, 2] + m[2, 0]) * invR
234+
real.w = (m[2, 1] - m[1, 2]) * invR
235+
}
236+
m[1, 1] > m[2, 2] -> {
237+
val r = sqrt(1f + m[1, 1] - m[0, 0] - m[2, 2])
238+
val invR = 0.5f / r
239+
real.x = (m[1, 0] + m[0, 1]) * invR
240+
real.y = 0.5f * r
241+
real.z = (m[2, 1] + m[1, 2]) * invR
242+
real.w = (m[0, 2] - m[2, 0]) * invR
243+
}
244+
else -> {
245+
val r = sqrt(1f + m[2, 2] - m[0, 0] - m[1, 1])
246+
val invR = 0.5f / r
247+
real.x = (m[0, 2] + m[2, 0]) * invR
248+
real.y = (m[2, 1] + m[1, 2]) * invR
249+
real.z = 0.5f * r
250+
real.w = (m[1, 0] - m[0, 1]) * invR
251+
}
252+
}
253+
254+
val dual = Quat(
255+
-0.5f * (m[0, 3] * real.x + m[1, 3] * real.y + m[2, 3] * real.z),
256+
0.5f * (m[0, 3] * real.w + m[1, 3] * real.z - m[2, 3] * real.y),
257+
0.5f * (-m[0, 3] * real.z + m[1, 3] * real.w + m[2, 3] * real.x),
258+
0.5f * (m[0, 3] * real.y - m[1, 3] * real.x + m[2, 3] * real.w))
259+
return res(real, dual)
260+
}
261+
262+
companion object : dualQuat_operators() {
263+
@JvmField
264+
val size = Quat.size * 2
265+
266+
/** Creates an identity dual quaternion.
267+
* @see gtx_dual_quaternion */
268+
fun identity() = DualQuat()
269+
}
270+
271+
override fun equals(other: Any?) = other is DualQuat && real == other.real && dual == other.dual
272+
override fun hashCode() = 31 * real.hashCode() + dual.hashCode()
273+
}

0 commit comments

Comments
 (0)