实现并测试二维和三维位姿

Author: YdrMaster

build.gradle.kts

@@ -8,7 +8,7 @@ plugins {
 }
 
 group = "org.mechdancer"
-version = "0.2.6-dev-2"
+version = "0.2.7-dev-1"
 
 repositories {
     mavenCentral()

src/main/kotlin/org/mechdancer/geometry/rotation3d/Angle3D.kt

@@ -461,5 +461,4 @@ data class Angle3D(val first: Angle, val second: Angle, val third: Angle, val ax
 
     override fun equals(other: Any?): Boolean =
         other is Angle3D && other.matrix == matrix
-
-}
+}

src/main/kotlin/org/mechdancer/geometry/rotation3d/AxesOrder.kt

@@ -27,5 +27,4 @@ enum class AxesOrder(val first: Int, val second: Int, val third: Int) {
             values().find { it.first == f && it.second == s && it.third == t }
                 ?: throw IllegalArgumentException()
     }
-
-}
+}

src/main/kotlin/org/mechdancer/geometry/transformation/Builder.kt

@@ -1,13 +1,13 @@
 package org.mechdancer.geometry.transformation
 
-import org.mechdancer.algebra.function.vector.times
 import org.mechdancer.algebra.implement.matrix.builder.matrix
-import org.mechdancer.algebra.implement.vector.*
-import org.mechdancer.geometry.angle.toAngle
+import org.mechdancer.algebra.implement.vector.Vector3D
+import org.mechdancer.algebra.implement.vector.vector2DOf
+import org.mechdancer.algebra.implement.vector.vector3DOf
+import org.mechdancer.algebra.implement.vector.vector3DOfZero
 import org.mechdancer.geometry.angle.toRad
 import org.mechdancer.geometry.angle.toVector
-import kotlin.math.cos
-import kotlin.math.sin
+import kotlin.math.atan2
 
 fun pose2D(x: Number = 0, y: Number = 0, theta: Number = 0) =
     Pose2D(vector2DOf(x, y), theta.toRad())
@@ -26,9 +26,8 @@ fun quaternion(r: Number = 0, v: Vector3D = vector3DOfZero()) =
 
 fun Transformation.toPose2D(): Pose2D {
     require(dim == 2) { "2d transformation is required" }
-    val p = invoke(vector2DOfZero()).to2D()
-    val d = invokeLinear(.0.toRad().toVector()).to2D().toAngle()
-    return Pose2D(p, d)
+    return Pose2D(vector2DOf(matrix[0, 2], matrix[1, 2]),
+                  atan2(matrix[0, 0], matrix[1, 0]).toRad())
 }
 
 fun Pose2D.toTransformation(): Transformation {
@@ -42,38 +41,6 @@ fun Pose2D.toTransformation(): Transformation {
         })
 }
 
-fun Transformation.toPose3D(): Pose3D {
-    require(dim == 3) { "3d transformation is required" }
-    val p = invoke(vector3DOfZero()).to3D()
-    TODO()
-}
-
-fun Pose3D.toTransformation(): Transformation {
-    val (x, y, z) = p
-    val half = theta.asRadian() / 2
-    val a = cos(half)
-    val (b, c, d) = u * sin(half)
-
-    val ab = a * b
-    val ac = a * c
-    val ad = a * d
-    val bc = b * c
-    val bd = b * d
-    val cd = c * d
-
-    val b2 = b * b
-    val c2 = c * c
-    val d2 = d * d
-
-    return Transformation(
-        matrix {
-            row(1 - 2 * c2 - 2 * d2, 2 * bc - 2 * ad, 2 * ac + 2 * bd, x)
-            row(2 * bc + 2 * ad, 1 - 2 * b2 - 2 * d2, 2 * cd - 2 * ab, y)
-            row(2 * bd - 2 * ac, 2 * ab + 2 * cd, 1 - 2 * b2 - 2 * c2, z)
-            row(0, 0, 0, 1)
-        })
-}
-
 fun Pose2D.to3D(): Pose3D =
     Pose3D(p = vector3DOf(p.x, p.y, 0),
            d = vector3DOf(0, 0, d.asRadian() / 2))

src/main/kotlin/org/mechdancer/geometry/transformation/Pose3D.kt

@@ -57,10 +57,10 @@ data class Pose3D(
                       q1 * q0.conjugate)
     }
 
-    override fun toString() = "p = ${p.simpleString()}, u = ${u.simpleString()}, θ = $theta"
+    override fun toString() = "p = ${p.simpleView()}, u = ${u.simpleView()}, θ = $theta"
 
     private companion object {
-        fun Vector3D.simpleString() = "($x, $y, $z)"
+        fun Vector3D.simpleView() = "($x, $y, $z)"
 
         fun Pose3D.toQuaternions(): Pair<Quaternion, Quaternion> {
             val (x, y, z) = p
@@ -71,9 +71,7 @@ data class Pose3D(
 
         fun pose3D(p: Quaternion, d: Quaternion): Pose3D {
             assert(doubleEquals(p.r, .0))
-            val u = d.v
-            val half = atan2(u.length, d.r)
-            return Pose3D(p.v, u.normalize() * half)
+            return Pose3D(p.v, d.v.run { normalize() * atan2(length, d.r) })
         }
     }
 }

src/test/kotlin/org/mechdancer/geometry/transformation/TestPose3D.kt

@@ -7,9 +7,7 @@ import org.mechdancer.geometry.angle.toRad
 import kotlin.math.PI
 
 class TestPose3D {
-    /**
-     * 测试里程计算法
-     */
+    /** 测试三维里程算法 */
     @Test
     fun test() {
         val step0 = pose3D()