diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
new file mode 100644
index 000000000..7273b6a50
--- /dev/null
+++ b/.github/CODEOWNERS
@@ -0,0 +1,3 @@
+@altavir
+
+/kmath-trajectory @ESchouten
\ No newline at end of file
diff --git a/.space/CODEOWNERS b/.space/CODEOWNERS
new file mode 100644
index 000000000..e69de29bb
diff --git a/README.md b/README.md
index 39cbf19c9..b9d36df50 100644
--- a/README.md
+++ b/README.md
@@ -262,6 +262,11 @@ One can still use generic algebras though.
 > - [linear algebra operations](kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/api/LinearOpsTensorAlgebra.kt) : Advanced linear algebra operations like LU decomposition, SVD, etc.
 
 
+### [kmath-trajectory](kmath-trajectory)
+> Path and trajectory optimization
+>
+> **Maturity**: PROTOTYPE
+
 ### [kmath-viktor](kmath-viktor)
 > 
 >
@@ -288,8 +293,7 @@ performance and flexibility.
 
 We expect to focus on creating convenient universal API first and then work on increasing performance for specific
 cases. We expect the worst KMath benchmarks will perform better than native Python, but worse than optimized
-native/SciPy (mostly due to boxing operations on primitive numbers). The best performance of optimized parts could be
-better than SciPy.
+native/SciPy (mostly due to boxing operations on primitive numbers). The best performance of optimized parts could be better than SciPy.
 
 ## Requirements
 
@@ -321,4 +325,4 @@ Gradle `6.0+` is required for multiplatform artifacts.
 The project requires a lot of additional work. The most important thing we need is a feedback about what features are
 required the most. Feel free to create feature requests. We are also welcome to code contributions, especially in issues
 marked with
