修改 Pose2D 转 Matrix 的错误

Author: YdrMaster

build.gradle.kts

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

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

@@ -42,7 +42,7 @@ fun Pose2D.to3D(): Pose3D =
 fun MatrixTransformation.toPose2D(): Pose2D {
     require(dim == 2) { "2d transformation is required" }
     return Pose2D(vector2D(matrix[0, 2], matrix[1, 2]),
-                  atan2(matrix[0, 0], matrix[1, 0]).toRad())
+                  atan2(matrix[1, 0], matrix[0, 0]).toRad())
 }
 
 fun Pose2D.toMatrixTransformation(): MatrixTransformation {

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

@@ -5,6 +5,7 @@ import org.mechdancer.algebra.implement.vector.vector2D
 import org.mechdancer.algebra.implement.vector.vector2DOfZero
 import org.mechdancer.geometry.angle.toRad
 import kotlin.math.PI
+import kotlin.test.assertEquals
 
 class TestPose2D {
     /**
@@ -37,4 +38,10 @@ class TestPose2D {
             "里程计标记错误：$step4 ≠ $delta2"
         }
     }
+
+    @Test
+    fun testToMatrix() {
+        val a = pose2D(1, -2, 0)
+        assertEquals(a, a.toMatrixTransformation().toPose2D())
+    }
 }