提供变换等价性判定；删除里程接口，改为扩展函数；改正一处旋转顺序问题

src/main/kotlin/org/mechdancer/algebra/Utility.kt

@@ -3,7 +3,7 @@ package org.mechdancer.algebra
 /**
  * 双精度浮点的精度
  */
-internal const val DOUBLE_PRECISION = 5E-15
+internal const val DOUBLE_PRECISION = 1E-12
 
 // 精度范围
 internal val DOUBLE_EQUALS_RANGE = -DOUBLE_PRECISION..DOUBLE_PRECISION

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

@@ -24,6 +24,18 @@ import kotlin.math.abs
 fun MatrixTransformation.transform(pose: Pose2D) =
     Pose2D(invoke(pose.p).to2D(), invokeLinear(pose.d.toVector()).to2D().toAngle())
 
+/** 增量 [delta] 累加到里程 */
+infix fun <T : Transformation<T>> T.plusDelta(delta: T): T =
+    this * delta
+
+/** 里程回滚到增量 [delta] 之前 */
+infix fun <T : Transformation<T>> T.minusDelta(delta: T) =
+    this * delta.inverse()
+
+/** 计算里程从起点 [origin] 到当前状态的增量 */
+infix fun <T : Transformation<T>> T.minusState(origin: T) =
+    origin.inverse() * this
+
 /**
  * 最小二乘法从点对集推算空间变换子
  * 任意维无约束

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

@@ -27,6 +27,9 @@ data class MatrixTransformation(val matrix: Matrix) : Transformation<MatrixTrans
     override fun inverse(): MatrixTransformation =
         MatrixTransformation(matrix.inverse())
 
+    override fun equivalentWith(others: MatrixTransformation) =
+        matrix == others.matrix
+
     init {
         require(dim > 0) { "transform matrix must be square" }
     }

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

@@ -17,7 +17,7 @@ import org.mechdancer.geometry.angle.unaryMinus
  * @param d 方向
  */
 data class Pose2D(val p: Vector2D, val d: Angle) :
-    Transformation<Pose2D>, Odometry<Pose2D> {
+    Transformation<Pose2D> {
     override val dim: Int get() = 2
 
     override fun times(p: Vector): Vector2D {
@@ -31,14 +31,9 @@ data class Pose2D(val p: Vector2D, val d: Angle) :
     override fun inverse(): Pose2D =
         Pose2D(-p rotate -d, -d)
 
-    override fun plusDelta(delta: Pose2D) =
-        this * delta
-
-    override fun minusDelta(delta: Pose2D) =
-        this * delta.inverse()
-
-    override fun minusState(mark: Pose2D) =
-        mark.inverse() * this
+    override fun equivalentWith(others: Pose2D) =
+        this === others
+        || p == others.p && d.adjust() == others.d.adjust()
 
     override fun toString() = "(${p.x}, ${p.y})($d)"
 }

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

@@ -18,7 +18,7 @@ import kotlin.math.*
  * @param d 方向
  */
 data class Pose3D(val p: Vector3D, val d: Vector3D)
-    : Transformation<Pose3D>, Odometry<Pose3D> {
+    : Transformation<Pose3D> {
     /*
      存储方案是这样设计的：
      p 表示位置，没什么可说的；
@@ -46,33 +46,13 @@ data class Pose3D(val p: Vector3D, val d: Vector3D)
     override fun inverse(): Pose3D =
         Pose3D(-p rotateVector -d, -d)
 
-    /** 增量 [delta] 累加到里程 */
-    override fun plusDelta(delta: Pose3D) =
-        this * delta
-
-    /** 里程回滚到增量 [delta] 之前 */
-    override fun minusDelta(delta: Pose3D) =
-        this * delta.inverse()
-
-    /** 计算里程从标记 [mark] 到当前状态的增量 */
-    override fun minusState(mark: Pose3D) =
-        mark.inverse() * this
-
     override fun toString() = "p = ${p.simpleView()}, u = ${u.simpleView()}, θ = $theta"
 
-    infix fun equivalentWith(others: Pose3D): Boolean {
-        if (this === others) return true
-        if (p != others.p) return false
-        if (d == others.d) return true
-        val axis0 = d.normalize()
-        val axis1 = others.d.normalize()
-        val delta = abs(when (axis0) {
-                            axis1  -> others.d.length
-                            -axis1 -> others.d.length + PI / 2
-                            else   -> return false
-                        } - d.length)
-        return (delta / PI).let { doubleEquals(it.toInt().toDouble(), it) }
-    }
+    override fun equivalentWith(others: Pose3D): Boolean =
+        this === others
+        || (p == others.p
+            && (d == others.d
+                || d.asAngle() == others.d.asAngle()))
 
     private companion object {
         fun Vector3D.simpleView() = "($x, $y, $z)"
@@ -91,11 +71,13 @@ data class Pose3D(val p: Vector3D, val d: Vector3D)
 
         infix fun Vector3D.rotateVector(d: Vector3D): Vector3D {
             val q = d.asAngle()
-            return (q * asPosition() * q.conjugate).v
+            val result = q * asPosition() * q.conjugate
+            require(doubleEquals(result.r, .0))
+            return result.v
         }
 
         infix fun Vector3D.rotateAngle(d: Vector3D): Vector3D {
-            val q = d.asAngle() * asAngle()
+            val q = asAngle() * d.asAngle()
             val r = q.r
             val v = q.v
             return v.normalize() * atan2(v.length, r)

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

@@ -1,5 +1,7 @@
 package org.mechdancer.geometry.transformation
 
+import org.mechdancer.algebra.doubleEquals
+import org.mechdancer.algebra.hash
 import org.mechdancer.algebra.implement.vector.vector3D
 import kotlin.math.sqrt
 
@@ -57,4 +59,15 @@ data class Quaternion(
             c * e + d * f + a * g - b * h,
             d * e - c * f + b * g + a * h)
     }
+
+    override fun equals(other: Any?) =
+        this === other
+        || (other is Quaternion
+            && doubleEquals(a, other.a)
+            && doubleEquals(b, other.b)
+            && doubleEquals(c, other.c)
+            && doubleEquals(d, other.d))
+
+    override fun hashCode() =
+        hash(a, b, c, d)
 }

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

@@ -40,4 +40,9 @@ interface Transformation<T : Transformation<T>> {
      * @return `A` -> `B`
      */
     fun inverse(): T
+
+    /**
+     * 等价性判断
+     */
+    infix fun equivalentWith(others: T): Boolean
 }

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

@@ -18,12 +18,14 @@ class TestTransformation {
 
     @Test
     fun testPose3D() {
-        val a = pose3D(+1, -2, +3, -4/*, +5, -6*/)
-        val b = pose3D(+6, -5, +4/*, -3, +2, -1*/)
+        val a = pose3D(+1, -2, +3, -4, +5, -6)
+        val b = pose3D(+6, -5, +4, -3, +2, -1)
         val c = a * b
         assertEquals(pose3D(), a * a.inverse())
         assertEquals(pose3D(), b * b.inverse())
 
+        println(a)
+        println(c * b.inverse())
         assert(a equivalentWith c * b.inverse())
         assert(b equivalentWith a.inverse() * c)
     }