-[waiting for a hero](https://github.com/mipt-npm/kmath/labels/waiting%20for%20a%20hero) label.
+[waiting for a hero](https://github.com/mipt-npm/kmath/labels/waiting%20for%20a%20hero) label.
\ No newline at end of file
diff --git a/docs/templates/README-TEMPLATE.md b/docs/templates/README-TEMPLATE.md
index 0ab284ee9..1633f3ff1 100644
--- a/docs/templates/README-TEMPLATE.md
+++ b/docs/templates/README-TEMPLATE.md
@@ -69,8 +69,7 @@ performance and flexibility.
 
 We expect to focus on creating convenient universal API first and then work on increasing performance for specific
 cases. We expect the worst KMath benchmarks will perform better than native Python, but worse than optimized
-native/SciPy (mostly due to boxing operations on primitive numbers). The best performance of optimized parts could be
-better than SciPy.
+native/SciPy (mostly due to boxing operations on primitive numbers). The best performance of optimized parts could be better than SciPy.
 
 ## Requirements
 
@@ -102,4 +101,4 @@ Gradle `6.0+` is required for multiplatform artifacts.
 The project requires a lot of additional work. The most important thing we need is a feedback about what features are
 required the most. Feel free to create feature requests. We are also welcome to code contributions, especially in issues
 marked with
-[waiting for a hero](https://github.com/mipt-npm/kmath/labels/waiting%20for%20a%20hero) label.
+[waiting for a hero](https://github.com/mipt-npm/kmath/labels/waiting%20for%20a%20hero) label.
\ No newline at end of file
diff --git a/kmath-trajectory/README.md b/kmath-trajectory/README.md
new file mode 100644
index 000000000..ac2930b04
--- /dev/null
+++ b/kmath-trajectory/README.md
@@ -0,0 +1,34 @@
+# kmath-trajectory
+
+
+
+
+## Artifact:
+
+The Maven coordinates of this project are `space.kscience:kmath-trajectory:0.3.1-dev-1`.
+
+**Gradle Groovy:**
+```groovy
+repositories {
+    maven { url 'https://repo.kotlin.link' }
+    mavenCentral()
+}
+
+dependencies {
+    implementation 'space.kscience:kmath-trajectory:0.3.1-dev-1'
+}
+```
+**Gradle Kotlin DSL:**
+```kotlin
+repositories {
+    maven("https://repo.kotlin.link")
+    mavenCentral()
+}
+
+dependencies {
+    implementation("space.kscience:kmath-trajectory:0.3.1-dev-1")
+}
+```
+
+## Contributors
+Erik Schouten (github: @ESchouten, email: erik-schouten@hotmail.nl)
diff --git a/kmath-trajectory/build.gradle.kts b/kmath-trajectory/build.gradle.kts
new file mode 100644
index 000000000..db80be02a
--- /dev/null
+++ b/kmath-trajectory/build.gradle.kts
@@ -0,0 +1,17 @@
+plugins {
+    kotlin("multiplatform")
+    id("ru.mipt.npm.gradle.common")
+    id("ru.mipt.npm.gradle.native")
+}
+
+kotlin.sourceSets.commonMain {
+    dependencies {
+        api(projects.kmath.kmathGeometry)
+    }
+}
+
+readme {
+    description = "Path and trajectory optimization"
+    maturity = ru.mipt.npm.gradle.Maturity.PROTOTYPE
+    propertyByTemplate("artifact", rootProject.file("docs/templates/ARTIFACT-TEMPLATE.md"))
+}
diff --git a/kmath-trajectory/docs/README-TEMPLATE.md b/kmath-trajectory/docs/README-TEMPLATE.md
new file mode 100644
index 000000000..eb8e4a0c0
--- /dev/null
+++ b/kmath-trajectory/docs/README-TEMPLATE.md
@@ -0,0 +1,13 @@
+# kmath-trajectory
+
+
+${features}
+
+${artifact}
+
+## Author
+Erik Schouten
+
+Github: ESchouten
+
+Email: erik-schouten@hotmail.nl
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt
new file mode 100644
index 000000000..ee4f38662
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt
@@ -0,0 +1,136 @@
+/*
+ * Copyright 2018-2021 KMath contributors.
+ * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
+ */
+
+package space.kscience.kmath.trajectory.dubins
+
+import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.segments.Arc
+import space.kscience.kmath.trajectory.segments.Segment
+import space.kscience.kmath.trajectory.segments.Straight
+import space.kscience.kmath.trajectory.segments.components.Circle
+import space.kscience.kmath.trajectory.segments.components.Pose2D
+import kotlin.math.acos
+import kotlin.math.cos
+import kotlin.math.sin
+
+public class DubinsPath(
+    public val a: Arc,
+    public val b: Segment,
+    public val c: Arc,
+) {
+
+    public val type: TYPE = TYPE.valueOf(
+        arrayOf(
+            a.direction.name[0],
+            if (b is Arc) b.direction.name[0] else 'S',
+            c.direction.name[0]
+        ).toCharArray().concatToString()
+    )
+
+    public val length: Double = a.length + b.length + c.length
+
+    public enum class TYPE {
+        RLR, LRL, RSR, LSL, RSL, LSR
+    }
+
+    public companion object {
+        public fun all(start: Pose2D, end: Pose2D, turningRadius: Double): List<DubinsPath> =
+            listOfNotNull(
+                rlr(start, end, turningRadius),
+                lrl(start, end, turningRadius),
+                rsr(start, end, turningRadius),
+                lsl(start, end, turningRadius),
+                rsl(start, end, turningRadius),
+                lsr(start, end, turningRadius)
+            )
+        public fun shortest(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath =
+            all(start, end, turningRadius).minByOrNull { it.length }!!
+
+        public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
+            val c1 = start.getRightCircle(turningRadius)
+            val c2 = end.getRightCircle(turningRadius)
+            val centers = Straight(c1.center, c2.center)
+            if (centers.length > turningRadius * 4) return null
+
+            var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
+            var dX = turningRadius * sin(theta)
+            var dY = turningRadius * cos(theta)
+            val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2)
+            val e = Circle(p, turningRadius)
+            val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
+            theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
+            dX = turningRadius * sin(theta)
+            dY = turningRadius * cos(theta)
+            val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
+            val a1 = Arc.of(c1.center, start, p1, Arc.Direction.RIGHT)
+            val a2 = Arc.of(e.center, p1, p2, Arc.Direction.LEFT)
+            val a3 = Arc.of(c2.center, p2, end, Arc.Direction.RIGHT)
+            return DubinsPath(a1, a2, a3)
+        }
+
+        public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
+            val c1 = start.getLeftCircle(turningRadius)
+            val c2 = end.getLeftCircle(turningRadius)
+            val centers = Straight(c1.center, c2.center)
+            if (centers.length > turningRadius * 4) return null
+
+            var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
+            var dX = turningRadius * sin(theta)
+            var dY = turningRadius * cos(theta)
+            val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2)
+            val e = Circle(p, turningRadius)
+            val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
+            theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
+            dX = turningRadius * sin(theta)
+            dY = turningRadius * cos(theta)
+            val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
+            val a1 = Arc.of(c1.center, start, p1, Arc.Direction.LEFT)
+            val a2 = Arc.of(e.center, p1, p2, Arc.Direction.RIGHT)
+            val a3 = Arc.of(c2.center, p2, end, Arc.Direction.LEFT)
+            return DubinsPath(a1, a2, a3)
+        }
+
+        public fun rsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath {
+            val c1 = start.getRightCircle(turningRadius)
+            val c2 = end.getRightCircle(turningRadius)
+            val s = leftOuterTangent(c1, c2)
+            val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
+            val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
+            return DubinsPath(a1, s, a3)
+        }
+
+        public fun lsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath {
+            val c1 = start.getLeftCircle(turningRadius)
+            val c2 = end.getLeftCircle(turningRadius)
+            val s = rightOuterTangent(c1, c2)
+            val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
+            val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
+            return DubinsPath(a1, s, a3)
+        }
+
+        public fun rsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
+            val c1 = start.getRightCircle(turningRadius)
+            val c2 = end.getLeftCircle(turningRadius)
+            val s = rightInnerTangent(c1, c2)
+            if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
+
+            val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
+            val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
+            return DubinsPath(a1, s, a3)
+        }
+
+        public fun lsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
+            val c1 = start.getLeftCircle(turningRadius)
+            val c2 = end.getRightCircle(turningRadius)
+            val s = leftInnerTangent(c1, c2)
+            if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
+
+            val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
+            val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
+            return DubinsPath(a1, s, a3)
+        }
+    }
+}
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt
new file mode 100644
index 000000000..547cb99fc
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt
@@ -0,0 +1,67 @@
+/*
+ * Copyright 2018-2021 KMath contributors.
+ * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
+ */
+
+package space.kscience.kmath.trajectory.dubins
+
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.segments.Straight
+import space.kscience.kmath.trajectory.segments.components.Circle
+import space.kscience.kmath.trajectory.segments.components.Pose2D
+import kotlin.math.PI
+import kotlin.math.acos
+import kotlin.math.cos
+import kotlin.math.sin
+
+private enum class SIDE {
+    LEFT, RIGHT
+}
+
+internal fun Pose2D.getLeftCircle(radius: Double): Circle = getTangentCircles(radius).first
+internal fun Pose2D.getRightCircle(radius: Double): Circle = getTangentCircles(radius).second
+internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle, Circle> {
+    val dX = radius * cos(theta)
+    val dY = radius * sin(theta)
+    return Circle(Vector2D(x - dX, y + dY), radius) to Circle(Vector2D(x + dX, y - dY), radius)
+}
+
+internal fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT)
+internal fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT)
+private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight {
+    val centers = Straight(a.center, b.center)
+    val p1 = when (side) {
+        SIDE.LEFT -> Vector2D(
+            a.center.x - a.radius * cos(centers.theta),
+            a.center.y + a.radius * sin(centers.theta)
+        )
+        SIDE.RIGHT -> Vector2D(
+            a.center.x + a.radius * cos(centers.theta),
+            a.center.y - a.radius * sin(centers.theta)
+        )
+    }
+    return Straight(
+        p1,
+        Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
+    )
+}
+
+internal fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT)
+internal fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT)
+private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight? {
+    val centers = Straight(base.center, direction.center)
+    if (centers.length < base.radius * 2) return null
+    val angle = theta(
+        when (side) {
+            SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length)
+            SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length)
+        }
+    )
+    val dX = base.radius * sin(angle)
+    val dY = base.radius * cos(angle)
+    val p1 = Vector2D(base.center.x + dX, base.center.y + dY)
+    val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY)
+    return Straight(p1, p2)
+}
+
+internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI)
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt
new file mode 100644
index 000000000..1c02dd952
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt
@@ -0,0 +1,59 @@
+package space.kscience.kmath.trajectory.segments
+
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.dubins.theta
+import space.kscience.kmath.trajectory.segments.components.Circle
+import space.kscience.kmath.trajectory.segments.components.Pose2D
+import kotlin.math.PI
+
+public data class Arc(
+    public val circle: Circle,
+    public val start: Pose2D,
+    public val end: Pose2D
+) : Segment {
+
+    internal companion object {
+        fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): Arc {
+            val s1 = Straight(center, start)
+            val s2 = Straight(center, end)
+            val pose1 = calculatePose(start, s1.theta, direction)
+            val pose2 = calculatePose(end, s2.theta, direction)
+            return Arc(Circle(center, s1.length), pose1, pose2)
+        }
+
+        private fun calculatePose(vector: Vector2D, theta: Double, direction: Direction): Pose2D =
+            Pose2D.of(
+                vector,
+                when (direction) {
+                    Direction.LEFT -> theta(theta - PI / 2)
+                    Direction.RIGHT -> theta(theta + PI / 2)
+                }
+            )
+    }
+
+    internal enum class Direction {
+        LEFT, RIGHT
+    }
+
+    override val length: Double
+        get() {
+            val angle: Double =
+                theta(if (direction == Direction.LEFT) start.theta - end.theta else end.theta - start.theta)
+            val proportion = angle / (2 * PI)
+            return circle.circumference * proportion
+        }
+
+    internal val direction: Direction
+        get() = if (start.y < circle.center.y) {
+            if (start.theta > PI) Direction.RIGHT else Direction.LEFT
+        } else if (start.y > circle.center.y) {
+            if (start.theta < PI) Direction.RIGHT else Direction.LEFT
+        } else {
+            if (start.theta == 0.0) {
+                if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
+            } else {
+                if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
+            }
+        }
+
+}
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt
new file mode 100644
index 000000000..8a1d086fc
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt
@@ -0,0 +1,5 @@
+package space.kscience.kmath.trajectory.segments
+
+public interface Segment {
+    public val length: Double
+}
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt
new file mode 100644
index 000000000..444025d83
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt
@@ -0,0 +1,18 @@
+package space.kscience.kmath.trajectory.segments
+
+import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.dubins.theta
+import kotlin.math.PI
+import kotlin.math.atan2
+
+public data class Straight(
+    internal val start: Vector2D,
+    internal val end: Vector2D
+) : Segment {
+    override val length: Double
+        get() = start.distanceTo(end)
+
+    internal val theta: Double
+        get() = theta(atan2(end.x - start.x, end.y - start.y))
+}
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt
new file mode 100644
index 000000000..946dd8c6e
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt
@@ -0,0 +1,11 @@
+package space.kscience.kmath.trajectory.segments.components
+
+import space.kscience.kmath.geometry.Vector2D
+import kotlin.math.PI
+
+public open class Circle(
+    internal val center: Vector2D,
+    internal val radius: Double
+) {
+    internal val circumference = radius * 2 * PI
+}
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
new file mode 100644
index 000000000..c49da3187
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
@@ -0,0 +1,13 @@
+package space.kscience.kmath.trajectory.segments.components
+
+import space.kscience.kmath.geometry.Vector2D
+
+public data class Pose2D(
+    override val x: Double,
+    override val y: Double,
+    public val theta: Double
+) : Vector2D {
+    internal companion object {
+        internal fun of(vector: Vector2D, theta: Double) = Pose2D(vector.x, vector.y, theta)
+    }
+}
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
new file mode 100644
index 000000000..92b2f1df9
--- /dev/null
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
@@ -0,0 +1,26 @@
+package space.kscience.kmath.trajectory
+
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.segments.Straight
+import space.kscience.kmath.trajectory.segments.components.Pose2D
+import kotlin.math.PI
+import kotlin.math.abs
+import kotlin.math.sin
+
+const val maxFloatDelta = 0.000001
+
+fun Double.radiansToDegrees() = this * 180 / PI
+
+fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta
+fun Pose2D.equalsFloat(other: Pose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && theta.equalFloat(other.theta)
+
+fun Straight.inverse() = Straight(end, start)
+fun Straight.shift(shift: Int, width: Double): Straight {
+    val dX = width * sin(inverse().theta)
+    val dY = width * sin(theta)
+
+    return Straight(
+        Vector2D(start.x - dX * shift, start.y - dY * shift),
+        Vector2D(end.x - dX * shift, end.y - dY * shift)
+    )
+}
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
new file mode 100644
index 000000000..47e6ac2ef
--- /dev/null
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
@@ -0,0 +1,66 @@
+/*
+ * Copyright 2018-2021 KMath contributors.
+ * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
+ */
+
+package space.kscience.kmath.trajectory.dubins
+
+import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.equalFloat
+import space.kscience.kmath.trajectory.equalsFloat
+import space.kscience.kmath.trajectory.inverse
+import space.kscience.kmath.trajectory.segments.Arc
+import space.kscience.kmath.trajectory.segments.Straight
+import space.kscience.kmath.trajectory.segments.components.Pose2D
+import space.kscience.kmath.trajectory.shift
+import kotlin.test.Test
+import kotlin.test.assertNotNull
+import kotlin.test.assertTrue
+
+
+class DubinsTests {
+
+    @Test
+    fun dubinsTest() {
+        val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
+        val lineP1 = straight.shift(1, 10.0).inverse()
+
+        val start = Pose2D.of(straight.end, straight.theta)
+        val end = Pose2D.of(lineP1.start, lineP1.theta)
+        val radius = 2.0
+        val dubins = DubinsPath.all(start, end, radius)
+
+        val absoluteDistance = start.distanceTo(end)
+        println("Absolute distance: $absoluteDistance")
+
+        val expectedLengths = mapOf(
+            DubinsPath.TYPE.RLR to 13.067681939031397,
+            DubinsPath.TYPE.RSR to 12.28318530717957,
+            DubinsPath.TYPE.LSL to 32.84955592153878,
+            DubinsPath.TYPE.RSL to 23.37758938854081,
+            DubinsPath.TYPE.LSR to 23.37758938854081
+        )
+
+        expectedLengths.forEach {
+            val path = dubins.find { p -> p.type === it.key }
+            assertNotNull(path, "Path ${it.key} not found")
+            println("${it.key}: ${path.length}")
+            assertTrue(it.value.equalFloat(path.length))
+
+            assertTrue(start.equalsFloat(path.a.start))
+            assertTrue(end.equalsFloat(path.c.end))
+
+            // Not working, theta double precision inaccuracy
+            if (path.b is Arc) {
+                val b = path.b as Arc
+                assertTrue(path.a.end.equalsFloat(b.start))
+                assertTrue(path.c.start.equalsFloat(b.end))
+            } else if (path.b is Straight) {
+                val b = path.b as Straight
+                assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta)))
+                assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
+            }
+        }
+    }
+}
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
new file mode 100644
index 000000000..5b4ae6d7a
--- /dev/null
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
@@ -0,0 +1,19 @@
+package space.kscience.kmath.trajectory.segments
+
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.radiansToDegrees
+import space.kscience.kmath.trajectory.segments.components.Circle
+import kotlin.test.Test
+import kotlin.test.assertEquals
+
+class ArcTests {
+
+    @Test
+    fun arcTest() {
+        val circle = Circle(Vector2D(0.0, 0.0), 2.0)
+        val arc = Arc.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT)
+        assertEquals(circle.circumference / 4, arc.length, 1.0)
+        assertEquals(0.0, arc.start.theta.radiansToDegrees())
+        assertEquals(90.0, arc.end.theta.radiansToDegrees())
+    }
+}
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
new file mode 100644
index 000000000..e8184e178
--- /dev/null
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
@@ -0,0 +1,32 @@
+package space.kscience.kmath.trajectory.segments
+
+import space.kscience.kmath.geometry.Euclidean2DSpace
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.radiansToDegrees
+import kotlin.math.pow
+import kotlin.math.sqrt
+import kotlin.test.Test
+import kotlin.test.assertEquals
+
+class LineTests {
+
+    @Test
+    fun lineTest() {
+        val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
+        assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length)
+        assertEquals(45.0, straight.theta.radiansToDegrees())
+    }
+
+    @Test
+    fun lineAngleTest() {
+        val zero = Vector2D(0.0, 0.0)
+        val north = Straight(Euclidean2DSpace.zero, Vector2D(0.0, 2.0))
+        assertEquals(0.0, north.theta.radiansToDegrees())
+        val east = Straight(Euclidean2DSpace.zero, Vector2D(2.0, 0.0))
+        assertEquals(90.0, east.theta.radiansToDegrees())
+        val south = Straight(Euclidean2DSpace.zero, Vector2D(0.0, -2.0))
+        assertEquals(180.0, south.theta.radiansToDegrees())
+        val west = Straight(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0))
+        assertEquals(270.0, west.theta.radiansToDegrees())
+    }
+}
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt
new file mode 100644
index 000000000..6f28885e0
--- /dev/null
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt
@@ -0,0 +1,23 @@
+/*
+ * Copyright 2018-2021 KMath contributors.
+ * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
+ */
+
+package space.kscience.kmath.trajectory.segments.components
+
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.maxFloatDelta
+import kotlin.test.Test
+import kotlin.test.assertEquals
+
+class CircleTests {
+
+    @Test
+    fun arcTest() {
+        val center = Vector2D(0.0, 0.0)
+        val radius = 2.0
+        val expectedCircumference = 12.56637
+        val circle = Circle(center, radius)
+        assertEquals(expectedCircumference, circle.circumference, maxFloatDelta)
+    }
+}
diff --git a/settings.gradle.kts b/settings.gradle.kts
index 336e1c36b..829e59144 100644
--- a/settings.gradle.kts
+++ b/settings.gradle.kts
@@ -45,6 +45,7 @@ include(
     ":kmath-jupyter",
     ":kmath-symja",
     ":kmath-jafama",
+    ":kmath-trajectory",
     ":examples",
     ":benchmarks",
-)
\ No newline at end of file
+)