From 35e5b64606d9123ce8ad9c0afb6778d16e43c55d Mon Sep 17 00:00:00 2001
From: DBotThePony <dbotthepony@yandex.ru>
Date: Mon, 21 Feb 2022 12:16:42 +0700
Subject: [PATCH] Going up the stairs again

---
 .../ru/dbotthepony/kbox2d/api/IProxieable.kt  |   5 +-
 .../kotlin/ru/dbotthepony/kbox2d/api/Math.kt  |  73 ++++++---
 .../dbotthepony/kbox2d/collision/Distance.kt  |  12 +-
 .../kbox2d/collision/TimeOfImpact.kt          |  14 +-
 .../ru/dbotthepony/kbox2d/dynamics/B2World.kt |   4 +-
 .../kbox2d/dynamics/ContactManager.kt         |  13 +-
 .../kotlin/ru/dbotthepony/kstarbound/Main.kt  |   8 +-
 .../kstarbound/client/ClientSettings.kt       |   2 +-
 .../kstarbound/client/StarboundClient.kt      |   2 +-
 .../dbotthepony/kstarbound/util/Formatter.kt  |   1 +
 .../kstarbound/world/entities/AliveEntity.kt  | 138 ++++++++++++++++++
 .../kstarbound/world/entities/PlayerEntity.kt |  52 +++----
 12 files changed, 243 insertions(+), 81 deletions(-)

diff --git a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/IProxieable.kt b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/IProxieable.kt
index 1af78907..0c70a305 100644
--- a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/IProxieable.kt
+++ b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/IProxieable.kt
@@ -6,6 +6,9 @@ import ru.dbotthepony.kbox2d.collision.DynamicTree
 import ru.dbotthepony.kbox2d.collision.BroadPhase
 
 fun interface ProxyQueryCallback {
+	/**
+	 * Return false to terminate query.
+	 */
 	fun invoke(nodeId: Int, userData: Any?): Boolean
 }
 
@@ -61,7 +64,7 @@ interface IProxieable {
 	 * Query an AABB for overlapping proxies. The callback class
 	 * is called for each proxy that overlaps the supplied AABB.
 	 *
-	 * @return Whenever callback returned false
+	 * @return Whenever callback terminated query early
 	 */
 	fun query(aabb: AABB, callback: ProxyQueryCallback): Boolean
 
diff --git a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Math.kt b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Math.kt
index c0221bc2..95ed919a 100644
--- a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Math.kt
+++ b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Math.kt
@@ -1,3 +1,6 @@
+
+@file:Suppress("unused")
+
 package ru.dbotthepony.kbox2d.api
 
 import ru.dbotthepony.kvector.api.concrete.IMatrix2d
@@ -130,8 +133,10 @@ class Rotation(
 	}
 }
 
-/// A transform contains translation and rotation. It is used to represent
-/// the position and orientation of rigid frames.
+/**
+ * A transform contains translation and rotation. It is used to represent
+ * the position and orientation of rigid frames.
+ */
 class Transform(
 	position: Vector2d,
 	val rotation: Rotation,
@@ -200,13 +205,17 @@ class Transform(
 	}
 }
 
-/** This describes the motion of a body/shape for TOI computation.
+/**
+ * This describes the motion of a body/shape for TOI computation.
  * Shapes are defined with respect to the body origin, which may
  * no coincide with the center of mass. However, to support dynamics
  * we must interpolate the center of mass position.
  */
 class Sweep {
-	var localCenter: Vector2d = Vector2d.ZERO ///< local center of mass position
+	/**
+	 * local center of mass position
+	 */
+	var localCenter: Vector2d = Vector2d.ZERO
 		set(value) {
 			if (!value.isFinite) {
 				throw IllegalArgumentException("Tried to set illegal local center $value")
@@ -215,7 +224,10 @@ class Sweep {
 			field = value
 		}
 
-	var c0: Vector2d = Vector2d.ZERO ///< center world positions
+	/**
+	 * center world positions
+	 */
+	var c0: Vector2d = Vector2d.ZERO
 		set(value) {
 			if (!value.isFinite) {
 				throw IllegalArgumentException("Tried to set illegal center world position $value")
@@ -224,7 +236,10 @@ class Sweep {
 			field = value
 		}
 
-	var c: Vector2d = Vector2d.ZERO ///< center world positions
+	/**
+	 * center world positions
+	 */
+	var c: Vector2d = Vector2d.ZERO
 		set(value) {
 			if (!value.isFinite) {
 				throw IllegalArgumentException("Tried to set illegal center world position $value")
@@ -233,7 +248,13 @@ class Sweep {
 			field = value
 		}
 
-	var a0: Double = 0.0 ///< world angles
+	var oldCenter by this::c0
+	var newCenter by this::c
+
+	/**
+	 * world angles
+	 */
+	var a0: Double = 0.0
 		set(value) {
 			if (!value.isFinite()) {
 				throw IllegalArgumentException("Tried to set illegal non finite $value")
@@ -246,7 +267,10 @@ class Sweep {
 			field = value
 		}
 
-	var a: Double = 0.0 ///< world angles
+	/**
+	 * world angles
+	 */
+	var a: Double = 0.0
 		set(value) {
 			if (!value.isFinite()) {
 				throw IllegalArgumentException("Tried to set illegal non finite $value")
@@ -259,8 +283,13 @@ class Sweep {
 			field = value
 		}
 
-	/// Fraction of the current time step in the range [0,1]
-	/// c0 and a0 are the positions at alpha0.
+	var oldAngles by this::a0
+	var newAngles by this::a
+
+	/**
+	 * Fraction of the current time step in the range [0,1]
+	 * [c0] and [a0] are the positions at alpha0.
+	 */
 	var alpha0: Double = 0.0
 		set(value) {
 			if (!value.isFinite()) {
@@ -276,24 +305,30 @@ class Sweep {
 
 	/** Get the interpolated transform at a specific time.
 	 * @param transform the output transform
-	 * @param beta is a factor in [0,1], where 0 indicates alpha0.
+	 * @param beta is a factor in [0,1], where 0 indicates [alpha0].
 	 * https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
 	 */
-	fun getTransform(loadInto: Transform, beta: Double) {
-		loadInto.position = c0 * (1.0 - beta) + c * beta
+	fun getTransform(transform: Transform, beta: Double) {
+		transform.position = c0 * (1.0 - beta) + c * beta
 		val angle = (1.0 - beta) * a0 + beta * a
-		loadInto.rotation.set(angle)
-		loadInto.position -= loadInto.position * localCenter
+		transform.rotation.set(angle)
+		transform.position -= transform.position * localCenter
 	}
 
+	/** Get the interpolated transform at a specific time.
+	 * @param beta is a factor in [0,1], where 0 indicates [alpha0].
+	 * https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
+	 */
 	fun getTransform(beta: Double): Transform {
 		val v = Transform()
 		getTransform(v, beta)
 		return v
 	}
 
-	/// Advance the sweep forward, yielding a new initial state.
-	/// @param alpha the new initial time.
+	/**
+	 * Advance the sweep forward, yielding a new initial state.
+	 * @param alpha the new initial time.
+	 */
 	fun advance(alpha: Double) {
 		require(alpha < 1.0) { "Bad advance value $alpha" }
 
@@ -303,7 +338,9 @@ class Sweep {
 		alpha0 = alpha
 	}
 
-	/// Normalize the angles.
+	/**
+	 * Normalize the angles.
+	 */
 	fun normalize() {
 		val d = floor(a0 / (PI * 2.0)) * 2.0 * PI
 		a0 -= d
diff --git a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/Distance.kt b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/Distance.kt
index 612b49de..2c224a0d 100644
--- a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/Distance.kt
+++ b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/Distance.kt
@@ -7,6 +7,7 @@ import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape
 import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
 import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 import ru.dbotthepony.kvector.vector.ndouble.times
+import java.util.*
 
 var b2_gjkCalls = 0
 	private set
@@ -21,11 +22,11 @@ var b2_gjkMaxIters = 0
  * A distance proxy is used by the GJK algorithm.
  * It encapsulates any shape.
  */
-class DistanceProxy {
+class DistanceProxy(shape: IShape<*>, index: Int) {
 	val vertices: List<Vector2d>
 	val radius: Double
 
-	constructor(shape: IShape<*>, index: Int) {
+	init {
 		when (shape.type) {
 			IShape.Type.CIRCLE -> {
 				val circle = shape as CircleShape
@@ -41,7 +42,7 @@ class DistanceProxy {
 
 			IShape.Type.POLYGON -> {
 				val polygon = shape as PolygonShape
-				vertices = polygon.vertices
+				vertices = Collections.unmodifiableList(polygon.vertices)
 				radius = polygon.radius
 			}
 
@@ -62,11 +63,6 @@ class DistanceProxy {
 		}
 	}
 
-	constructor(vertices: List<Vector2d>, radius: Double) {
-		this.vertices = vertices
-		this.radius = radius
-	}
-
 	/**
 	 * Get the supporting vertex index in the given direction.
 	 */
diff --git a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/TimeOfImpact.kt b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/TimeOfImpact.kt
index 9c075bb8..68b92adb 100644
--- a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/TimeOfImpact.kt
+++ b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/TimeOfImpact.kt
@@ -230,20 +230,22 @@ const val k_maxIterations = 20
 fun b2TimeOfImpact(
 	proxyA: DistanceProxy,
 	proxyB: DistanceProxy,
-	_sweepA: Sweep,
-	_sweepB: Sweep,
+	sweepA: Sweep,
+	sweepB: Sweep,
 	tMax: Double,		// defines sweep interval [0, tMax]
 ): TOIOutput {
-	var timer = System.nanoTime()
+	val timer = System.nanoTime()
 
 	b2_toiCalls++
 
 	var state = TOIOutput.State.UNKNOWN
 	var t = tMax
 
-	// TODO
-	val sweepA = _sweepA.copy()
-	val sweepB = _sweepB.copy()
+	@Suppress("name_shadowing")
+	val sweepA = sweepA.copy()
+
+	@Suppress("name_shadowing")
+	val sweepB = sweepB.copy()
 
 	sweepA.normalize()
 	sweepB.normalize()
diff --git a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/B2World.kt b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/B2World.kt
index f4a70c71..be0a8f47 100644
--- a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/B2World.kt
+++ b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/B2World.kt
@@ -599,8 +599,8 @@ class B2World(
 					val output = b2TimeOfImpact(
 						proxyA = DistanceProxy(fA.shape, indexA),
 						proxyB = DistanceProxy(fB.shape, indexB),
-						_sweepA = bA.sweep,
-						_sweepB = bB.sweep,
+						sweepA = bA.sweep,
+						sweepB = bB.sweep,
 						tMax = 1.0
 					)
 
diff --git a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/ContactManager.kt b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/ContactManager.kt
index 47a209fe..5145faec 100644
--- a/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/ContactManager.kt
+++ b/src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/ContactManager.kt
@@ -44,8 +44,8 @@ class ContactManager {
 
 		// Remove from the world.
 		run {
-			val prev = contact.prev as AbstractContact?
-			val next = contact.next as AbstractContact?
+			val prev = contact.prev
+			val next = contact.next
 
 			prev?.next = next
 			next?.prev = prev
@@ -93,7 +93,6 @@ class ContactManager {
 		// Update awake contacts.
 
 		for (c in contactListIterator) {
-			c as AbstractContact
 			val fixtureA = c.fixtureA
 			val fixtureB = c.fixtureB
 			val indexA = c.childIndexA
@@ -119,8 +118,8 @@ class ContactManager {
 				c.isFlaggedForFiltering = false
 			}
 
-			val activeA = bodyA.isAwake && bodyA.type != ru.dbotthepony.kbox2d.api.BodyType.STATIC
-			val activeB = bodyB.isAwake && bodyB.type != ru.dbotthepony.kbox2d.api.BodyType.STATIC
+			val activeA = bodyA.isAwake && bodyA.type != BodyType.STATIC
+			val activeB = bodyB.isAwake && bodyB.type != BodyType.STATIC
 
 			// At least one body must be awake and it must be dynamic or kinematic.
 			if (!activeA && !activeB) {
@@ -144,7 +143,7 @@ class ContactManager {
 		broadPhase.updatePairs(this::addPair)
 	}
 
-	fun addPair(proxyUserDataA: Any?, proxyUserDataB: Any?) {
+	private fun addPair(proxyUserDataA: Any?, proxyUserDataB: Any?) {
 		val proxyA = proxyUserDataA as FixtureProxy
 		val proxyB = proxyUserDataB as FixtureProxy
 
@@ -200,7 +199,7 @@ class ContactManager {
 		// Contact creation may swap fixtures.
 		// Insert into the world.
 		c.next = contactList
-		(contactList as AbstractContact?)?.prev = c
+		contactList?.prev = c
 		contactList = c
 
 		// Connect to island graph.
diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/Main.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/Main.kt
index 9a832f4a..6b18cc94 100644
--- a/src/main/kotlin/ru/dbotthepony/kstarbound/Main.kt
+++ b/src/main/kotlin/ru/dbotthepony/kstarbound/Main.kt
@@ -4,6 +4,8 @@ import org.apache.logging.log4j.LogManager
 import org.lwjgl.Version
 import org.lwjgl.glfw.GLFW.glfwSetWindowShouldClose
 import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.collision.DistanceProxy
+import ru.dbotthepony.kbox2d.collision.b2TimeOfImpact
 import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
 import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
 import ru.dbotthepony.kstarbound.client.StarboundClient
@@ -130,7 +132,7 @@ fun main() {
 		}
 
 		run {
-			val stripes = 4
+			val stripes = 0
 
 			for (stripe in 0 until stripes) {
 				for (x in 0 .. (stripes - stripe)) {
@@ -178,10 +180,6 @@ fun main() {
 	client.gl.box2dRenderer.drawAABB = false
 	client.gl.box2dRenderer.drawJoints = false
 
-	client.onPostDrawWorld {
-
-	}
-
 	ent.spawn()
 
 	while (client.renderFrame()) {
diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/client/ClientSettings.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/client/ClientSettings.kt
index c19323a6..14751b3c 100644
--- a/src/main/kotlin/ru/dbotthepony/kstarbound/client/ClientSettings.kt
+++ b/src/main/kotlin/ru/dbotthepony/kstarbound/client/ClientSettings.kt
@@ -8,5 +8,5 @@ data class ClientSettings(
 	 */
 	var scale: Float = 2f,
 
-	var debugCollisions: Boolean = true,
+	var debugCollisions: Boolean = false,
 )
diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/client/StarboundClient.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/client/StarboundClient.kt
index 6a9524a2..e807bfea 100644
--- a/src/main/kotlin/ru/dbotthepony/kstarbound/client/StarboundClient.kt
+++ b/src/main/kotlin/ru/dbotthepony/kstarbound/client/StarboundClient.kt
@@ -260,7 +260,7 @@ class StarboundClient : AutoCloseable {
 		val runtime = Runtime.getRuntime()
 
 		gl.font.render("FPS: ${(averageFramesPerSecond * 100f).toInt() / 100f}", scale = 0.4f)
-		gl.font.render("Mem: ${formatBytesShort(runtime.totalMemory() - runtime.freeMemory())}", x = viewportWidth.toFloat(), scale = 0.4f, alignX = TextAlignX.RIGHT)
+		gl.font.render("JVM Heap: ${formatBytesShort(runtime.totalMemory() - runtime.freeMemory())}", y = gl.font.lineHeight * 0.5f, scale = 0.4f)
 
 		GLFW.glfwSwapBuffers(window)
 		GLFW.glfwPollEvents()
diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/util/Formatter.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/util/Formatter.kt
index f1836c40..3c738647 100644
--- a/src/main/kotlin/ru/dbotthepony/kstarbound/util/Formatter.kt
+++ b/src/main/kotlin/ru/dbotthepony/kstarbound/util/Formatter.kt
@@ -11,6 +11,7 @@ fun formatBytesShort(input: Long): String {
 		in 0 until KIBIBYTE -> "${input}b"
 		in KIBIBYTE until MEBIBYTE -> "${(((input / KIBIBYTE).toDouble() + (input % KIBIBYTE).toDouble() / KIBIBYTE) * 100.0).toLong().toDouble() / 100.0}KiB"
 		in MEBIBYTE until GIBIBYTE -> "${(((input / MEBIBYTE).toDouble() + (input % MEBIBYTE).toDouble() / MEBIBYTE) * 100.0).toLong().toDouble() / 100.0}MiB"
+		in GIBIBYTE until TEBIBYTE -> "${(((input / GIBIBYTE).toDouble() + (input % GIBIBYTE).toDouble() / GIBIBYTE) * 100.0).toLong().toDouble() / 100.0}GiB"
 		else -> "${input}b"
 	}
 }
diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/world/entities/AliveEntity.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/world/entities/AliveEntity.kt
index 7ecbe2b7..f9e6f5f9 100644
--- a/src/main/kotlin/ru/dbotthepony/kstarbound/world/entities/AliveEntity.kt
+++ b/src/main/kotlin/ru/dbotthepony/kstarbound/world/entities/AliveEntity.kt
@@ -1,5 +1,11 @@
 package ru.dbotthepony.kstarbound.world.entities
 
+import ru.dbotthepony.kbox2d.api.ContactEdge
+import ru.dbotthepony.kbox2d.api.FixtureDef
+import ru.dbotthepony.kbox2d.api.b2_linearSlop
+import ru.dbotthepony.kbox2d.api.b2_polygonRadius
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+import ru.dbotthepony.kbox2d.dynamics.B2Fixture
 import ru.dbotthepony.kstarbound.client.ClientWorld
 import ru.dbotthepony.kstarbound.world.World
 import ru.dbotthepony.kvector.util2d.AABB
@@ -72,6 +78,32 @@ abstract class WalkableMovementController<T : IWalkableEntity>(entity: T) : Move
 
 	protected abstract val moveDirection: Move
 
+	protected var sensorA: B2Fixture? = null
+	protected var sensorB: B2Fixture? = null
+	protected var bodyFixture: B2Fixture? = null
+
+	protected abstract fun recreateBodyFixture()
+
+	protected open fun recreateSensors() {
+		sensorA?.destroy()
+		sensorB?.destroy()
+
+		val bodyFixture = bodyFixture ?: return
+		val aabb = bodyFixture.shape.computeAABB(0)
+
+		val sensorheight = (aabb.height - stepSize) / 2.0
+
+		sensorA = body.createFixture(FixtureDef(
+			shape = PolygonShape().also { it.setAsBox(0.2, sensorheight, Vector2d(-aabb.width / 2.0 - 0.2, aabb.height / 2.0 - sensorheight), 0.0) },
+			isSensor = true,
+		))
+
+		sensorB = body.createFixture(FixtureDef(
+			shape = PolygonShape().also { it.setAsBox(0.2, sensorheight, Vector2d(aabb.width / 2.0 + 0.2, aabb.height / 2.0 - sensorheight), 0.0) },
+			isSensor = true,
+		))
+	}
+
 	var wantsToDuck = false
 	open var isDucked = false
 		protected set
@@ -131,6 +163,8 @@ abstract class WalkableMovementController<T : IWalkableEntity>(entity: T) : Move
 
 	protected abstract fun canUnDuck(): Boolean
 
+	protected var previousVelocity = Vector2d.ZERO
+
 	protected open fun thinkMovement(delta: Double) {
 		if (onGround && !isDucked) {
 			when (moveDirection) {
@@ -151,6 +185,54 @@ abstract class WalkableMovementController<T : IWalkableEntity>(entity: T) : Move
 							body.linearVelocity += -delta * world.gravity * 2.0
 						}
 					}
+
+					var wantToStepUp = false
+					var foundContact: ContactEdge? = null
+
+					for (contact in body.contactEdgeIterator) {
+						if (contact.contact.manifold.localNormal.dot(Vector2d.NEGATIVE_X) >= 0.95) {
+							// we hit something to our left
+							wantToStepUp = true
+							foundContact = contact
+							break
+						}
+					}
+
+					if (wantToStepUp) {
+						// make sure something we hit is actually a staircase of sort, and not geometry edges
+						val aabbFound: AABB
+
+						if (foundContact!!.contact.fixtureB == sensorA) {
+							aabbFound = foundContact.contact.fixtureA.getAABB(foundContact.contact.childIndexA)
+						} else {
+							aabbFound = foundContact.contact.fixtureB.getAABB(foundContact.contact.childIndexB)
+						}
+
+						if (aabbFound.maxs.y - b2_polygonRadius * 2.0 <= body.worldSpaceAABB.mins.y) {
+							// just a crack on sidewalk
+							wantToStepUp = false
+						}
+					}
+
+					if (wantToStepUp) {
+						var stepHeightClear = true
+
+						for (contact in body.contactEdgeIterator) {
+							if (contact.contact.fixtureA == sensorA || contact.contact.fixtureB == sensorA) {
+								if (contact.contact.isTouching) {
+									stepHeightClear = false
+									break
+								}
+							}
+						}
+
+						if (stepHeightClear) {
+							val velocity = if (previousVelocity.length > body.linearVelocity.length) previousVelocity else body.linearVelocity
+							body.setTransform(body.position + Vector2d(x = -0.05, y = stepSize), body.angle)
+							body.linearVelocity = velocity
+							//body.linearVelocity += Vector2d(y = -delta * 18.0 * stepSize * world.gravity.y)
+						}
+					}
 				}
 
 				Move.MOVE_RIGHT -> {
@@ -166,9 +248,59 @@ abstract class WalkableMovementController<T : IWalkableEntity>(entity: T) : Move
 							body.linearVelocity += -delta * world.gravity * 2.0
 						}
 					}
+
+					var wantToStepUp = false
+					var foundContact: ContactEdge? = null
+
+					for (contact in body.contactEdgeIterator) {
+						if (contact.contact.manifold.localNormal.dot(Vector2d.POSITIVE_X) >= 0.95) {
+							// we hit something to our right
+							wantToStepUp = true
+							foundContact = contact
+							break
+						}
+					}
+
+					if (wantToStepUp) {
+						// make sure something we hit is actually a staircase of sort, and not geometry edges
+						val aabbFound: AABB
+
+						if (foundContact!!.contact.fixtureB == sensorB) {
+							aabbFound = foundContact.contact.fixtureA.getAABB(foundContact.contact.childIndexA)
+						} else {
+							aabbFound = foundContact.contact.fixtureB.getAABB(foundContact.contact.childIndexB)
+						}
+
+						if (aabbFound.maxs.y - b2_polygonRadius * 2.0 <= body.worldSpaceAABB.mins.y) {
+							// just a crack on sidewalk
+							wantToStepUp = false
+						}
+					}
+
+					if (wantToStepUp) {
+						var stepHeightClear = true
+
+						for (contact in body.contactEdgeIterator) {
+							if (contact.contact.fixtureA == sensorB || contact.contact.fixtureB == sensorB) {
+								if (contact.contact.isTouching) {
+									stepHeightClear = false
+									break
+								}
+							}
+						}
+
+						if (stepHeightClear) {
+							val velocity = if (previousVelocity.length > body.linearVelocity.length) previousVelocity else body.linearVelocity
+							body.setTransform(body.position + Vector2d(x = 0.05, y = stepSize), body.angle)
+							body.linearVelocity = velocity
+							//body.linearVelocity += Vector2d(y = -delta * 18.0 * stepSize * world.gravity.y)
+						}
+					}
 				}
 			}
 
+			previousVelocity = body.linearVelocity
+
 			if (jumpRequested) {
 				jumpRequested = false
 				nextJump = world.timer + 0.1
@@ -195,9 +327,15 @@ abstract class WalkableMovementController<T : IWalkableEntity>(entity: T) : Move
 
 		if (wantsToDuck && onGround) {
 			isDucked = true
+			recreateBodyFixture()
+			recreateSensors()
+			body.isAwake = true
 		} else if (isDucked) {
 			if (canUnDuck()) {
 				isDucked = false
+				recreateBodyFixture()
+				recreateSensors()
+				body.isAwake = true
 			}
 		}
 	}
diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/world/entities/PlayerEntity.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/world/entities/PlayerEntity.kt
index 21375ece..8419ef6a 100644
--- a/src/main/kotlin/ru/dbotthepony/kstarbound/world/entities/PlayerEntity.kt
+++ b/src/main/kotlin/ru/dbotthepony/kstarbound/world/entities/PlayerEntity.kt
@@ -9,44 +9,32 @@ import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 class PlayerMovementController(entity: PlayerEntity) : WalkableMovementController<PlayerEntity>(entity) {
 	public override var moveDirection = Move.STAND_STILL
-	private var bodyFixture: B2Fixture
 
-	override var isDucked: Boolean = false
-		set(value) {
-			if (value == field)
-				return
+	override fun recreateBodyFixture() {
+		bodyFixture?.destroy()
 
-			field = value
-
-			bodyFixture.destroy()
-
-			if (value) {
-				bodyFixture = body.createFixture(FixtureDef(
-					shape = DUCKING,
-					friction = 0.4,
-					density = 1.9,
-				))
-			} else {
-				bodyFixture = body.createFixture(FixtureDef(
-					shape = STANDING,
-					friction = 0.4,
-					density = 1.9,
-				))
-			}
-
-			body.isAwake = true
+		if (isDucked) {
+			bodyFixture = body.createFixture(FixtureDef(
+				shape = DUCKING,
+				friction = 0.4,
+				density = 1.9,
+			))
+		} else {
+			bodyFixture = body.createFixture(FixtureDef(
+				shape = STANDING,
+				friction = 0.4,
+				density = 1.9,
+			))
 		}
-
-	override fun canUnDuck(): Boolean {
-		return world.isSpaceEmptyFromTiles(STANDING_AABB + position)
 	}
 
 	init {
-		bodyFixture = body.createFixture(FixtureDef(
-			shape = STANDING,
-			friction = 0.4,
-			density = 1.9,
-		))
+		recreateBodyFixture()
+		recreateSensors()
+	}
+
+	override fun canUnDuck(): Boolean {
+		return world.isSpaceEmptyFromTiles(STANDING_AABB + position)
 	}
 
 	companion object {