KBox2D

src/main/kotlin/ru/dbotthepony/kbox2d/api/B2TimeStep.kt

@@ -0,0 +1,155 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+/**
+ * Profiling data. Times are in milliseconds.
+ */
+interface IProfileData {
+	val step: Long
+	val collide: Long
+	val solve: Long
+	val solveInit: Long
+	val solveVelocity: Long
+	val solvePosition: Long
+	val broadphase: Long
+	val solveTOI: Long
+	val integratePositions: Long
+}
+
+/**
+ * Profiling data. Times are in nanoseconds.
+ */
+data class ProfileSnapshot(
+	override val step: Long,
+	override val collide: Long,
+	override val solve: Long,
+	override val solveInit: Long,
+	override val solveVelocity: Long,
+	override val solvePosition: Long,
+	override val broadphase: Long,
+	override val solveTOI: Long,
+	override val integratePositions: Long,
+) : IProfileData
+
+/**
+ * Profiling data. Times are in nanoseconds.
+ */
+internal data class ProfileData(
+	override var step: Long = 0L,
+	override var collide: Long = 0L,
+	override var solve: Long = 0L,
+	override var solveInit: Long = 0L,
+	override var solveVelocity: Long = 0L,
+	override var solvePosition: Long = 0L,
+	override var broadphase: Long = 0L,
+	override var solveTOI: Long = 0L,
+	override var integratePositions: Long = 0L,
+) : IProfileData {
+	fun snapshot(): ProfileSnapshot {
+		return ProfileSnapshot(
+			step = step,
+			collide = collide,
+			solve = solve,
+			solveInit = solveInit,
+			solveVelocity = solveVelocity,
+			solvePosition = solvePosition,
+			broadphase = broadphase,
+			solveTOI = solveTOI,
+			integratePositions = integratePositions,
+		)
+	}
+}
+
+/**
+ * This is an internal structure.
+ */
+data class B2TimeStep(
+	var dt: Double,
+	var inv_dt: Double,
+	var dtRatio: Double,
+	var velocityIterations: Int,
+	var positionIterations: Int,
+	var warmStarting: Boolean,
+)
+
+/**
+ * This is an internal structure.
+ */
+internal class B2Position(
+	c: Vector2d = Vector2d.ZERO,
+	a: Double = 0.0,
+) {
+	var c: Vector2d = c
+		set(value) {
+			if (!value.isFinite) {
+				throw IllegalArgumentException("Tried to set illegal position $value")
+			}
+
+			field = value
+		}
+
+	var a: Double = a
+		set(value) {
+			if (!value.isFinite()) {
+				throw IllegalArgumentException("Tried to set non-finite angle $value")
+			}
+
+			if (value.isNaN()) {
+				throw IllegalArgumentException("Tried to set NaN angle")
+			}
+
+			field = value
+		}
+
+	init {
+		// KBox2D: trigger sanity checks at least once
+		this.c = c
+		this.a = a
+	}
+}
+
+/**
+ * This is an internal structure.
+ */
+internal class B2Velocity(
+	v: Vector2d = Vector2d.ZERO,
+	w: Double = 0.0,
+) {
+	var v: Vector2d = v
+		set(value) {
+			if (!value.isFinite) {
+				throw IllegalArgumentException("Tried to set illegal velocity $value")
+			}
+
+			field = value
+		}
+
+	var w: Double = w
+		set(value) {
+			if (!value.isFinite()) {
+				throw IllegalArgumentException("Tried to set non-finite angular angle $value")
+			}
+
+			if (value.isNaN()) {
+				throw IllegalArgumentException("Tried to set NaN angular angle")
+			}
+
+			field = value
+		}
+
+	init {
+		// KBox2D: trigger sanity checks at least once
+		this.v = v
+		this.w = w
+	}
+}
+
+/**
+ * Solver Data
+ */
+internal class B2SolverData(
+	var step: B2TimeStep,
+	var positions: List<B2Position>,
+	var velocities: List<B2Velocity>
+)

src/main/kotlin/ru/dbotthepony/kbox2d/api/Body.kt

@@ -0,0 +1,524 @@
+package ru.dbotthepony.kbox2d.api
+
+/*
+MIT License
+
+Original author Erin Catto (c) 2019
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+/// The body type.
+/// static: zero mass, zero velocity, may be manually moved
+/// kinematic: zero mass, non-zero velocity set by user, moved by solver
+/// dynamic: positive mass, non-zero velocity determined by forces, moved by solver
+enum class BodyType {
+	STATIC,
+	KINEMATIC,
+	DYNAMIC
+}
+
+/// A body definition holds all the data needed to construct a rigid body.
+/// You can safely re-use body definitions. Shapes are added to a body after construction.
+data class BodyDef(
+	/**
+	 * The world position of the body. Avoid creating bodies at the origin
+	 * since this can lead to many overlapping shapes.
+ 	 */
+	var position: Vector2d = Vector2d.ZERO,
+
+	/**
+	 * The world angle of the body in radians.
+	 */
+	var angle: Double = 0.0,
+
+	/**
+	 * The linear velocity of the body's origin in world co-ordinates.
+	 */
+	var linearVelocity: Vector2d = Vector2d.ZERO,
+
+	/**
+	 * The angular velocity of the body.
+	 */
+	var angularVelocity: Double = 0.0,
+
+	/**
+	 * Linear damping is use to reduce the linear velocity. The damping parameter
+	 * can be larger than 1.0f but the damping effect becomes sensitive to the
+	 * time step when the damping parameter is large.
+	 * Units are 1/time
+	 */
+	var linearDamping: Double = 0.0,
+
+	/**
+	 * Angular damping is use to reduce the angular velocity. The damping parameter
+	 * can be larger than 1.0f but the damping effect becomes sensitive to the
+	 * time step when the damping parameter is large.
+	 * Units are 1/time
+ 	 */
+	var angularDamping: Double = 0.0,
+
+	/**
+	 * Set this flag to false if this body should never fall asleep. Note that
+	 * this increases CPU usage.
+ 	 */
+	var allowSleep: Boolean = true,
+
+	/**
+	 * Is this body initially awake or sleeping?
+	 */
+	var awake: Boolean = true,
+
+	/**
+	 * Should this body be prevented from rotating? Useful for characters.
+	 */
+	var fixedRotation: Boolean = false,
+
+	/**
+	 * Is this a fast moving body that should be prevented from tunneling through
+	 * other moving bodies? Note that all bodies are prevented from tunneling through
+	 * kinematic and static bodies. This setting is only considered on dynamic bodies.
+	 * @warning You should use this flag sparingly since it increases processing time.
+	 */
+	var bullet: Boolean = false,
+
+	/**
+	 * The body type: static, kinematic, or dynamic.
+	 * Note: if a dynamic body would have zero mass, the mass is set to one.
+	 */
+	var type: BodyType = BodyType.STATIC,
+
+	/**
+	 * Does this body start out enabled?
+	 */
+	var enabled: Boolean = true,
+
+	/**
+	 * Use this to store application specific body data.
+	 */
+	var userData: Any? = null,
+
+	/**
+	 * Scale the gravity applied to this body.
+	 */
+	var gravityScale: Double = 1.0,
+) {
+	fun validate() {
+		require(angularVelocity.isFinite()) { "Angular velocity is infinite" }
+		require(linearDamping.isFinite()) { "Linear damping is infinite" }
+		require(angularDamping.isFinite()) { "Angular damping is infinite" }
+		require(angle.isFinite()) { "Angular velocity is infinite" }
+		require(gravityScale.isFinite()) { "Gravity scale is infinite" }
+		require(position.isFinite) { "Position is infinite" }
+
+		require(!angularVelocity.isNaN()) { "Angular velocity is NaN" }
+		require(!linearDamping.isNaN()) { "Linear damping is NaN" }
+		require(!angularDamping.isNaN()) { "Angular damping is NaN" }
+		require(!angle.isNaN()) { "Angular velocity is NaN" }
+		require(!gravityScale.isNaN()) { "Gravity scale is NaN" }
+
+		require(angularDamping >= 0.0) { "Angular damping must be non negative, $angularDamping given" }
+		require(linearDamping >= 0.0) { "Linear damping must be non negative, $linearDamping given" }
+	}
+}
+
+enum class BodyFlags(val bitmask: Int) {
+	ISLAND(0x1),
+	AWAKE(0x2),
+	AUTO_SLEEP(0x4),
+	BULLET(0x8),
+	FIXED_ROTATION(0x10),
+	ENABLED(0x20),
+	TOI(0x40);
+
+	fun isit(other: Int): Boolean {
+		return (other and bitmask) == bitmask
+	}
+
+	fun update(other: Int, state: Boolean): Int {
+		if (state)
+			return or(other)
+		else
+			return not(other)
+	}
+
+	fun and(other: Int): Int {
+		return other and bitmask
+	}
+
+	fun or(other: Int): Int {
+		return other or bitmask
+	}
+
+	fun not(other: Int): Int {
+		return other and bitmask.inv()
+	}
+}
+
+interface IBody {
+	/** Creates a fixture and attach it to this body. Use this function if you need
+	 * to set some fixture parameters, like friction. Otherwise you can create the
+	 * fixture directly from a shape.
+	 * If the density is non-zero, this function automatically updates the mass of the body.
+	 * Contacts are not created until the next time step.
+	 * @param def the fixture definition.
+	 * @warning This function is locked during callbacks.
+	 */
+	fun createFixture(def: FixtureDef): IFixture
+
+	/**
+	 * Creates a fixture from a shape and attach it to this body.
+	 * This is a convenience function. Use b2FixtureDef if you need to set parameters
+	 * like friction, restitution, user data, or filtering.
+	 * If the density is non-zero, this function automatically updates the mass of the body.
+	 * @param shape the shape to be cloned.
+	 * @param density the shape density (set to zero for static bodies).
+	 * @warning This function is locked during callbacks.
+	 */
+	fun createFixture(shape: IShape<*>, density: Double): IFixture {
+		return createFixture(
+			FixtureDef(
+				shape = shape,
+				density = density
+			)
+		)
+	}
+
+	/**
+	 * Destroy a fixture. This removes the fixture from the broad-phase and
+	 * destroys all contacts associated with this fixture. This will
+	 * automatically adjust the mass of the body if the body is dynamic and the
+	 * fixture has positive density.
+	 * All fixtures attached to a body are implicitly destroyed when the body is destroyed.
+	 * @param fixture the fixture to be removed.
+	 * @warning This function is locked during callbacks.
+	 */
+	fun destroyFixture(fixture: IFixture)
+
+	/**
+	 * Set the position of the body's origin and rotation.
+	 * Manipulating a body's transform may cause non-physical behavior.
+	 * Note: contacts are updated on the next call to b2World::Step.
+	 * @param position the world position of the body's local origin.
+	 * @param angle the world rotation in radians.
+	 */
+	fun setTransform(position: Vector2d, angle: Double)
+
+	/**
+	 * Get the body transform for the body's origin.
+	 * @return the world transform of the body's origin.
+	 */
+	val transform: Transform
+
+	/**
+	 * Get the world body origin position.
+	 * @return the world position of the body's origin.
+	 */
+	val position: Vector2d
+
+	/**
+	 * Get the angle in radians.
+	 * @return the current world rotation angle in radians.
+	 */
+	val angle: Double
+
+	/**
+	 * Get the world position of the center of mass.
+	 */
+	val worldCenter: Vector2d
+
+	/**
+	 * Get the local position of the center of mass.
+	 */
+	val localCenter: Vector2d
+
+	/**
+	 * The linear velocity of the body's origin in world co-ordinates.
+	 */
+	var linearVelocity: Vector2d
+
+	/**
+	 * The angular velocity of the body.
+	 */
+	var angularVelocity: Double
+
+	/**
+	 * Apply a force at a world point. If the force is not
+	 * applied at the center of mass, it will generate a torque and
+	 * affect the angular velocity. This wakes up the body.
+	 * @param force the world force vector, usually in Newtons (N).
+	 * @param point the world position of the point of application.
+	 * @param wake also wake up the body
+	 */
+	fun applyForce(force: Vector2d, point: Vector2d, wake: Boolean = true)
+
+	/**
+	 * Apply a force to the center of mass. This wakes up the body.
+	 * @param force the world force vector, usually in Newtons (N).
+	 * @param wake also wake up the body
+	 */
+	fun applyForceToCenter(force: Vector2d, wake: Boolean = true)
+
+	/**
+	 * Apply a torque. This affects the angular velocity
+	 * without affecting the linear velocity of the center of mass.
+	 * @param torque about the z-axis (out of the screen), usually in N-m.
+	 * @param wake also wake up the body
+	 */
+	fun applyTorque(torque: Double, wake: Boolean = true)
+
+	/**
+	 * Apply an impulse at a point. This immediately modifies the velocity.
+	 * It also modifies the angular velocity if the point of application
+	 * is not at the center of mass. This wakes up the body.
+	 * @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
+	 * @param point the world position of the point of application.
+	 * @param wake also wake up the body
+	 */
+	fun applyLinearImpulse(impulse: Vector2d, point: Vector2d, wake: Boolean = true)
+
+	/**
+	 * Apply an impulse to the center of mass. This immediately modifies the velocity.
+	 * @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
+	 * @param wake also wake up the body
+	 */
+	fun applyLinearImpulseToCenter(impulse: Vector2d, wake: Boolean = true)
+
+	/**
+	 * Apply an angular impulse.
+	 * @param impulse the angular impulse in units of kg*m*m/s
+	 * @param wake also wake up the body
+	 */
+	fun applyAngularImpulse(impulse: Double, wake: Boolean = true)
+
+	/**
+	 * Get the total mass of the body.
+	 * @return the mass, usually in kilograms (kg).
+	 */
+	val mass: Double
+
+	/**
+	 * Get the rotational inertia of the body about the local origin.
+	 * @return the rotational inertia, usually in kg-m^2.
+	 */
+	val inertia: Double
+
+	/**
+	 * Get the mass data of the body.
+	 * @return a struct containing the mass, inertia and center of the body.
+	 * Set the mass properties to override the mass properties of the fixtures.
+	 * Note that this changes the center of mass position.
+	 * Note that creating or destroying fixtures can also alter the mass.
+	 * This function has no effect if the body isn't dynamic.
+	 * @param data the mass properties.
+	 */
+	var massData: MassData
+
+	/**
+	 * This resets the mass properties to the sum of the mass properties of the fixtures.
+	 * This normally does not need to be called unless you called SetMassData to override
+	 * the mass and you later want to reset the mass.
+	 */
+	fun resetMassData()
+
+	/**
+	 * Get the world coordinates of a point given the local coordinates.
+	 * @param localPoint a point on the body measured relative the the body's origin.
+	 * @return the same point expressed in world coordinates.
+	 */
+	fun getWorldPoint(localPoint: Vector2d): Vector2d
+
+	/**
+	 * Get the world coordinates of a vector given the local coordinates.
+	 * @param localVector a vector fixed in the body.
+	 * @return the same vector expressed in world coordinates.
+	 */
+	fun getWorldVector(localPoint: Vector2d): Vector2d
+
+	/**
+	 * Gets a local point relative to the body's origin given a world point.
+	 * @param worldPoint a point in world coordinates.
+	 * @return the corresponding local point relative to the body's origin.
+	 */
+	fun getLocalPoint(worldPoint: Vector2d): Vector2d
+
+	/**
+	 * Gets a local vector given a world vector.
+	 * @param worldVector a vector in world coordinates.
+	 * @return the corresponding local vector.
+	 */
+	fun getLocalVector(worldVector: Vector2d): Vector2d
+
+	/**
+	 * Get the world linear velocity of a world point attached to this body.
+	 * @param worldPoint a point in world coordinates.
+	 * @return the world velocity of a point.
+	 */
+	fun getLinearVelocityFromWorldPoint(worldPoint: Vector2d): Vector2d
+
+	/**
+	 * Get the world velocity of a local point.
+	 * @param localPoint a point in local coordinates.
+	 * @return the world velocity of a point.
+	 */
+	fun getLinearVelocityFromLocalPoint(localPoint: Vector2d): Vector2d
+
+	var linearDamping: Double
+	var angularDamping: Double
+	var gravityScale: Double
+
+	/**
+	 * Set the type of this body. This may alter the mass and velocity.
+	 */
+	var type: BodyType
+
+	/**
+	 * Should this body be treated like a bullet for continuous collision detection?
+	 */
+	var isBullet: Boolean
+
+	/**
+	 * You can disable sleeping on this body. If you disable sleeping, the
+	 * body will be woken.
+	 */
+	var allowAutoSleep: Boolean
+
+	/**
+	 * Set the sleep state of the body. A sleeping body has very
+	 * low CPU cost.
+	 * @param flag set to true to wake the body, false to put it to sleep.
+	 */
+	var isAwake: Boolean
+
+	/**
+	 * Allow a body to be disabled. A disabled body is not simulated and cannot
+	 * be collided with or woken up.
+	 *
+	 * If you pass a flag of true, all fixtures will be added to the broad-phase.
+	 *
+	 * If you pass a flag of false, all fixtures will be removed from the
+	 * broad-phase and all contacts will be destroyed.
+	 *
+	 * Fixtures and joints are otherwise unaffected. You may continue
+	 * to create/destroy fixtures and joints on disabled bodies.
+	 *
+	 * Fixtures on a disabled body are implicitly disabled and will
+	 * not participate in collisions, ray-casts, or queries.
+	 *
+	 * Joints connected to a disabled body are implicitly disabled.
+	 *
+	 * An diabled body is still owned by a b2World object and remains
+	 * in the body list.
+	 */
+	var isEnabled: Boolean
+
+	/**
+	 * Set this body to have fixed rotation. This causes the mass to be reset.
+	 */
+	var isFixedRotation: Boolean
+
+	/**
+	 * Get the list of all fixtures attached to this body.
+	 *
+	 * Kotlin: This is not a list, but, as in C++ impl, a custom
+	 * linked list.
+ 	 */
+	val fixtureList: IFixture?
+
+	val fixtureIterator: Iterator<IFixture> get() {
+		return object : Iterator<IFixture> {
+			private var node = fixtureList
+
+			override fun hasNext(): Boolean {
+				return node != null
+			}
+
+			override fun next(): IFixture {
+				val old = node!!
+				node = old.next
+				return old
+			}
+		}
+	}
+
+	/**
+	 * Get the list of all joints attached to this body.
+	 *
+	 * Kotlin: This is not a list, but, as in C++ impl, a custom
+	 * linked list.
+ 	 */
+	val jointList: JointEdge?
+
+	val jointIterator: Iterator<JointEdge> get() {
+		return object : Iterator<JointEdge> {
+			private var node = jointList
+
+			override fun hasNext(): Boolean {
+				return node != null
+			}
+
+			override fun next(): JointEdge {
+				val old = node!!
+				node = old.next
+				return old
+			}
+		}
+	}
+
+	/**
+	 * Get the list of all contacts attached to this body.
+	 * @warning this list changes during the time step and you may
+	 * miss some collisions if you don't use b2ContactListener.
+	 *
+	 * Kotlin: This is not a list, but, as in C++ impl, a custom
+	 * linked list.
+	 */
+	val contactEdge: ContactEdge?
+
+	val contactEdgeIterator: Iterator<ContactEdge> get() {
+		return object : Iterator<ContactEdge> {
+			private var node = contactEdge
+
+			override fun hasNext(): Boolean {
+				return node != null
+			}
+
+			override fun next(): ContactEdge {
+				val old = node!!
+				node = old.next
+				return old
+			}
+		}
+	}
+
+	val next: IBody?
+	val prev: IBody?
+
+	val userData: Any?
+
+	/**
+	 * Get the parent world of this body.
+	 */
+	val world: IB2World
+
+	/// Dump this body to a file
+	fun dump()
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/BroadPhase.kt

@@ -0,0 +1,50 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+typealias b2Pair = Pair<Int, Int>
+
+const val e_nullProxy = -1
+
+/**
+ * The broad-phase is used for computing pairs and performing volume queries and ray casts.
+ * This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
+ * It is up to the client to consume the new pairs and to track subsequent overlap.
+ */
+interface IBroadPhase : IProxieable, IMovable {
+	/**
+	 * Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.
+ 	 */
+	fun touchProxy(proxyID: Int)
+
+	/**
+	 * Test overlap of fat AABBs.
+	 */
+	fun testOverlap(proxyIDA: Int, proxyIDB: Int): Boolean
+
+	/**
+	 * Get the number of proxies.
+	 */
+	val proxyCount: Int
+
+	/**
+	 * Update the pairs. This results in pair callbacks. This can only add pairs.
+	 */
+	fun updatePairs(callback: (Any?, Any?) -> Unit)
+
+	/**
+	 * Get the height of the embedded tree.
+	 */
+	val treeHeight: Int
+
+	/**
+	 * Get the balance of the embedded tree.
+	 */
+	val treeBalance: Int
+
+	/**
+	 * Get the quality metric of the embedded tree.
+	 */
+	val treeQuality: Double
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/Collision.kt

@@ -0,0 +1,126 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+/// The features that intersect to form the contact point
+/// This must be 4 bytes or less. ?
+data class ContactFeature(
+	var indexA: Int = 0, ///< Feature index on shapeA
+	var indexB: Int = 0, ///< Feature index on shapeB
+	var typeA: Type = Type.VERTEX, ///< The feature type on shapeA
+	var typeB: Type = Type.VERTEX, ///< The feature type on shapeB
+) {
+	enum class Type {
+		VERTEX,
+		FACE,
+	}
+}
+
+/// Contact ids to facilitate warm starting.
+data class ContactID(
+	var key: Int = 0, ///< Used to quickly compare contact ids.
+	val cf: ContactFeature = ContactFeature(),
+)
+
+/**
+ * A manifold point is a contact point belonging to a contact
+ * manifold. It holds details related to the geometry and dynamics
+ * of the contact points.
+ *
+ * The local point usage depends on the manifold type:
+ * - e_circles: the local center of circleB
+ * - e_faceA: the local center of cirlceB or the clip point of polygonB
+ * - e_faceB: the clip point of polygonA
+ *
+ * This structure is stored across time steps, so we keep it small.
+ *
+ * Note: the impulses are used for internal caching and may not
+ * provide reliable contact forces, especially for high speed collisions.
+ */
+data class ManifoldPoint(
+	var localPoint: Vector2d = Vector2d.ZERO, ///< usage depends on manifold type
+	var normalImpulse: Double = 0.0, ///< the non-penetration impulse
+	var tangentImpulse: Double = 0.0, ///< the friction impulse
+	var id: ContactID = ContactID(), ///< uniquely identifies a contact point between two shapes
+)
+
+/**
+ * A manifold for two touching convex shapes.
+ *
+ * Box2D supports multiple types of contact:
+ * - clip point versus plane with radius
+ * - point versus point with radius (circles)
+ *
+ * The local point usage depends on the manifold type:
+ * - e_circles: the local center of circleA
+ * - e_faceA: the center of faceA
+ * - e_faceB: the center of faceB
+ *
+ * Similarly the local normal usage:
+ * - e_circles: not used
+ * - e_faceA: the normal on polygonA
+ * - e_faceB: the normal on polygonB
+ *
+ * We store contacts in this way so that position correction can
+ * account for movement, which is critical for continuous physics.
+ *
+ * All contact scenarios must be expressed in one of these types.
+ *
+ * This structure is stored across time steps, so we keep it small.
+ */
+data class Manifold(
+	val localNormal: Vector2d = Vector2d.ZERO,
+	val localPoint: Vector2d = Vector2d.ZERO,
+	val type: Type? = null,
+	val points: List<ManifoldPoint> = listOf(),
+) {
+	enum class Type {
+		CIRCLES,
+		FACE_A,
+		FACE_B,
+	}
+
+	companion object {
+		val EMPTY = Manifold()
+	}
+}
+
+interface IWorldManifold {
+	val normal: Vector2d
+	val points: Array<Vector2d>
+	val separations: DoubleArray
+}
+
+/// This is used for determining the state of contact points.
+enum class PointState {
+	NULL, ///< point does not exist
+	ADD, ///< point was added in the update
+	PERSIST, ///< point persisted across the update
+	REMOVE ///< point was removed in the update
+}
+
+/// Used for computing contact manifolds.
+data class ClipVertex(
+	var v: Vector2d = Vector2d.ZERO,
+	var id: ContactID = ContactID(),
+)
+
+/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+data class RayCastInput(
+	val p1: Vector2d,
+	val p2: Vector2d,
+	val maxFraction: Double,
+)
+
+/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
+/// come from b2RayCastInput.
+data class RayCastOutput(
+	// В оригинале этого нет, но для un-C++шивания кода, оно должно быть тут
+	val hit: Boolean,
+	val normal: Vector2d = Vector2d.ZERO,
+	val fraction: Double = 1.0,
+) {
+	companion object {
+		val MISS = RayCastOutput(false)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/Constants.kt

@@ -0,0 +1,85 @@
+package ru.dbotthepony.kbox2d.api
+
+import kotlin.math.PI
+
+/// You can use this to change the length scale used by your game.
+/// For example for inches you could use 39.4.
+const val b2_lengthUnitsPerMeter = 1.0
+
+/// The maximum number of vertices on a convex polygon. You cannot increase
+/// this too much because b2BlockAllocator has a maximum object size.
+const val b2_maxPolygonVertices = 8
+
+/// Collision
+
+/// The maximum number of contact points between two convex shapes. Do
+/// not change this value.
+const val b2_maxManifoldPoints = 2
+
+/// This is used to fatten AABBs in the dynamic tree. This allows proxies
+/// to move by a small amount without triggering a tree adjustment.
+/// This is in meters.
+const val b2_aabbExtension = b2_lengthUnitsPerMeter * 0.1
+
+/// This is used to fatten AABBs in the dynamic tree. This is used to predict
+/// the future position based on the current displacement.
+/// This is a dimensionless multiplier.
+const val b2_aabbMultiplier = 4.0
+
+/// A small length used as a collision and constraint tolerance. Usually it is
+/// chosen to be numerically significant, but visually insignificant. In meters.
+const val b2_linearSlop = 0.005 * b2_lengthUnitsPerMeter
+
+/// A small angle used as a collision and constraint tolerance. Usually it is
+/// chosen to be numerically significant, but visually insignificant.
+const val b2_angularSlop = 2.0 / 180.0 * PI
+
+/// The radius of the polygon/edge shape skin. This should not be modified. Making
+/// this smaller means polygons will have an insufficient buffer for continuous collision.
+/// Making it larger may create artifacts for vertex collision.
+const val b2_polygonRadius = 2.0 * b2_linearSlop
+
+/// Maximum number of sub-steps per contact in continuous physics simulation.
+const val b2_maxSubSteps = 8
+
+// Dynamics
+
+/// Maximum number of contacts to be handled to solve a TOI impact.
+const val b2_maxTOIContacts = 32
+
+/// The maximum linear position correction used when solving constraints. This helps to
+/// prevent overshoot. Meters.
+const val b2_maxLinearCorrection = (0.2 * b2_lengthUnitsPerMeter)
+
+/// The maximum angular position correction used when solving constraints. This helps to
+/// prevent overshoot.
+const val b2_maxAngularCorrection = (8.0 / 180.0 * PI)
+
+/// The maximum linear translation of a body per step. This limit is very large and is used
+/// to prevent numerical problems. You shouldn't need to adjust this. Meters.
+const val b2_maxTranslation = (2.0 * b2_lengthUnitsPerMeter)
+const val b2_maxTranslationSquared = (b2_maxTranslation * b2_maxTranslation)
+
+/// The maximum angular velocity of a body. This limit is very large and is used
+/// to prevent numerical problems. You shouldn't need to adjust this.
+const val b2_maxRotation = (0.5 * PI)
+const val b2_maxRotationSquared = (b2_maxRotation * b2_maxRotation)
+
+/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so
+/// that overlap is removed in one time step. However using values close to 1 often lead
+/// to overshoot.
+const val b2_baumgarte = 0.2
+const val b2_toiBaumgarte = 0.75
+
+// Sleep
+
+/// The time that a body must be still before it will go to sleep.
+const val b2_timeToSleep = 0.5
+
+/// A body cannot sleep if its linear velocity is above this tolerance.
+const val b2_linearSleepTolerance = (0.01 * b2_lengthUnitsPerMeter)
+
+/// A body cannot sleep if its angular velocity is above this tolerance.
+const val b2_angularSleepTolerance = (2.0f / 180.0f * PI)
+
+const val b2_epsilon = 1E-9

src/main/kotlin/ru/dbotthepony/kbox2d/api/Contact.kt

@@ -0,0 +1,146 @@
+package ru.dbotthepony.kbox2d.api
+
+import kotlin.math.sqrt
+
+/// Friction mixing law. The idea is to allow either fixture to drive the friction to zero.
+/// For example, anything slides on ice.
+fun b2MixFriction(friction1: Double, friction2: Double): Double {
+	return sqrt(friction1 * friction2)
+}
+
+/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.
+/// For example, a superball bounces on anything.
+fun b2MixRestitution(restitution1: Double, restitution2: Double): Double {
+	return if (restitution1 > restitution2) restitution1 else restitution2
+}
+
+/// Restitution mixing law. This picks the lowest value.
+fun b2MixRestitutionThreshold(threshold1: Double, threshold2: Double): Double {
+	return if (threshold1 < threshold2) threshold1 else threshold2
+}
+
+data class ContactRegister(
+	val primary: Boolean,
+)
+
+/**
+ * A contact edge is used to connect bodies and contacts together
+ * in a contact graph where each body is a node and each contact
+ * is an edge. A contact edge belongs to a doubly linked list
+ * maintained in each attached body. Each contact has two contact
+ * nodes, one for each attached body.
+ */
+data class ContactEdge(
+	val other: ru.dbotthepony.kbox2d.api.IBody, ///< provides quick access to the other body attached.
+	val contact: IContact, ///< the contact
+	var prev: ContactEdge? = null, ///< the previous contact edge in the body's contact list
+	var next: ContactEdge? = null, ///< the next contact edge in the body's contact list
+)
+
+enum class ContactFlags(val bitmask: Int) {
+	ISLAND(0x1),
+	TOUCHING(0x2),
+	ENABLED(0x4),
+	FILTER(0x8),
+	BULLET_HIT(0x10),
+	TOI(0x20);
+
+	fun isit(other: Int): Boolean {
+		return (other and bitmask) == bitmask
+	}
+
+	fun update(other: Int, state: Boolean): Int {
+		if (state)
+			return or(other)
+		else
+			return not(other)
+	}
+
+	fun and(other: Int): Int {
+		return other and bitmask
+	}
+
+	fun or(other: Int): Int {
+		return other or bitmask
+	}
+
+	fun not(other: Int): Int {
+		return other and bitmask.inv()
+	}
+}
+
+/**
+ * The class manages contact between two shapes. A contact exists for each overlapping
+ * AABB in the broad-phase (except if filtered). Therefore a contact object may exist
+ * that has no contact points.
+ */
+interface IContact {
+	/// Get the contact manifold. Do not modify the manifold unless you understand the
+	/// internals of Box2D.
+	val manifold: Manifold
+
+	/// Get the world manifold.
+	val worldManifold: IWorldManifold
+
+	/// Is this contact touching?
+	val isTouching: Boolean
+
+	/// Enable/disable this contact. This can be used inside the pre-solve
+	/// contact listener. The contact is only disabled for the current
+	/// time step (or sub-step in continuous collisions).
+	var isEnabled: Boolean
+
+	/// Get the next contact in the world's contact list.
+	val next: IContact?
+	val prev: IContact?
+
+	/// Get fixture A in this contact.
+	val fixtureA: IFixture
+
+	/// Get the child primitive index for fixture A.
+	val childIndexA: Int
+
+	/// Get fixture B in this contact.
+	val fixtureB: IFixture
+
+	/// Get the child primitive index for fixture B.
+	val childIndexB: Int
+
+	/// Override the default friction mixture. You can call this in b2ContactListener::PreSolve.
+	/// This value persists until set or reset.
+	/// Get the friction.
+	var friction: Double
+
+	/// Reset the friction mixture to the default value.
+	fun resetFriction() {
+		friction = b2MixFriction(fixtureA.friction, fixtureB.friction)
+	}
+
+	fun flagForFiltering()
+
+	/// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve.
+	/// The value persists until you set or reset.
+	var restitution: Double
+
+	/// Reset the restitution to the default value.
+	fun resetRestitution() {
+		restitution = b2MixRestitution(fixtureA.restitution, fixtureB.restitution)
+	}
+
+	/// Override the default restitution velocity threshold mixture. You can call this in b2ContactListener::PreSolve.
+	/// The value persists until you set or reset.
+	/// Get the restitution threshold.
+	var restitutionThreshold: Double
+
+	/// Reset the restitution threshold to the default value.
+	fun resetRestitutionThreshold() {
+		restitutionThreshold = b2MixRestitutionThreshold(fixtureA.restitutionThreshold, fixtureB.restitutionThreshold)
+	}
+
+	/// Set the desired tangent speed for a conveyor belt behavior. In meters per second.
+	/// Get the desired tangent speed. In meters per second.
+	var tangentSpeed: Double
+
+	/// Evaluate this contact with your own manifold and transforms.
+	fun evaluate(xfA: Transform, xfB: Transform): Manifold
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/ContactManager.kt

@@ -0,0 +1,37 @@
+package ru.dbotthepony.kbox2d.api
+
+/**
+ * Delegate of b2World.
+ */
+interface IContactManager {
+	fun addPair(proxyUserDataA: Any?, proxyUserDataB: Any?)
+
+	fun findNewContacts()
+
+	fun destroy(contact: IContact)
+
+	fun collide()
+
+	val broadPhase: IBroadPhase
+	var contactList: IContact?
+
+	val contactListIterator: Iterator<IContact> get() {
+		return object : Iterator<IContact> {
+			private var node = contactList
+
+			override fun hasNext(): Boolean {
+				return node != null
+			}
+
+			override fun next(): IContact {
+				val old = node!!
+				node = old.next
+				return old
+			}
+		}
+	}
+
+	val contactCount: Int
+	var contactFilter: IContactFilter?
+	var contactListener: IContactListener?
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/Distance.kt

@@ -0,0 +1,72 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+/**
+ * A distance proxy is used by the GJK algorithm.
+ * It encapsulates any shape.
+ */
+interface IDistanceProxy {
+	val vertices: List<Vector2d>
+	val radius: Double
+
+	/**
+	 * Get the supporting vertex index in the given direction.
+	 */
+	fun getSupport(d: Vector2d): Int
+
+	/**
+	 * Get the supporting vertex in the given direction.
+	 */
+	fun getSupportVertex(d: Vector2d): Vector2d
+}
+
+/**
+ * Used to warm start b2Distance.
+ * Set count to zero on first call.
+ */
+data class SimplexCache(
+	val metric: Double = 0.0, ///< length or area
+	val count: Int = 0,
+	val indexA: IntArray = IntArray(0), ///< vertices on shape A
+	val indexB: IntArray = IntArray(0), ///< vertices on shape B
+)
+
+/**
+ * Input for b2Distance.
+ * You have to option to use the shape radii
+ * in the computation. Even
+ */
+data class DistanceInput(
+	var proxyA: IDistanceProxy,
+	var proxyB: IDistanceProxy,
+	var transformA: Transform = Transform(),
+	var transformB: Transform = Transform(),
+	var useRadii: Boolean = false
+)
+
+/**
+ * Output for b2Distance.
+ */
+data class DistanceOutput(
+	val pointA: Vector2d,		///< closest point on shapeA
+	val pointB: Vector2d,		///< closest point on shapeB
+	val distance: Double,
+	val iterations: Int,	///< number of GJK iterations used
+	val newCache: SimplexCache
+)
+
+data class ShapeCastInput(
+	var proxyA: IDistanceProxy,
+	var proxyB: IDistanceProxy,
+	var transformA: Transform,
+	var transformB: Transform,
+	var translationB: Vector2d,
+)
+
+data class ShapeCastOutput(
+	var point: Vector2d = Vector2d.ZERO,
+	var normal: Vector2d = Vector2d.ZERO,
+	var lambda: Double = 1.0,
+	var iterations: Int = 0,
+)

src/main/kotlin/ru/dbotthepony/kbox2d/api/DynamicTree.kt

@@ -0,0 +1,76 @@
+package ru.dbotthepony.kbox2d.api
+
+import it.unimi.dsi.fastutil.ints.IntArraySet
+import ru.dbotthepony.kbox2d.collision.DynamicTree
+import ru.dbotthepony.kbox2d.collision.b2_nullNode
+import ru.dbotthepony.kstarbound.math.AABB
+
+private const val DEBUG_CYCLIC_REFERENCES = true
+
+/// A node in the dynamic tree. The client does not interact with this directly.
+data class TreeNode(
+	val tree: DynamicTree,
+	/// Enlarged AABB
+	var aabb: AABB? = null,
+
+	var child1: Int = 0,
+	var child2: Int = 0,
+
+	// leaf = 0, free node = -1
+	var height: Int = 0,
+
+	var moved: Boolean = false,
+
+	var userData: Any? = null,
+) {
+	val isLeaf get() = child1 == b2_nullNode
+
+	// union
+	private var _union: Int = b2_nullNode
+
+	var parent by this::_union
+	var next by this::_union
+
+	internal fun validate() {
+		if (!DEBUG_CYCLIC_REFERENCES)
+			return
+
+		val seen = IntArraySet()
+		var parent = parent
+
+		while (parent != b2_nullNode) {
+			if (!seen.add(parent)) {
+				throw IllegalStateException("Cycle detected: $seen")
+			}
+
+			seen.add(parent)
+			parent = tree.nodes[parent].parent
+		}
+	}
+}
+
+interface IDynamicTree : IProxieable, IMovable {
+	fun wasMoved(proxyID: Int): Boolean
+	fun clearMoved(proxyID: Int)
+
+	/**
+	 * Validate this tree. For testing.
+	 */
+	fun validate()
+
+	/**
+	 * Compute the height of the binary tree in O(N) time. Should not be
+	 * called often.
+	 */
+	val height: Int
+
+	/// Get the maximum balance of an node in the tree. The balance is the difference
+	/// in height of the two children of a node.
+	val maxBalance: Int
+
+	/// Get the ratio of the sum of the node areas to the root area.
+	val getAreaRatio: Double
+
+	/// Build an optimal tree. Very expensive. For testing.
+	fun rebuildBottomUp()
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/Fixture.kt

@@ -0,0 +1,181 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+sealed interface IFilter {
+	/**
+	 * The collision category bits. Normally you would just set one bit.
+	 */
+	val categoryBits: Long
+
+	/**
+	 * The collision mask bits. This states the categories that this
+	 * shape would accept for collision.
+	 */
+	val maskBits: Long
+
+	/**
+	 * Collision groups allow a certain group of objects to never collide (negative)
+	 * or always collide (positive). Zero means no collision group. Non-zero group
+	 * filtering always wins against the mask bits.
+	 */
+	val groupIndex: Int
+}
+
+data class Filter(
+	override var categoryBits: Long = 0x1L,
+	override var maskBits: Long = 0xFFFFL,
+	override var groupIndex: Int = 0,
+) : IFilter {
+	fun immutable(): ImmutableFilter {
+		return ImmutableFilter(categoryBits, maskBits, groupIndex)
+	}
+}
+
+data class ImmutableFilter(
+	override val categoryBits: Long,
+	override val maskBits: Long,
+	override val groupIndex: Int,
+) : IFilter
+
+data class FixtureDef(
+	/// The shape, this must be set. The shape will be cloned.
+	var shape: IShape<*>? = null,
+
+	var friction: Double = 0.2,
+
+	/// The restitution (elasticity) usually in the range [0,1].
+	var restitution: Double = 0.0,
+
+	/// Restitution velocity threshold, usually in m/s. Collisions above this
+	/// speed have restitution applied (will bounce).
+	var restitutionThreshold: Double = 1.0 * b2_lengthUnitsPerMeter,
+
+	/// The density, usually in kg/m^2.
+	var density: Double = 0.0,
+
+	/// A sensor shape collects contact information but never generates a collision
+	/// response.
+	var isSensor: Boolean = false,
+
+	/// Use this to store application specific fixture data.
+	var userData: Any? = null,
+
+	val filter: Filter = Filter()
+)
+
+data class FixtureProxy(
+	var aabb: AABB,
+	val fixture: IFixture,
+	val childIndex: Int,
+) {
+	private var setProxyID = false
+
+	var proxyId: Int = e_nullProxy
+		set(value) {
+			if (!setProxyID) {
+				field = value
+				setProxyID = true
+				return
+			}
+
+			throw IllegalStateException("FixtureProxy should be immutable (tried to set value $value, already having $field)")
+		}
+}
+
+/**
+ * A fixture is used to attach a shape to a body for collision detection. A fixture
+ * inherits its transform from its parent. Fixtures hold additional non-geometric data
+ * such as friction, collision filters, etc.
+ * Fixtures are created via b2Body::CreateFixture.
+ * @warning you cannot reuse fixtures.
+ */
+interface IFixture {
+	/// Get the type of the child shape. You can use this to down cast to the concrete shape.
+	/// @return the shape type.
+	val type: IShape.Type
+
+	/// Get the child shape. You can modify the child shape, however you should not change the
+	/// number of vertices because this will crash some collision caching mechanisms.
+	/// Manipulating the shape may lead to non-physical behavior.
+	val shape: IShape<*>
+
+	/// Set if this fixture is a sensor.
+	/// Is this fixture a sensor (non-solid)?
+	/// @return the true if the shape is a sensor.
+	var isSensor: Boolean
+
+	/// Set the contact filtering data. This will not update contacts until the next time
+	/// step when either parent body is active and awake.
+	/// This automatically calls Refilter.
+	var filter: IFilter
+
+	/// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide.
+	fun refilter()
+
+	/// Get the parent body of this fixture. This is nullptr if the fixture is not attached.
+	/// @return the parent body.
+	val body: ru.dbotthepony.kbox2d.api.IBody?
+
+	/// Get the next fixture in the parent body's fixture list.
+	/// @return the next shape.
+	val next: IFixture?
+
+	/// Get the user data that was assigned in the fixture definition. Use this to
+	/// store your application specific data.
+	val userData: Any?
+
+	/// Test a point for containment in this fixture.
+	/// @param p a point in world coordinates.
+	fun testPoint(p: Vector2d): Boolean {
+		return shape.testPoint(checkNotNull(body) { "Tried to use detached fixture" }.transform, p)
+	}
+
+	/// Cast a ray against this shape.
+	/// @param output the ray-cast results.
+	/// @param input the ray-cast input parameters.
+	/// @param childIndex the child shape index (e.g. edge index)
+	fun rayCast(input: RayCastInput, childIndex: Int): RayCastOutput {
+		return shape.rayCast(input, checkNotNull(body) { "Tried to use detached fixture" }.transform, childIndex)
+	}
+
+	/// Get the mass data for this fixture. The mass data is based on the density and
+	/// the shape. The rotational inertia is about the shape's origin. This operation
+	/// may be expensive.
+	fun getMassData(): MassData {
+		return shape.computeMass(density)
+	}
+
+	/// Set the density of this fixture. This will _not_ automatically adjust the mass
+	/// of the body. You must call b2Body::ResetMassData to update the body's mass.
+	/// Get the density of this fixture.
+	var density: Double
+
+	/// Get the coefficient of friction.
+	/// Set the coefficient of friction. This will _not_ change the friction of
+	/// existing contacts.
+	var friction: Double
+
+	/// Get the coefficient of restitution.
+	/// Set the coefficient of restitution. This will _not_ change the restitution of
+	/// existing contacts.
+	var restitution: Double
+
+	/// Get the restitution velocity threshold.
+	/// Set the restitution threshold. This will _not_ change the restitution threshold of
+	/// existing contacts.
+	var restitutionThreshold: Double
+
+	/// Get the fixture's AABB. This AABB may be enlarge and/or stale.
+	/// If you need a more accurate AABB, compute it using the shape and
+	/// the body transform.
+	fun getAABB(childIndex: Int): AABB {
+		return proxies[childIndex].aabb
+	}
+
+	/// Dump this fixture to the log file. (Log4J)
+	fun dump(childIndex: Int) { }
+
+	val proxies: List<FixtureProxy>
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/IDebugDraw.kt

@@ -0,0 +1,52 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.util.Color
+
+/**
+ * Implement and register this class with a b2World to provide debug drawing of physics
+ * entities in your game.
+ */
+interface IDebugDraw {
+	var drawShapes: Boolean
+	var drawJoints: Boolean
+	var drawAABB: Boolean
+	var drawPairs: Boolean
+	var drawCenterOfMess: Boolean
+
+	/**
+	 * Draw a closed polygon provided in CCW order.
+	 */
+	fun drawPolygon(vertices: List<Vector2d>, color: Color)
+
+	/**
+	 * Draw a solid closed polygon provided in CCW order.
+	 */
+	fun drawSolidPolygon(vertices: List<Vector2d>, color: Color)
+
+	/**
+	 * Draw a circle.
+	 */
+	fun drawCircle(center: Vector2d, radius: Double, color: Color)
+
+	/**
+	 * Draw a solid circle.
+	 */
+	fun drawSolidCircle(center: Vector2d, radius: Double, axis: Vector2d, color: Color)
+
+	/**
+	 * Draw a line segment.
+	 */
+	fun drawSegment(p1: Vector2d, p2: Vector2d, color: Color)
+
+	/**
+	 * Draw a transform. Choose your own length scale.
+	 * @param xf a transform.
+	 */
+	fun drawTransform(xf: Transform)
+
+	/**
+	 * Draw a point.
+	 */
+	fun drawPoint(p: Vector2d, size: Double, color: Color)
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/IMovable.kt

@@ -0,0 +1,12 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+interface IMovable {
+	/**
+	 * Shift the world origin. Useful for large worlds.
+	 * The shift formula is: position -= newOrigin
+	 * @param newOrigin the new origin with respect to the old origin
+	 */
+	fun shiftOrigin(newOrigin: Vector2d)
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/IProxieable.kt

@@ -0,0 +1,76 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+fun interface ProxyQueryCallback {
+	fun invoke(nodeId: Int, userData: Any?): Boolean
+}
+
+fun interface ProxyRayCastCallback {
+	fun invoke(subInput: RayCastInput, nodeId: Int, userData: Any?): Double
+}
+
+sealed interface IProxieable {
+	/**
+	 * For [IBroadPhase]:
+	 * Create a proxy with an initial AABB. Pairs are not reported until
+	 * UpdatePairs is called.
+	 *
+	 * For [IDynamicTree]:
+	 * Create a proxy. Provide a tight fitting AABB and a userData pointer.
+	 */
+	fun createProxy(aabb: AABB, userData: Any?): Int
+
+	/**
+	 * For [IBroadPhase]:
+	 * Destroy a proxy. It is up to the client to remove any pairs.
+	 *
+	 * For [IDynamicTree]:
+	 * Destroy a proxy. This asserts if the id is invalid.
+	 */
+	fun destroyProxy(proxyID: Int)
+
+	/**
+	 * For [IBroadPhase]:
+	 * Call MoveProxy as many times as you like, then when you are done
+	 * call UpdatePairs to finalized the proxy pairs (for your time step).
+	 * @return true
+	 *
+	 * For [IDynamicTree]:
+	 * Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
+	 * then the proxy is removed from the tree and re-inserted. Otherwise
+	 * the function returns immediately.
+	 * @return true if the proxy was re-inserted.
+	 */
+	fun moveProxy(proxyID: Int, aabb: AABB, displacement: Vector2d): Boolean
+
+	/**
+	 * Get user data from a proxy. Returns nullptr if the id is invalid.
+ 	 */
+	fun getUserData(proxyID: Int): Any?
+
+	/**
+	 * Get the fat AABB for a proxy.
+	 */
+	fun getFatAABB(proxyID: Int): AABB
+
+	/**
+	 * Query an AABB for overlapping proxies. The callback class
+	 * is called for each proxy that overlaps the supplied AABB.
+	 *
+	 * @return Whenever callback returned false
+	 */
+	fun query(aabb: AABB, callback: ProxyQueryCallback): Boolean
+
+	/**
+	 * Ray-cast against the proxies in the tree. This relies on the callback
+	 * to perform a exact ray-cast in the case were the proxy contains a shape.
+	 * The callback also performs the any collision filtering. This has performance
+	 * roughly equal to k * log(n), where k is the number of collisions and n is the
+	 * number of proxies in the tree.
+	 * @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+	 * @param callback a callback class that is called for each proxy that is hit by the ray.
+	 */
+	fun rayCast(input: RayCastInput, callback: ProxyRayCastCallback)
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/Joint.kt

@@ -0,0 +1,773 @@
+package ru.dbotthepony.kbox2d.api
+
+import org.apache.logging.log4j.LogManager
+import ru.dbotthepony.kbox2d.dynamics.joint.AbstractJoint
+import ru.dbotthepony.kstarbound.math.Vector2d
+import kotlin.math.PI
+
+data class StiffnessResult(val stiffness: Double, val damping: Double)
+
+/**
+ * Utility to compute linear stiffness values from frequency and damping ratio
+ */
+fun b2LinearStiffness(
+	frequencyHertz: Double,
+	dampingRatio: Double,
+	bodyA: IBody?,
+	bodyB: IBody?,
+): StiffnessResult {
+	val massA = bodyA?.mass ?: 0.0
+	val massB = bodyB?.mass ?: 0.0
+	val mass: Double
+
+	if (massA > 0.0 && massB > 0.0) {
+		mass = massA * massB / (massA + massB)
+	} else if (massA > 0.0) {
+		mass = massA
+	} else {
+		mass = massB
+	}
+
+	val omega = 2.0 * PI * frequencyHertz
+
+	return StiffnessResult(
+		stiffness = mass * omega * omega,
+		damping = 2.0 * mass * dampingRatio * omega
+	)
+}
+
+/**
+ * Utility to compute rotational stiffness values frequency and damping ratio
+ */
+fun b2AngularStiffness(
+	frequencyHertz: Double,
+	dampingRatio: Double,
+	bodyA: IBody?,
+	bodyB: IBody?,
+): StiffnessResult {
+	val inertiaA = bodyA?.inertia ?: 0.0
+	val inertiaB = bodyB?.inertia ?: 0.0
+	val inertia: Double
+
+	if (inertiaA > 0.0 && inertiaB > 0.0) {
+		inertia = inertiaA * inertiaB / (inertiaA + inertiaB)
+	} else if (inertiaA > 0.0) {
+		inertia = inertiaA
+	} else {
+		inertia = inertiaB
+	}
+
+	val omega = 2.0 * PI * frequencyHertz
+
+	return StiffnessResult(
+		stiffness = inertia * omega * omega,
+		damping = 2.0 * inertia * dampingRatio * omega
+	)
+}
+
+enum class JointType {
+	REVOLUTE,
+	PRISMATIC,
+	DISTANCE,
+	PULLEY,
+	MOUSE,
+	GEAR,
+	WHEEL,
+	WELD,
+	FRICTION,
+	MOTOR
+}
+
+data class Jacobian(
+	val linear: Vector2d,
+	val angularA: Double,
+	val angularB: Double
+)
+
+/**
+ * A joint edge is used to connect bodies and joints together
+ * in a joint graph where each body is a node and each joint
+ * is an edge. A joint edge belongs to a doubly linked list
+ * maintained in each attached body. Each joint has two joint
+ * nodes, one for each attached body.
+ */
+class JointEdge(
+	other: IBody?, ///< provides quick access to the other body attached.
+	val joint: IJoint, ///< the joint
+	var prev: JointEdge? = null, ///< the previous joint edge in the body's joint list
+	var next: JointEdge? = null ///< the next joint edge in the body's joint list
+) {
+	val otherNullable: IBody? = other
+	val other: IBody get() = checkNotNull(otherNullable) { "Other body is not present" }
+}
+
+sealed interface IJointDef {
+	/**
+	 * The joint type is set automatically for concrete joint types.
+	 */
+	val type: JointType
+
+	/**
+	 * The first attached body.
+	 */
+	val bodyA: IBody?
+
+	/**
+	 * The second attached body.
+	 */
+	val bodyB: IBody?
+
+	/**
+	 * Set this flag to true if the attached bodies should collide.
+	 */
+	var collideConnected: Boolean
+
+	/**
+	 * Use this to attach application specific data to your joints.
+	 */
+	var userData: Any?
+}
+
+class DistanceJointDef(
+	b1: IBody,
+	b2: IBody,
+	anchor1: Vector2d,
+	anchor2: Vector2d
+) : IJointDef {
+	override val type: JointType = JointType.DISTANCE
+
+	/**
+	 * The rest length of this joint. Clamped to a stable minimum value.
+	 */
+	var length: Double
+
+	/**
+	 * Minimum length. Clamped to a stable minimum value.
+	 */
+	var minLength: Double
+
+	/**
+	 * Maximum length. Must be greater than or equal to the minimum length.
+	 */
+	var maxLength: Double
+
+	/**
+	 * The linear stiffness in N/m.
+	 */
+	val stiffness: Double = 0.0
+
+	/**
+	 * The linear damping in N*s/m.
+	 */
+	val damping: Double = 0.0
+
+	override var bodyA: IBody = b1
+	override var bodyB: IBody = b2
+
+	override var collideConnected: Boolean = false
+	override var userData: Any? = null
+
+	/**
+	 * The local anchor point relative to bodyA's origin.
+	 */
+	var localAnchorA: Vector2d = bodyA.getLocalPoint(anchor1)
+
+	/**
+	 * The local anchor point relative to bodyB's origin.
+	 */
+	var localAnchorB: Vector2d = bodyB.getLocalPoint(anchor2)
+
+	init {
+		val d = anchor2 - anchor1
+		length = b2Max(d.length, b2_linearSlop)
+		minLength = length
+		maxLength = length
+	}
+}
+
+class RevoluteJointDef(
+	b1: IBody,
+	b2: IBody,
+	anchor: Vector2d,
+) : IJointDef {
+	override val type: JointType = JointType.REVOLUTE
+	override var bodyA: IBody = b1
+	override var bodyB: IBody = b2
+	override var collideConnected: Boolean = false
+	override var userData: Any? = null
+
+	/**
+	 * The local anchor point relative to bodyA's origin.
+	 */
+	var localAnchorA = bodyA.getLocalPoint(anchor)
+
+	/**
+	 * The local anchor point relative to bodyB's origin.
+	 */
+	var localAnchorB = bodyB.getLocalPoint(anchor)
+
+	/**
+	 * The bodyB angle minus bodyA angle in the reference state (radians).
+	 */
+	var referenceAngle = bodyB.angle - bodyA.angle
+
+	/**
+	 * A flag to enable joint limits.
+	 */
+	var enableLimit = false
+
+	/**
+	 * The lower angle for the joint limit (radians).
+	 */
+	var lowerAngle = 0.0
+
+	/**
+	 * The upper angle for the joint limit (radians).
+	 */
+	var upperAngle = 0.0
+
+	/**
+	 * A flag to enable the joint motor.
+	 */
+	var enableMotor = false
+
+	/**
+	 * The desired motor speed. Usually in radians per second.
+	 */
+	var motorSpeed = 0.0
+
+	/**
+	 * The maximum motor torque used to achieve the desired motor speed.
+	 *
+	 * Usually in N-m.
+	 */
+	var maxMotorTorque = 0.0
+}
+
+class PrismaticJointDef(
+	b1: IBody,
+	b2: IBody,
+	anchor: Vector2d,
+	axis: Vector2d,
+) : IJointDef {
+	override val type: JointType = JointType.PRISMATIC
+	override var bodyA: IBody = b1
+	override var bodyB: IBody = b2
+	override var collideConnected: Boolean = false
+	override var userData: Any? = null
+
+	/**
+	 * The local anchor point relative to bodyA's origin.
+	 */
+	var localAnchorA = bodyA.getLocalPoint(anchor)
+
+	/**
+	 * The local anchor point relative to bodyB's origin.
+	 */
+	var localAnchorB = bodyB.getLocalPoint(anchor)
+
+	/**
+	 * The local translation unit axis in bodyA.
+	 */
+	var localAxisA = bodyA.getLocalVector(axis)
+
+	/**
+	 * The constrained angle between the bodies: bodyB_angle - bodyA_angle.
+	 */
+	var referenceAngle = bodyB.angle - bodyA.angle
+
+	/**
+	 * Enable/disable the joint limit.
+	 */
+	var enableLimit = false
+
+	/**
+	 * The lower translation limit, usually in meters.
+	 */
+	var lowerTranslation = 0.0
+
+	/**
+	 * The upper translation limit, usually in meters.
+	 */
+	var upperTranslation = 0.0
+
+	/**
+	 * Enable/disable the joint motor.
+	 */
+	var enableMotor = false
+
+	/**
+	 * The maximum motor torque, usually in N-m.
+	 */
+	var maxMotorForce = 0.0
+
+	/**
+	 * The desired motor speed in radians per second.
+	 */
+	var motorSpeed = 0.0
+}
+
+/**
+ * Pulley joint definition. This requires two ground anchors,
+ * two dynamic body anchor points, and a pulley ratio.
+ */
+class PulleyJointDef(
+	b1: IBody,
+	b2: IBody,
+
+	/**
+	 * The first ground anchor in world coordinates. This point never moves.
+	 */
+	var groundAnchorA: Vector2d,
+
+	/**
+	 * The second ground anchor in world coordinates. This point never moves.
+	 */
+	var groundAnchorB: Vector2d,
+
+	anchorA: Vector2d,
+	anchorB: Vector2d,
+	ratio: Double,
+) : IJointDef {
+	override val type: JointType = JointType.PULLEY
+	override var bodyA: IBody = b1
+	override var bodyB: IBody = b2
+	override var collideConnected: Boolean = false
+	override var userData: Any? = null
+
+	/**
+	 * The pulley ratio, used to simulate a block-and-tackle.
+	 */
+	var ratio: Double = ratio
+		set(value) {
+			require(value > b2_epsilon) { "Ratio is too small: $value" }
+			field = value
+		}
+
+	init {
+		// KBox2D: trigger sanity check at least once
+		this.ratio = ratio
+	}
+
+	/**
+	 * The local anchor point relative to bodyA's origin.
+	 */
+	var localAnchorA = bodyA.getLocalPoint(anchorA)
+
+	/**
+	 * The local anchor point relative to bodyB's origin.
+	 */
+	var localAnchorB = bodyB.getLocalPoint(anchorB)
+
+	/**
+	 * The a reference length for the segment attached to bodyA.
+	 */
+	var lengthA: Double = (anchorA - groundAnchorA).length
+
+	/**
+	 * The b reference length for the segment attached to bodyB.
+	 */
+	var lengthB: Double = (anchorB - groundAnchorB).length
+}
+
+/**
+ * Gear joint definition. This definition requires two existing
+ * revolute or prismatic joints (any combination will work).
+ * @warning bodyB on the input joints must both be dynamic
+ */
+class GearJointDef(
+	override var bodyA: IBody,
+	override var bodyB: IBody,
+
+	/**
+	 * The first revolute/prismatic joint attached to the gear joint.
+	 */
+	var joint1: AbstractJoint,
+
+	/**
+	 * The second revolute/prismatic joint attached to the gear joint.
+	 */
+	var joint2: AbstractJoint,
+
+	/**
+	 * The gear ratio.
+	 * @see ru.dbotthepony.kbox2d.dynamics.joint.GearJoint for explanation.
+	 */
+	var ratio: Double,
+) : IJointDef {
+	override val type: JointType = JointType.GEAR
+	override var collideConnected: Boolean = false
+	override var userData: Any? = null
+}
+
+/**
+ * Mouse joint definition. This requires a world target point,
+ * tuning parameters, and the time step.
+ *
+ * KBox2D: In Box2D, Body A is not used and you put meaningless data into it just to not make it crash.
+ *
+ * In KBox2D, Body A is also meaningless, but you are not required to put anything it (leave as null).
+ *
+ * Putting anything in Body A or leaving it as null has no effect, other than bloating body A joint linked list.
+ */
+class MouseJointDef(
+	/**
+	 * The initial world target point. This is assumed
+	 * to coincide with the body anchor initially.
+     */
+	var target: Vector2d,
+
+	override var bodyB: IBody,
+	override val bodyA: IBody? = null,
+
+	/**
+	 * The maximum constraint force that can be exerted
+	 * to move the candidate body. Usually you will express
+	 * as some multiple of the weight (multiplier * mass * gravity).
+	 */
+	var maxForce: Double = 0.0,
+	var stiffness: Double = 0.0,
+	var damping: Double = 0.0,
+) : IJointDef {
+	override val type: JointType = JointType.MOUSE
+
+	override var collideConnected: Boolean = false
+	override var userData: Any? = null
+
+	fun linearStiffness(
+		frequencyHertz: Double,
+		dampingRatio: Double,
+		bodyA: IBody?,
+		bodyB: IBody?
+	): MouseJointDef {
+		val (stiffness, damping) = b2LinearStiffness(frequencyHertz, dampingRatio, bodyA, bodyB)
+		this.stiffness = stiffness
+		this.damping = damping
+		return this
+	}
+
+	fun angularStiffness(
+		frequencyHertz: Double,
+		dampingRatio: Double,
+		bodyA: IBody?,
+		bodyB: IBody?
+	): MouseJointDef {
+		val (stiffness, damping) = b2AngularStiffness(frequencyHertz, dampingRatio, bodyA, bodyB)
+		this.stiffness = stiffness
+		this.damping = damping
+		return this
+	}
+}
+
+/**
+ * Wheel joint definition. This requires defining a line of
+ * motion using an axis and an anchor point. The definition uses local
+ * anchor points and a local axis so that the initial configuration
+ * can violate the constraint slightly. The joint translation is zero
+ * when the local anchor points coincide in world space. Using local
+ * anchors and a local axis helps when saving and loading a game.
+ */
+class WheelJointDef(
+	override var bodyA: IBody,
+	override var bodyB: IBody,
+	anchor: Vector2d,
+	axis: Vector2d,
+
+	/**
+	 * Enable/disable the joint limit.
+	 */
+	var enableLimit: Boolean = false,
+
+	/**
+	 * The lower translation limit, usually in meters.
+	 */
+	var lowerTranslation: Double = 0.0,
+
+	/**
+	 * The upper translation limit, usually in meters.
+	 */
+	var upperTranslation: Double = 0.0,
+
+	/**
+	 * Enable/disable the joint motor.
+	 */
+	var enableMotor: Boolean = false,
+
+	/**
+	 * The maximum motor torque, usually in N-m.
+	 */
+	var maxMotorTorque: Double = 0.0,
+
+	/**
+	 * The desired motor speed in radians per second.
+	 */
+	var motorSpeed: Double = 0.0,
+
+	/**
+	 * Suspension stiffness. Typically in units N/m.
+	 */
+	var stiffness: Double = 0.0,
+
+	/**
+	 * Suspension damping. Typically in units of N*s/m.
+	 */
+	var damping: Double = 0.0,
+) : IJointDef {
+	override val type: JointType = JointType.WHEEL
+	override var collideConnected: Boolean = false
+	override var userData: Any? = null
+
+	/**
+	 * The local anchor point relative to bodyA's origin.
+	 */
+	var localAnchorA: Vector2d = bodyA.getLocalPoint(anchor)
+
+	/**
+	 * The local anchor point relative to bodyB's origin.
+	 */
+	var localAnchorB: Vector2d = bodyB.getLocalPoint(anchor)
+
+	/**
+	 * The local translation axis in bodyA.
+	 */
+	var localAxisA: Vector2d = bodyA.getLocalVector(axis)
+
+	fun linearStiffness(
+		frequencyHertz: Double,
+		dampingRatio: Double,
+		bodyA: IBody = this.bodyA,
+		bodyB: IBody = this.bodyB
+	): WheelJointDef {
+		val (stiffness, damping) = b2LinearStiffness(frequencyHertz, dampingRatio, bodyA, bodyB)
+		this.stiffness = stiffness
+		this.damping = damping
+		return this
+	}
+
+	fun angularStiffness(
+		frequencyHertz: Double,
+		dampingRatio: Double,
+		bodyA: IBody = this.bodyA,
+		bodyB: IBody = this.bodyB
+	): WheelJointDef {
+		val (stiffness, damping) = b2AngularStiffness(frequencyHertz, dampingRatio, bodyA, bodyB)
+		this.stiffness = stiffness
+		this.damping = damping
+		return this
+	}
+}
+
+/**
+ * Weld joint definition. You need to specify local anchor points
+ * where they are attached and the relative body angle. The position
+ * of the anchor points is important for computing the reaction torque.
+ */
+class WeldJointDef(
+	override var bodyA: IBody,
+	override var bodyB: IBody,
+	anchor: Vector2d,
+
+	/**
+	 * Suspension stiffness. Typically in units N/m.
+	 */
+	var stiffness: Double = 0.0,
+
+	/**
+	 * Suspension damping. Typically in units of N*s/m.
+	 */
+	var damping: Double = 0.0,
+) : IJointDef {
+	override val type: JointType = JointType.WELD
+	override var collideConnected: Boolean = false
+	override var userData: Any? = null
+
+	/**
+	 * The bodyB angle minus bodyA angle in the reference state (radians).
+	 */
+	var referenceAngle: Double = bodyB.angle - bodyA.angle
+
+	/**
+	 * The local anchor point relative to bodyA's origin.
+	 */
+	var localAnchorA: Vector2d = bodyA.getLocalPoint(anchor)
+
+	/**
+	 * The local anchor point relative to bodyB's origin.
+	 */
+	var localAnchorB: Vector2d = bodyB.getLocalPoint(anchor)
+
+	fun linearStiffness(
+		frequencyHertz: Double,
+		dampingRatio: Double,
+		bodyA: IBody = this.bodyA,
+		bodyB: IBody = this.bodyB
+	): WeldJointDef {
+		val (stiffness, damping) = b2LinearStiffness(frequencyHertz, dampingRatio, bodyA, bodyB)
+		this.stiffness = stiffness
+		this.damping = damping
+		return this
+	}
+
+	fun angularStiffness(
+		frequencyHertz: Double,
+		dampingRatio: Double,
+		bodyA: IBody = this.bodyA,
+		bodyB: IBody = this.bodyB
+	): WeldJointDef {
+		val (stiffness, damping) = b2AngularStiffness(frequencyHertz, dampingRatio, bodyA, bodyB)
+		this.stiffness = stiffness
+		this.damping = damping
+		return this
+	}
+}
+
+/**
+ * Friction joint definition.
+ */
+class FrictionJointDef(
+	override var bodyA: IBody,
+	override var bodyB: IBody,
+	anchor: Vector2d,
+
+	/**
+	 * The maximum friction force in N.
+	 */
+	var maxForce: Double = 0.0,
+
+	/**
+	 * The maximum friction torque in N-m.
+	 */
+	var maxTorque: Double = 0.0,
+
+	override var collideConnected: Boolean = false,
+	override var userData: Any? = null
+) : IJointDef {
+	override val type: JointType = JointType.FRICTION
+
+	/**
+	 * The local anchor point relative to bodyA's origin.
+	 */
+	var localAnchorA: Vector2d = bodyA.getLocalPoint(anchor)
+
+	/**
+	 * The local anchor point relative to bodyB's origin.
+	 */
+	var localAnchorB: Vector2d = bodyB.getLocalPoint(anchor)
+}
+
+class MotorJointDef(
+	override var bodyA: IBody,
+	override var bodyB: IBody,
+
+	/**
+	 * The maximum motor force in N.
+	 */
+	var maxForce: Double = 1.0,
+
+	/**
+	 * The maximum motor torque in N-m.
+	 */
+	var maxTorque: Double = 1.0,
+
+	override var collideConnected: Boolean = false,
+	override var userData: Any? = null
+) : IJointDef {
+	override val type: JointType = JointType.MOTOR
+
+	/**
+	 * Position of bodyB minus the position of bodyA, in bodyA's frame, in meters.
+	 */
+	var linearOffset: Vector2d = bodyA.getLocalPoint(bodyB.position)
+
+	/**
+	 * The bodyB angle minus bodyA angle in radians.
+	 */
+	var angularOffset: Double = bodyB.angle - bodyA.angle
+
+	/**
+	 * Position correction factor in the range [0,1].
+	 */
+	var correctionFactor: Double = 0.3
+		set(value) {
+			require(value in 0.0 .. 1.0) { "Invalid correction factor of $value" }
+			field = value
+		}
+}
+
+interface IJoint : IMovable {
+	/**
+	 * Get the type of the concrete joint.
+	 */
+	val type: JointType
+
+	/**
+	 * Get the first body attached to this joint.
+	 */
+	val bodyA: IBody
+
+	/**
+	 * Get the second body attached to this joint.
+	 */
+	val bodyB: IBody
+
+	/**
+	 * Get the anchor point on bodyA in world coordinates.
+	 */
+	val anchorA: Vector2d
+
+	/**
+	 * Get the anchor point on bodyB in world coordinates.
+	 */
+	val anchorB: Vector2d
+
+	/**
+	 * Get the reaction force on bodyB at the joint anchor in Newtons.
+	 */
+	fun getReactionForce(inv_dt: Double): Vector2d
+
+	/**
+	 * Get the reaction torque on bodyB in N*m.
+	 */
+	fun getReactionTorque(inv_dt: Double): Double
+
+	/**
+	 * Get the next joint the world joint list.
+	 *
+	 * Kotlin: This is not a list, but, as in C++ impl, a custom
+	 * linked list.
+	 */
+	val next: IJoint?
+	val prev: IJoint?
+
+	fun destroy() {
+		bodyA.world.destroyJoint(this)
+	}
+
+	/**
+	 * Short-cut function to determine if either body is enabled.
+	 */
+	val isEnabled: Boolean get() = bodyA.isEnabled || bodyB.isEnabled
+
+	/**
+	 * Get collide connected.
+	 * Note: modifying the collide connect flag won't work correctly because
+	 * the flag is only checked when fixture AABBs begin to overlap.
+	 */
+	val collideConnected: Boolean
+
+	/**
+	 * Dump this joint to the log file.
+	 */
+	fun dump() { LOGGER.warn("Dump is not supported for this join type: $this") }
+
+	/**
+	 * Debug draw this joint
+	 */
+	fun draw(draw: IDebugDraw) { }
+
+	var userData: Any?
+
+	companion object {
+		private val LOGGER = LogManager.getLogger()
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/Math.kt

@@ -0,0 +1,510 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.*
+import kotlin.math.*
+
+/// "Next Largest Power of 2
+/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
+/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
+/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
+/// largest power of 2. For a 32-bit value:"
+fun b2NextPowerOfTwo(x: Int): Int {
+	var x = x
+	x = x or (x ushr 1)
+	x = x or (x ushr 2)
+	x = x or (x ushr 4)
+	x = x or (x ushr 8)
+	x = x or (x ushr 16)
+	return x + 1
+}
+
+fun b2IsPowerOfTwo(x: Int): Boolean {
+	return x > 0 && (x and (x - 1)) == 0
+}
+
+/// Rotation
+class Rotation(
+	s: Double,
+	c: Double,
+) {
+	/// Initialize from an angle in radians
+	constructor(angle: Double) : this(sin(angle), cos(angle))
+
+	override fun toString(): String {
+		return "Rotation($s, $c)"
+	}
+
+	var s: Double = s
+		set(value) {
+			if (!value.isFinite()) {
+				throw IllegalArgumentException("Tried to set illegal non-finite sinus $value")
+			}
+
+			if (value.isNaN()) {
+				throw IllegalArgumentException("Tried to set illegal NaN sinus")
+			}
+
+			field = value
+		}
+
+	var c: Double = c
+		set(value) {
+			if (!value.isFinite()) {
+				throw IllegalArgumentException("Tried to set illegal non-finite cosine $value")
+			}
+
+			if (value.isNaN()) {
+				throw IllegalArgumentException("Tried to set illegal NaN cosine")
+			}
+
+			field = value
+		}
+
+	init {
+		if (!s.isFinite()) {
+			throw IllegalArgumentException("Sinus is infinite")
+		}
+
+		if (!c.isFinite()) {
+			throw IllegalArgumentException("Cosines is infinite")
+		}
+
+		if (s.isNaN()) {
+			throw IllegalArgumentException("Sinus is NaN")
+		}
+
+		if (c.isNaN()) {
+			throw IllegalArgumentException("Cosines is NaN")
+		}
+	}
+
+	/// Set to the identity rotation
+	fun setIdentity() {
+		s = 0.0
+		c = 1.0
+	}
+
+	/// Set using an angle in radians.
+	fun set(angle: Double) {
+		s = sin(angle)
+		c = cos(angle)
+	}
+
+	/// Get the angle in radians
+	val angle: Double get() = atan2(s, c)
+
+	/// Get the x-axis
+	val xAxis: MutableVector2d get() = MutableVector2d(c, s)
+
+	/// Get the u-axis
+	val yAxis: MutableVector2d get() = MutableVector2d(-s, c)
+
+	/// Multiply two rotations: q * r
+	operator fun times(other: Rotation): Rotation {
+		return Rotation(
+			s = s * other.c + c * other.s,
+			c = c * other.c - s * other.s,
+		)
+	}
+
+	/// Transpose multiply two rotations: qT * r
+	fun timesT(other: Rotation): Rotation {
+		return Rotation(
+			s = c * other.s - s * other.c,
+			c = c * other.c + s * other.s,
+		)
+	}
+
+	operator fun times(v: IVector2d<*>): Vector2d {
+		return Vector2d(c * v.x - s * v.y, s * v.x + c * v.y)
+	}
+
+	fun timesT(v: IVector2d<*>): Vector2d {
+		return Vector2d(c * v.x + s * v.y, -s * v.x + c * v.y)
+	}
+}
+
+/// 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,
+) {
+	var position: Vector2d = position
+		set(value) {
+			if (!value.isFinite) {
+				throw IllegalArgumentException("Tried to set illegal position $value")
+			}
+
+			field = value
+		}
+
+	override fun toString(): String {
+		return "Transform($position, $rotation)"
+	}
+
+	constructor(position: Vector2d, rotation: Double) : this(position, Rotation(rotation))
+	constructor() : this(Vector2d.ZERO, Rotation(0.0))
+
+	// aliases for box2d C++ code
+	val q by this::rotation
+	var p by this::position
+
+	/// Set this to the identity transform.
+	fun setIdentity() {
+		position = Vector2d.ZERO
+		rotation.setIdentity()
+	}
+
+	/// Set this based on the position and angle.
+	fun set(position: IVector2d<*>, angle: Double) {
+		this.position = Vector2d(position.x, position.y)
+		rotation.set(angle)
+	}
+
+	operator fun times(other: Transform): Transform {
+		return Transform(
+			rotation = rotation * other.rotation,
+			position = rotation.times(other.position) + position
+		)
+	}
+
+	fun timesT(other: Transform): Transform {
+		return Transform(
+			rotation = rotation.timesT(other.rotation),
+			position = rotation.timesT(other.position - position)
+		)
+	}
+
+	operator fun times(v: IVector2d<*>): Vector2d {
+		return Vector2d(
+			x = rotation.c * v.x - rotation.s * v.y + position.x,
+			y = rotation.s * v.x + rotation.c * v.y + position.y
+		)
+	}
+
+	fun timesT(v: IVector2d<*>): Vector2d {
+		val px = v.x - position.x
+		val py = v.y - position.y
+
+		return Vector2d(
+			x = (rotation.c * px + rotation.s * py),
+			y = (-rotation.s * px + rotation.c * py)
+		)
+	}
+}
+
+/** 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
+		set(value) {
+			if (!value.isFinite) {
+				throw IllegalArgumentException("Tried to set illegal local center $value")
+			}
+
+			field = value
+		}
+
+	var c0: Vector2d = Vector2d.ZERO ///< center world positions
+		set(value) {
+			if (!value.isFinite) {
+				throw IllegalArgumentException("Tried to set illegal center world position $value")
+			}
+
+			field = value
+		}
+
+	var c: Vector2d = Vector2d.ZERO ///< center world positions
+		set(value) {
+			if (!value.isFinite) {
+				throw IllegalArgumentException("Tried to set illegal center world position $value")
+			}
+
+			field = value
+		}
+
+	var a0: Double = 0.0 ///< world angles
+		set(value) {
+			if (!value.isFinite()) {
+				throw IllegalArgumentException("Tried to set illegal non finite $value")
+			}
+
+			if (value.isNaN()) {
+				throw IllegalArgumentException("Tried to set illegal NaN value")
+			}
+
+			field = value
+		}
+
+	var a: Double = 0.0 ///< world angles
+		set(value) {
+			if (!value.isFinite()) {
+				throw IllegalArgumentException("Tried to set illegal non finite $value")
+			}
+
+			if (value.isNaN()) {
+				throw IllegalArgumentException("Tried to set illegal NaN value")
+			}
+
+			field = value
+		}
+
+	/// 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()) {
+				throw IllegalArgumentException("Tried to set illegal non finite $value")
+			}
+
+			if (value.isNaN()) {
+				throw IllegalArgumentException("Tried to set illegal NaN value")
+			}
+
+			field = value
+		}
+
+	/** 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.
+	 * 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
+		val angle = (1.0 - beta) * a0 + beta * a
+		loadInto.rotation.set(angle)
+		loadInto.position -= loadInto.position * localCenter
+	}
+
+	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.
+	fun advance(alpha: Double) {
+		require(alpha < 1.0) { "Bad advance value $alpha" }
+
+		val beta = (alpha - alpha0) / (1.0 - alpha0)
+		c0 += (c - c0) * beta
+		a0 += (a - a0) * beta
+		alpha0 = alpha
+	}
+
+	/// Normalize the angles.
+	fun normalize() {
+		val d = floor(a0 / (PI * 2.0)) * 2.0 * PI
+		a0 -= d
+		a -= d
+	}
+
+	fun copy(): Sweep {
+		return Sweep().also {
+			it.localCenter = localCenter
+			it.c0 = c0
+			it.c = c
+			it.a0 = a0
+			it.a = a
+			it.alpha0 = alpha0
+		}
+	}
+
+	fun load(from: Sweep) {
+		this.localCenter = from.localCenter
+		this.c0 = from.c0
+		this.c = from.c
+		this.a0 = from.a0
+		this.a = from.a
+		this.alpha0 = from.alpha0
+	}
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Dot(a: Vector2d, b: Vector2d): Double {
+	return a.dotProduct(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Cross(a: Vector2d, b: Vector2d): Double {
+	return a.crossProduct(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Cross(a: Vector2d, b: Double): Vector2d {
+	return a.crossProduct(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Cross(a: Double, b: Vector2d): Vector2d {
+	return a.crossProduct(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Abs(a: Vector2d): Vector2d {
+	return a.absoluteVector
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Abs(a: Double): Double {
+	return a.absoluteValue
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Mul(q: Rotation, v: Vector2d): Vector2d {
+	return q.times(v)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2MulT(q: Rotation, v: Vector2d): Vector2d {
+	return q.timesT(v)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Mul(t: Transform, v: Vector2d): Vector2d {
+	return t.times(v)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2MulT(t: Transform, v: Vector2d): Vector2d {
+	return t.timesT(v)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Mul22(t: AbstractMatrix3d<*>, v: Vector2d): Vector2d {
+	return v.times(t)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Mul(t: Transform, v: Transform): Transform {
+	return t.times(v)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2MulT(t: Transform, v: Transform): Transform {
+	return t.timesT(v)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Mul(t: AbstractMatrix2d<*>, v: Vector2d): Vector2d {
+	return v.times(t)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Mul(t: AbstractMatrix3d<*>, v: Vector3d): Vector3d {
+	return v.times(t)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Min(a: Double, b: Double): Double {
+	return a.coerceAtMost(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Min(a: Vector2d, b: Vector2d): Vector2d {
+	return a.minimumPerComponent(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Max(a: Int, b: Int): Int {
+	return a.coerceAtLeast(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Max(a: Double, b: Double): Double {
+	return a.coerceAtLeast(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Max(a: Vector2d, b: Vector2d): Vector2d {
+	return a.maximumPerComponent(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Distance(a: Vector2d, b: Vector2d): Double {
+	return a.distance(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2DistanceSquared(a: Vector2d, b: Vector2d): Double {
+	return a.distanceSquared(b)
+}
+
+/**
+ * Shortcut for faster porting of C++ code
+ */
+@Suppress("NOTHING_TO_INLINE")
+internal inline fun b2Clamp(a: Double, min: Double, max: Double): Double {
+	return b2Max(min, b2Min(a, max))
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/Shape.kt

@@ -0,0 +1,60 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+data class MassData(
+	val mass: Double,
+	val center: Vector2d,
+	val inertia: Double,
+)
+
+/// A shape is used for collision detection. You can create a shape however you like.
+/// Shapes used for simulation in b2World are created automatically when a b2Fixture
+/// is created. Shapes may encapsulate a one or more child shapes.
+interface IShape<S : IShape<S>> {
+	enum class Type {
+		CIRCLE,
+		EDGE,
+		POLYGON,
+		CHAIN,
+	}
+
+	/// Clone the concrete shape.
+	fun copy(): S
+
+	/// Get the type of this shape. You can use this to down cast to the concrete shape.
+	/// @return the shape type.
+	val type: Type
+
+	/// Get the number of child primitives.
+	val childCount: Int
+
+	/// Test a point for containment in this shape. This only works for convex shapes.
+	/// @param xf the shape world transform.
+	/// @param p a point in world coordinates.
+	fun testPoint(transform: Transform, p: Vector2d): Boolean { return false }
+
+	/// Cast a ray against a child shape.
+	/// @param output the ray-cast results.
+	/// @param input the ray-cast input parameters.
+	/// @param transform the transform to be applied to the shape.
+	/// @param childIndex the child shape index
+	fun rayCast(input: RayCastInput, transform: Transform, childIndex: Int): RayCastOutput
+
+	/// Given a transform, compute the associated axis aligned bounding box for a child shape.
+	/// @param aabb returns the axis aligned box.
+	/// @param xf the world transform of the shape.
+	/// @param childIndex the child shape
+	fun computeAABB(transform: Transform, childIndex: Int): AABB
+
+	/// Compute the mass properties of this shape using its dimensions and density.
+	/// The inertia tensor is computed about the local origin.
+	/// @param massData returns the mass data for this shape.
+	/// @param density the density in kilograms per meter squared.
+	fun computeMass(density: Double): MassData
+
+	/// Radius of a shape. For polygonal shapes this must be b2_polygonRadius. There is no support for
+	/// making rounded polygons.
+	var radius: Double
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/ShapeDefs.kt

@@ -0,0 +1,2 @@
+package ru.dbotthepony.kbox2d.api
+

src/main/kotlin/ru/dbotthepony/kbox2d/api/TimeOfImpact.kt

@@ -0,0 +1,28 @@
+package ru.dbotthepony.kbox2d.api
+
+/**
+ * Input parameters for b2TimeOfImpact
+ */
+data class TOIInput(
+	var proxyA: IDistanceProxy,
+	var proxyB: IDistanceProxy,
+	var sweepA: Sweep,
+	var sweepB: Sweep,
+	var tMax: Double,		// defines sweep interval [0, tMax]
+)
+
+/**
+ * Output parameters for b2TimeOfImpact.
+ */
+data class TOIOutput(
+	val state: State,
+	val t: Double,
+) {
+	enum class State {
+		UNKNOWN,
+		FAILED,
+		OVERLAPPED,
+		TOUCHING,
+		SEPARATED
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/World.kt

@@ -0,0 +1,246 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.client.gl.GLStateTracker
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+/**
+ * The world class manages all physics entities, dynamic simulation,
+ * and asynchronous queries. The world also contains efficient memory
+ * management facilities.
+ */
+interface IB2World : IMovable {
+	/**
+	 * Register a destruction listener. The listener is owned by you and must
+	 * remain in scope.
+	 */
+	var destructionListener: IDestructionListener?
+
+	/**
+	 * Register a contact filter to provide specific control over collision.
+	 * Otherwise the default filter is used (b2_defaultFilter). The listener is
+	 * owned by you and must remain in scope.
+	 */
+	var contactFilter: IContactFilter?
+
+	/**
+	 * Register a contact event listener. The listener is owned by you and must
+	 * remain in scope.
+	 */
+	var contactListener: IContactListener?
+
+	/**
+	 * Register a routine for debug drawing. The debug draw functions are called
+	 * inside with b2World::DebugDraw method. The debug draw object is owned
+	 * by you and must remain in scope.
+	 */
+	var debugDraw: IDebugDraw?
+
+	/**
+	 * Create a rigid body given a definition. No reference to the definition
+	 * is retained.
+	 * @warning This function is locked during callbacks.
+	 */
+	fun createBody(bodyDef: ru.dbotthepony.kbox2d.api.BodyDef): ru.dbotthepony.kbox2d.api.IBody
+
+	/**
+	 * Destroy a rigid body given a definition. No reference to the definition
+	 * is retained. This function is locked during callbacks.
+	 * @warning This automatically deletes all associated shapes and joints.
+	 * @warning This function is locked during callbacks.
+	 */
+	fun destroyBody(body: ru.dbotthepony.kbox2d.api.IBody)
+
+	/**
+	 * Create a joint to constrain bodies together. No reference to the definition
+	 * is retained. This may cause the connected bodies to cease colliding.
+	 * @warning This function is locked during callbacks.
+	 */
+	fun createJoint(jointDef: IJointDef): IJoint
+
+	/**
+	 * Destroy a joint. This may cause the connected bodies to begin colliding.
+	 * @warning This function is locked during callbacks.
+	 */
+	fun destroyJoint(joint: IJoint)
+
+	/**
+	 * Take a time step. This performs collision detection, integration,
+	 * and constraint solution.
+	 * @param dt the amount of time to simulate, this should not vary.
+	 * @param velocityIterations for the velocity constraint solver.
+	 * @param positionIterations for the position constraint solver.
+	 */
+	fun step(dt: Double, velocityIterations: Int, positionIterations: Int)
+
+	/**
+	 * Manually clear the force buffer on all bodies. By default, forces are cleared automatically
+	 * after each call to Step. The default behavior is modified by calling SetAutoClearForces.
+	 * The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain
+	 * a fixed sized time step under a variable frame-rate.
+	 * When you perform sub-stepping you will disable auto clearing of forces and instead call
+	 * ClearForces after all sub-steps are complete in one pass of your game loop.
+	 * @see SetAutoClearForces
+	 */
+	fun clearForces()
+
+	/**
+	 * Call this to draw shapes and other debug draw data. This is intentionally non-const.
+	 */
+	fun debugDraw()
+
+	/**
+	 * Query the world for all fixtures that potentially overlap the
+	 * provided AABB.
+	 * @param callback a user implemented callback class.
+	 * @param aabb the query box.
+	 */
+	fun queryAABB(aabb: AABB, callback: IQueryCallback)
+
+	/**
+	 * Ray-cast the world for all fixtures in the path of the ray. Your callback
+	 * controls whether you get the closest point, any point, or n-points.
+	 * The ray-cast ignores shapes that contain the starting point.
+	 * @param callback a user implemented callback class.
+	 * @param point1 the ray starting point
+	 * @param point2 the ray ending point
+	 */
+	fun rayCast(point1: Vector2d, point2: Vector2d, callback: IRayCastCallback)
+
+	/**
+	 * Get the world body list. With the returned body, use b2Body::GetNext to get
+	 * the next body in the world list. A nullptr body indicates the end of the list.
+	 * @return the head of the world body list.
+	 */
+	val bodyList: ru.dbotthepony.kbox2d.api.IBody?
+
+	val bodyListIterator: Iterator<ru.dbotthepony.kbox2d.api.IBody> get() {
+		return object : Iterator<ru.dbotthepony.kbox2d.api.IBody> {
+			private var node = bodyList
+
+			override fun hasNext(): Boolean {
+				return node != null
+			}
+
+			override fun next(): ru.dbotthepony.kbox2d.api.IBody {
+				val old = node!!
+				node = old.next
+				check(node != old) { "Hard loop detected at $old" }
+				return old
+			}
+		}
+	}
+
+	/**
+	 * Get the world joint list. With the returned joint, use b2Joint::GetNext to get
+	 * the next joint in the world list. A nullptr joint indicates the end of the list.
+	 * @return the head of the world joint list.
+	 */
+	val jointList: IJoint?
+
+	val jointListIterator: Iterator<IJoint> get() {
+		return object : Iterator<IJoint> {
+			private var node = jointList
+
+			override fun hasNext(): Boolean {
+				return node != null
+			}
+
+			override fun next(): IJoint {
+				val old = node!!
+				node = old.next
+				check(node != old) { "Hard loop detected at $old" }
+				return old
+			}
+		}
+	}
+
+	/**
+	 * Get the world contact list. With the returned contact, use b2Contact::GetNext to get
+	 * the next contact in the world list. A nullptr contact indicates the end of the list.
+	 * @return the head of the world contact list.
+	 * @warning contacts are created and destroyed in the middle of a time step.
+	 * Use b2ContactListener to avoid missing contacts.
+	 */
+	val contactList: IContact? get() = contactManager.contactList
+
+	val contactListIterator: Iterator<IContact> get() {
+		return object : Iterator<IContact> {
+			private var node = contactList
+
+			override fun hasNext(): Boolean {
+				return node != null
+			}
+
+			override fun next(): IContact {
+				val old = node!!
+				node = old.next
+				check(node != old) { "Hard loop detected at $old" }
+				return old
+			}
+		}
+	}
+
+	/**
+	 * Enable/disable sleep.
+	 */
+	var allowAutoSleep: Boolean
+
+	/**
+	 * Enable/disable warm starting. For testing.
+	 */
+	var warmStarting: Boolean
+
+	/**
+	 * Enable/disable continuous physics. For testing.
+	 */
+	var continuousPhysics: Boolean
+
+	/**
+	 * Enable/disable single stepped continuous physics. For testing.
+	 */
+	var enableSubStepping: Boolean
+
+	val proxyCount: Int get() = contactManager.broadPhase.proxyCount
+	val bodyCount: Int
+	val jointCount: Int
+	val contactCount: Int get() = contactManager.contactCount
+	val treeHeight: Int get() = contactManager.broadPhase.treeHeight
+	val treeBalance: Int get() = contactManager.broadPhase.treeBalance
+
+	/**
+	 * Get the quality metric of the dynamic tree. The smaller the better.
+	 * The minimum is 1.
+	 */
+	val treeQuality: Double get() = contactManager.broadPhase.treeQuality
+
+	/**
+	 * Change the global gravity vector.
+ 	 */
+	var gravity: Vector2d
+
+	/**
+	 * Is the world locked (in the middle of a time step).
+	 */
+	val isLocked: Boolean
+
+	/**
+	 * Set flag to control automatic clearing of forces after each time step.
+	 */
+	var autoClearForces: Boolean
+
+	/**
+	 * Get the current profile.
+	 */
+	val profileData: ru.dbotthepony.kbox2d.api.IProfileData
+
+	/**
+	 * Dump the world into the log file.
+	 * @warning this should be called outside of a time step.
+	 */
+	fun dump()
+
+	val contactManager: IContactManager
+
+	fun notifyNewContacts()
+}

src/main/kotlin/ru/dbotthepony/kbox2d/api/WorldCallbacks.kt

@@ -0,0 +1,139 @@
+package ru.dbotthepony.kbox2d.api
+
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+/**
+ * Implement this interface to provide collision filtering. In other words, you can implement
+ * this class if you want finer control over contact creation.
+ */
+interface IContactFilter {
+	/**
+	 * Return true if contact calculations should be performed between these two shapes.
+	 * @warning for performance reasons this is only called when the AABBs begin to overlap.
+	 */
+	fun shouldCollide(fixtureA: IFixture, fixtureB: IFixture): Boolean
+}
+
+/**
+ * Joints and fixtures are destroyed when their associated
+ * body is destroyed. Implement this listener so that you
+ * may nullify references to these joints and shapes.
+ */
+interface IDestructionListener {
+	/**
+	 * Called when any joint is about to be destroyed due
+	 * to the destruction of one of its attached bodies.
+	 */
+	fun sayGoodbye(joint: IJoint)
+
+	/**
+	 * Called when any fixture is about to be destroyed due
+	 * to the destruction of its parent body.
+	 */
+	fun sayGoodbye(fixture: IFixture)
+}
+
+/**
+ * Contact impulses for reporting. Impulses are used instead of forces because
+ * sub-step forces may approach infinity for rigid body collisions. These
+ * match up one-to-one with the contact points in b2Manifold.
+ */
+data class ContactImpulse(
+	val normalImpulses: DoubleArray,
+	val tangentImpulses: DoubleArray,
+)
+
+/**
+ * Implement this class to get contact information. You can use these results for
+ * things like sounds and game logic. You can also get contact results by
+ * traversing the contact lists after the time step. However, you might miss
+ * some contacts because continuous physics leads to sub-stepping.
+ *
+ * Additionally you may receive multiple callbacks for the same contact in a
+ * single time step.
+ *
+ * You should strive to make your callbacks efficient because there may be
+ * many callbacks per time step.
+ * @warning You cannot create/destroy Box2D entities inside these callbacks.
+ */
+interface IContactListener {
+	/**
+	 * Called when two fixtures begin to touch.
+	 */
+	fun beginContact(contact: IContact)
+
+	/**
+	 * Called when two fixtures cease to touch.
+	 */
+	fun endContact(contact: IContact)
+
+	/**
+	 * This is called after a contact is updated. This allows you to inspect a
+	 * contact before it goes to the solver. If you are careful, you can modify the
+	 * contact manifold (e.g. disable contact).
+	 *
+	 * A copy of the old manifold is provided so that you can detect changes.
+	 *
+	 * Note: this is called only for awake bodies.
+	 *
+	 * Note: this is called even when the number of contact points is zero.
+	 *
+	 * Note: this is not called for sensors.
+	 *
+	 * Note: if you set the number of contact points to zero, you will not
+	 * get an EndContact callback. However, you may get a BeginContact callback
+	 * the next step.
+	 */
+	fun preSolve(contact: IContact, oldManifold: Manifold)
+
+	/**
+	 * This lets you inspect a contact after the solver is finished. This is useful
+	 * for inspecting impulses.
+	 *
+	 * Note: the contact manifold does not include time of impact impulses, which can be
+	 * arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly
+	 * in a separate data structure.
+	 *
+	 * Note: this is only called for contacts that are touching, solid, and awake.
+	 */
+	fun postSolve(contact: IContact, impulse: ContactImpulse)
+}
+
+/**
+ * Callback class for AABB queries.
+ * See IWorld#query
+ */
+fun interface IQueryCallback {
+	/**
+	 * Called for each fixture found in the query AABB.
+	 * @return false to terminate the query.
+	 */
+	fun reportFixture(fixture: IFixture): Boolean
+}
+
+/**
+ * Callback class for ray casts.
+ * See IWorld#rayCast
+ */
+fun interface IRayCastCallback {
+	/**
+	 * Called for each fixture found in the query. You control how the ray cast
+	 * proceeds by returning a float:
+	 *
+	 * return -1: ignore this fixture and continue
+	 *
+	 * return 0: terminate the ray cast
+	 *
+	 * return fraction: clip the ray to this point
+	 *
+	 * return 1: don't clip the ray and continue
+	 *
+	 * @param fixture the fixture hit by the ray
+	 * @param point the point of initial intersection
+	 * @param normal the normal vector at the point of intersection
+	 * @param fraction the fraction along the ray at the point of intersection
+	 * @return -1 to filter, 0 to terminate, fraction to clip the ray for
+	 * closest hit, 1 to continue
+	 */
+	fun reportFixture(fixture: IFixture, point: Vector2d, normal: Vector2d, fraction: Double): Double
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/BroadPhase.kt

@@ -0,0 +1,152 @@
+package ru.dbotthepony.kbox2d.collision
+
+import it.unimi.dsi.fastutil.ints.IntArrayList
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+class BroadPhase : IBroadPhase {
+	private val moveBuffer = IntArrayList()
+	private val pairBuffer = ArrayList<b2Pair>()
+	private var moveCount = 0
+
+	private val tree = DynamicTree()
+
+	override var proxyCount: Int = 0
+		private set
+
+	override fun touchProxy(proxyID: Int) {
+		bufferMove(proxyID)
+	}
+
+	override fun createProxy(aabb: AABB, userData: Any?): Int {
+		val proxyId = tree.createProxy(aabb, userData)
+		proxyCount++
+		bufferMove(proxyId)
+		return proxyId
+	}
+
+	override fun destroyProxy(proxyID: Int) {
+		unBufferMove(proxyID)
+		proxyCount--
+		tree.destroyProxy(proxyID)
+	}
+
+	override fun moveProxy(proxyID: Int, aabb: AABB, displacement: Vector2d): Boolean {
+		if (tree.moveProxy(proxyID, aabb, displacement)) {
+			bufferMove(proxyID)
+			return true
+		}
+
+		return false
+	}
+
+	private fun bufferMove(proxyId: Int) {
+		if (moveBuffer.size > moveCount) {
+			moveBuffer[moveCount] = proxyId
+		} else {
+			moveBuffer.add(proxyId)
+		}
+
+		moveCount++
+	}
+
+	private fun unBufferMove(proxyId: Int) {
+		for (i in 0 until moveCount) {
+			if (moveBuffer.getInt(i) == proxyId) {
+				moveBuffer[i] = e_nullProxy
+			}
+		}
+	}
+
+	override fun query(aabb: AABB, callback: ProxyQueryCallback): Boolean {
+		return tree.query(aabb, callback)
+	}
+
+	override fun rayCast(input: RayCastInput, callback: ProxyRayCastCallback) {
+		tree.rayCast(input, callback)
+	}
+
+	override fun shiftOrigin(newOrigin: Vector2d) {
+		tree.shiftOrigin(newOrigin)
+	}
+
+	override fun getUserData(proxyID: Int): Any? {
+		return tree.getUserData(proxyID)
+	}
+
+	override fun getFatAABB(proxyID: Int): AABB {
+		return tree.getFatAABB(proxyID)
+	}
+
+	override val treeHeight: Int
+		get() = tree.height
+
+	override val treeBalance: Int
+		get() = tree.maxBalance
+
+	override val treeQuality: Double
+		get() = tree.getAreaRatio
+
+	override fun testOverlap(proxyIDA: Int, proxyIDB: Int): Boolean {
+		return tree.getFatAABB(proxyIDA).intersect(tree.getFatAABB(proxyIDB))
+	}
+
+	private var queryProxy: Int = -1
+
+	private fun queryCallback(proxyId: Int, userData: Any?): Boolean {
+		if (proxyId == queryProxy) {
+			return true
+		}
+
+		val moved = tree.wasMoved(proxyId)
+
+		if (moved && proxyId > queryProxy) {
+			// Both proxies are moving. Avoid duplicate pairs.
+			return true
+		}
+
+		if (proxyId > queryProxy) {
+			pairBuffer.add(queryProxy to proxyId)
+		} else {
+			pairBuffer.add(proxyId to queryProxy)
+		}
+
+		return true
+	}
+
+	override fun updatePairs(callback: (Any?, Any?) -> Unit) {
+		pairBuffer.clear()
+
+		for (i in 0 until moveCount) {
+			val value = moveBuffer.getInt(i)
+
+			if (value == e_nullProxy)
+				continue
+
+			val fatAABB = tree.getFatAABB(value)
+			queryProxy = value
+			tree.query(fatAABB, this::queryCallback)
+		}
+
+		// Send pairs to caller
+		for (primaryPair in pairBuffer) {
+			val userDataA = tree.getUserData(primaryPair.first)
+			val userDataB = tree.getUserData(primaryPair.second)
+
+			callback.invoke(userDataA, userDataB)
+		}
+
+		// Clear move flags
+		for (i in 0 until moveCount) {
+			val value = moveBuffer.getInt(i)
+
+			if (value != e_nullProxy) {
+				tree.clearMoved(value)
+			}
+		}
+
+		// Reset move buffer
+		moveCount = 0
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/Collision.kt

@@ -0,0 +1,184 @@
+package ru.dbotthepony.kbox2d.collision
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+import java.util.*
+import kotlin.collections.ArrayList
+
+/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
+/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
+fun b2GetPointStates(state1: Array<PointState>, state2: Array<PointState>, manifold1: Manifold, manifold2: Manifold) {
+	Arrays.fill(state1, PointState.NULL)
+	Arrays.fill(state2, PointState.NULL)
+
+	// Detect persists and removes.
+	for (i in manifold1.points.indices) {
+		val id = manifold1.points[i].id
+		state1[i] = PointState.REMOVE
+
+		for (j in manifold2.points.indices) {
+			if (manifold2.points[j].id.key == id.key) {
+				state1[i] = PointState.PERSIST
+				break
+			}
+		}
+	}
+
+	// Detect persists and adds.
+	for (i in manifold2.points.indices) {
+		val id = manifold2.points[i].id
+		state2[i] = PointState.ADD
+
+		for (j in manifold1.points.indices) {
+			if (manifold1.points[j].id.key == id.key) {
+				state1[i] = PointState.PERSIST
+				break
+			}
+		}
+	}
+}
+
+/**
+ * This is used to compute the current state of a contact manifold.
+ * Evaluate the manifold with supplied transforms. This assumes
+ * modest motion from the original state. This does not change the
+ * point count, impulses, etc. The radii must come from the shapes
+ * that generated the manifold.
+ */
+class WorldManifold(manifold: Manifold, xfA: Transform, radiusA: Double, xfB: Transform, radiusB: Double) :
+	IWorldManifold {
+	override var normal: Vector2d = Vector2d.ZERO
+		private set
+
+	override var points: Array<Vector2d> = Array(b2_maxManifoldPoints) { Vector2d.ZERO }
+		private set
+
+	override var separations: DoubleArray = DoubleArray(b2_maxManifoldPoints)
+		private set
+
+	init {
+		if (manifold.points.isNotEmpty()) {
+			when (manifold.type) {
+				Manifold.Type.CIRCLES -> {
+					normal = Vector2d.RIGHT
+
+					val pointA = b2Mul(xfA, manifold.localPoint);
+					val pointB = b2Mul(xfB, manifold.points[0].localPoint);
+
+					if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) {
+						normal = (pointB - pointA).normalized
+					}
+
+					val cA = pointA + radiusA * normal
+					val cB = pointB - radiusB * normal
+
+					points[0] = 0.5 * (cA + cB)
+					separations[0] = b2Dot(cB - cA, normal)
+				}
+
+				Manifold.Type.FACE_A -> {
+					normal = b2Mul(xfA.q, manifold.localNormal)
+					val planePoint = b2Mul(xfA, manifold.localPoint)
+
+					for (i in manifold.points.indices) {
+						val clipPoint = b2Mul(xfB, manifold.points[i].localPoint)
+						val cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
+						val cB = clipPoint - radiusB * normal;
+
+						points[i] = 0.5 * (cA + cB)
+						separations[i] = b2Dot(cB - cA, normal)
+					}
+				}
+
+				Manifold.Type.FACE_B -> {
+					normal = b2Mul(xfB.q, manifold.localNormal)
+					val planePoint = b2Mul(xfB, manifold.localPoint)
+
+					for (i in manifold.points.indices) {
+						val clipPoint = b2Mul(xfA, manifold.points[i].localPoint);
+						val cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
+						val cA = clipPoint - radiusA * normal;
+
+						points[i] = 0.5 * (cA + cB)
+						separations[i] = b2Dot(cA - cB, normal)
+					}
+
+					// Ensure normal points from A to B.
+					normal = -normal
+				}
+
+				null -> throw IllegalArgumentException()
+			}
+		}
+	}
+}
+
+// Sutherland-Hodgman clipping.
+internal fun b2ClipSegmentToLine(
+	vIn: Array<ClipVertex>,
+	normal: Vector2d,
+	offset: Double,
+	vertexIndexA: Int
+): Array<ClipVertex> {
+	// Start with no output points
+	val vOut = arrayOfNulls<ClipVertex>(2)
+	var count = 0
+
+	// Calculate the distance of end points to the line
+	val distance0 = b2Dot(normal, vIn[0].v) - offset
+	val distance1 = b2Dot(normal, vIn[1].v) - offset
+
+	// If the points are behind the plane
+	if (distance0 <= 0.0) vOut[count++] = vIn[0]
+	if (distance1 <= 0.0) vOut[count++] = vIn[1]
+
+	// If the points are on different sides of the plane
+	if (distance0 * distance1 < 0.0) {
+		// Find intersection point of edge and plane
+		val interp = distance0 / (distance0 - distance1)
+
+		val clipVertex = ClipVertex(
+			v = vIn[0].v + interp * (vIn[1].v - vIn[0].v),
+			id = ContactID(
+				cf = ContactFeature(
+					// VertexA is hitting edgeB.
+					indexA = vertexIndexA,
+					indexB = vIn[0].id.cf.indexB,
+					typeA = ContactFeature.Type.VERTEX,
+					typeB = ContactFeature.Type.FACE,
+				)
+			)
+		)
+
+		vOut[count] = clipVertex
+		count++
+
+		check(count == 2) { "Expected output to be 2 in size, got $count" }
+	}
+
+	if (count == 2)
+		return vOut as Array<ClipVertex>
+	else if (count == 1)
+		return arrayOf(vOut[0]!!)
+	else if (count == 0)
+		return arrayOf()
+	else
+		throw IllegalStateException(count.toString())
+}
+
+internal fun b2TestOverlap(
+	shapeA: IShape<*>,
+	indexA: Int,
+	shapeB: IShape<*>,
+	indexB: Int,
+	xfA: Transform,
+	xfB: Transform,
+): Boolean {
+	return b2Distance(
+		SimplexCache(),
+		DistanceProxy(shapeA, indexA),
+		DistanceProxy(shapeB, indexB),
+		xfA, xfB, true
+	).distance < 10.0 * b2_epsilon
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/Distance.kt

@@ -0,0 +1,688 @@
+package ru.dbotthepony.kbox2d.collision
+
+import com.google.common.collect.ImmutableList
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.collision.shapes.ChainShape
+import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
+import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.crossProduct
+import ru.dbotthepony.kstarbound.math.times
+
+var b2_gjkCalls = 0
+	private set
+
+var b2_gjkIters = 0
+	private set
+
+var b2_gjkMaxIters = 0
+	private set
+
+class DistanceProxy : IDistanceProxy {
+	override val vertices: List<Vector2d>
+	override val radius: Double
+
+	constructor(shape: IShape<*>, index: Int) {
+		when (shape.type) {
+			IShape.Type.CIRCLE -> {
+				val circle = shape as CircleShape
+				vertices = listOf(circle.position)
+				radius = circle.radius
+			}
+
+			IShape.Type.EDGE -> {
+				val edge = shape as EdgeShape
+				vertices = listOf(edge.vertex1, edge.vertex2)
+				radius = edge.radius
+			}
+
+			IShape.Type.POLYGON -> {
+				val polygon = shape as PolygonShape
+				vertices = polygon.vertices
+				radius = polygon.radius
+			}
+
+			IShape.Type.CHAIN -> {
+				val chain = shape as ChainShape
+				val faceA = chain.vertices[index]
+				val faceB: Vector2d
+
+				if (index + 1 < chain.vertices.size) {
+					faceB = chain.vertices[index + 1]
+				} else {
+					faceB = chain.vertices[0]
+				}
+
+				vertices = listOf(faceA, faceB)
+				radius = chain.radius
+			}
+		}
+	}
+
+	constructor(vertices: List<Vector2d>, radius: Double) {
+		this.vertices = vertices
+		this.radius = radius
+	}
+
+	override fun getSupport(d: Vector2d): Int {
+		var bestIndex = 0
+		var bestValue = vertices[0].dotProduct(d)
+
+		for (i in 1 until vertices.size) {
+			val value = vertices[i].dotProduct(d)
+
+			if (value > bestValue) {
+				bestIndex = i
+				bestValue = value
+			}
+		}
+
+		return bestIndex
+	}
+
+	override fun getSupportVertex(d: Vector2d): Vector2d {
+		return vertices[getSupport(d)]
+	}
+}
+
+data class SimplexVertex(
+	var wA: Vector2d = Vector2d.ZERO,		// support point in proxyA
+	var wB: Vector2d = Vector2d.ZERO,		// support point in proxyB
+	var w: Vector2d = Vector2d.ZERO,		// wB - wA
+	var a: Double = 0.0,			// barycentric coordinate for closest point
+	var indexA: Int = 0,		// wA index
+	var indexB: Int = 0,		// wB index
+) {
+	fun load(other: SimplexVertex) {
+		wA = other.wA
+		wB = other.wB
+		w = other.w
+		a = other.a
+		indexA = other.indexA
+		indexB = other.indexB
+	}
+}
+
+data class WitnessPoints(
+	val pA: Vector2d,
+	val pB: Vector2d,
+)
+
+class Simplex() {
+	var count: Int = 0
+	val v1 = SimplexVertex()
+	val v2 = SimplexVertex()
+	val v3 = SimplexVertex()
+	val vertices = arrayListOf(v1, v2, v3)
+
+	constructor(
+		cache: SimplexCache,
+		proxyA: IDistanceProxy,
+		transformA: Transform,
+		proxyB: IDistanceProxy,
+		transformB: Transform,
+	) : this() {
+		check(cache.count <= 3)
+		count = cache.count
+
+		// Copy data from cache.
+		for (i in 0 until count) {
+			val v = vertices[i]
+			v.indexA = cache.indexA[i]
+			v.indexB = cache.indexB[i]
+			val wALocal = proxyA.vertices[v.indexA]
+			val wBLocal = proxyB.vertices[v.indexB]
+			v.wA = transformA.times(wALocal)
+			v.wB = transformB.times(wBLocal)
+			v.w = v.wB - v.wA
+		}
+
+		// Compute the new simplex metric, if it is substantially different than
+		// old metric then flush the simplex.
+		if (count > 1) {
+			val metric1 = cache.metric
+			val metric2 = getMetric()
+
+			if (metric2 < 0.5 * metric1 || 2.0 * metric1 < metric2 || metric2 < b2_epsilon) {
+				// Reset the simplex.
+				count = 0
+			}
+		}
+
+		// If the cache is empty or invalid ...
+		if (count == 0) {
+			val v = v1
+			v.indexA = 0
+			v.indexB = 0
+			val wALocal = proxyA.vertices[0]
+			val wBLocal = proxyB.vertices[0]
+			v.wA = transformA.times(wALocal)
+			v.wB = transformB.times(wBLocal)
+			v.w = v.wB - v.wA;
+			v.a = 1.0
+			count = 1
+		}
+	}
+
+	fun writeCache(): SimplexCache {
+		return SimplexCache(
+			metric = getMetric(),
+			count = count,
+
+			indexA = IntArray(count).also {
+				for (i in 0 until count) {
+					it[i] = vertices[i].indexA
+				}
+			},
+
+			indexB = IntArray(count).also {
+				for (i in 0 until count) {
+					it[i] = vertices[i].indexB
+				}
+			}
+		)
+	}
+
+	fun getSearchDirection(): Vector2d {
+		return when (count) {
+			1 -> -v1.w
+			2 -> {
+				val e12 = v2.w - v1.w
+				val sgn = b2Cross(e12, -v1.w)
+
+				if (sgn > 0.0f) {
+					// Origin is left of e12.
+					return b2Cross(1.0, e12)
+				} else {
+					// Origin is right of e12.
+					return b2Cross(e12, 1.0)
+				}
+			}
+			else -> throw IllegalStateException(count.toString())
+		}
+	}
+
+	fun getClosestPoint(): Vector2d {
+		return when (count) {
+			1 -> v1.w
+			2 -> v1.a * v1.w + v2.a * v2.w
+			3 -> Vector2d.ZERO
+			else -> throw IllegalStateException(count.toString())
+		}
+	}
+
+	fun getWitnessPoints(): WitnessPoints {
+		return when (count) {
+			1 -> WitnessPoints(v1.wA, v1.wB)
+			2 -> WitnessPoints(
+				pA = v1.a * v1.wA + v2.a * v2.wA,
+				pB = v1.a * v1.wB + v2.a * v2.wB,
+			)
+			3 -> {
+				val pA = v1.a * v1.wA + v2.a * v2.wA + v3.a * v3.wA
+
+				WitnessPoints(
+					pA = pA,
+					pB = pA
+				)
+			}
+			else -> throw IllegalStateException(count.toString())
+		}
+	}
+
+	fun getMetric(): Double {
+		return when (count) {
+			1 -> 0.0
+			2 -> b2Distance(v1.w, v2.w)
+			3 -> b2Cross(v2.w - v1.w, v3.w - v1.w)
+			else -> throw IllegalStateException(count.toString())
+		}
+	}
+
+	/**
+	 * Solve a line segment using barycentric coordinates.
+	 *
+	 * p = a1 * w1 + a2 * w2
+	 * a1 + a2 = 1
+	 *
+	 * The vector from the origin to the closest point on the line is
+	 * perpendicular to the line.
+	 * e12 = w2 - w1
+	 * dot(p, e) = 0
+	 * a1 * dot(w1, e) + a2 * dot(w2, e) = 0
+	 *
+	 * 2-by-2 linear system
+	 * [1      1     ][a1] = [1]
+	 * [w1.e12 w2.e12][a2] = [0]
+	 *
+	 * Define
+	 * d12_1 =  dot(w2, e12)
+	 * d12_2 = -dot(w1, e12)
+	 * d12 = d12_1 + d12_2
+	 *
+	 * Solution
+	 * a1 = d12_1 / d12
+	 * a2 = d12_2 / d12
+	 */
+	fun solve2() {
+		val w1 = v1.w
+		val w2 = v2.w
+		val e12 = w2 - w1
+
+		// w1 region
+		val d12_2 = -w1.dotProduct(e12)
+		if (d12_2 <= 0.0) {
+			// a2 <= 0, so we clamp it to 0
+			v1.a = 1.0
+			count = 1
+			return
+		}
+
+		// w2 region
+		val d12_1 = w2.dotProduct(e12)
+		if (d12_1 <= 0.0) {
+			// a1 <= 0, so we clamp it to 0
+			v2.a = 1.0
+			count = 1
+			v1.load(v2) // TODO
+			return
+		}
+
+		// Must be in e12 region.
+		val inv_d12 = 1.0f / (d12_1 + d12_2)
+		v1.a = d12_1 * inv_d12
+		v2.a = d12_2 * inv_d12
+		count = 2
+	}
+
+	/**
+	 * Possible regions:
+	 * - points[2]
+	 * - edge points[0]-points[2]
+	 * - edge points[1]-points[2]
+	 * - inside the triangle
+	 */
+	fun solve3() {
+		val w1 = v1.w
+		val w2 = v2.w
+		val w3 = v3.w
+
+		// Edge12
+		// [1      1     ][a1] = [1]
+		// [w1.e12 w2.e12][a2] = [0]
+		// a3 = 0
+		val e12 = w2 - w1
+		val w1e12 = w1.dotProduct(e12)
+		val w2e12 = w2.dotProduct(e12)
+		val d12_1 = w2e12
+		val d12_2 = -w1e12
+
+		// Edge13
+		// [1      1     ][a1] = [1]
+		// [w1.e13 w3.e13][a3] = [0]
+		// a2 = 0
+		val e13 = w3 - w1
+		val w1e13 = w1.dotProduct(e13)
+		val w3e13 = w3.dotProduct(e13)
+		val d13_1 = w3e13
+		val d13_2 = -w1e13
+
+		// Edge23
+		// [1      1     ][a2] = [1]
+		// [w2.e23 w3.e23][a3] = [0]
+		// a1 = 0
+		val e23 = w3 - w2
+		val w2e23 = w2.dotProduct(e23)
+		val w3e23 = w3.dotProduct(e23)
+		val d23_1 = w3e23
+		val d23_2 = -w2e23
+
+		// Triangle123
+		val n123 = b2Cross(e12, e13)
+
+		val d123_1 = n123 * b2Cross(w2, w3)
+		val d123_2 = n123 * b2Cross(w3, w1)
+		val d123_3 = n123 * b2Cross(w1, w2)
+
+		// w1 region
+		if (d12_2 <= 0.0 && d13_2 <= 0.0) {
+			v1.a = 1.0
+			count = 1
+			return
+		}
+
+		// e12
+		if (d12_1 > 0.0 && d12_2 > 0.0 && d123_3 <= 0.0) {
+			val inv_d12 = 1.0 / (d12_1 + d12_2)
+			v1.a = d12_1 * inv_d12
+			v2.a = d12_2 * inv_d12
+			count = 2
+			return
+		}
+
+		// e13
+		if (d13_1 > 0.0 && d13_2 > 0.0 && d123_2 <= 0.0) {
+			val inv_d13 = 1.0 / (d13_1 + d13_2)
+			v1.a = d13_1 * inv_d13
+			v3.a = d13_2 * inv_d13
+			count = 2
+			v2.load(v3) // TODO
+			return
+		}
+
+		// w2 region
+		if (d12_1 <= 0.0 && d23_2 <= 0.0) {
+			v2.a = 1.0
+			count = 1
+			v1.load(v2) // TODO
+			return
+		}
+
+		// w3 region
+		if (d13_1 <= 0.0 && d23_1 <= 0.0) {
+			v3.a = 1.0
+			count = 1
+			v1.load(v3)
+			return
+		}
+
+		// e23
+		if (d23_1 > 0.0 && d23_2 > 0.0 && d123_1 <= 0.0) {
+			val inv_d23 = 1.0 / (d23_1 + d23_2)
+			v2.a = d23_1 * inv_d23
+			v3.a = d23_2 * inv_d23
+			count = 2
+			v1.load(v3)
+			return
+		}
+
+		// Must be in triangle123
+		val inv_d123 = 1.0 / (d123_1 + d123_2 + d123_3)
+		v1.a = d123_1 * inv_d123
+		v2.a = d123_2 * inv_d123
+		v3.a = d123_3 * inv_d123
+		count = 3
+	}
+}
+
+private const val k_maxIters = 20
+
+/**
+ * Compute the closest points between two shapes. Supports any combination of:
+ * b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.
+ * On the first call set b2SimplexCache.count to zero.
+ */
+fun b2Distance(
+	cache: SimplexCache,
+	proxyA: IDistanceProxy,
+	proxyB: IDistanceProxy,
+	transformA: Transform,
+	transformB: Transform,
+	useRadii: Boolean = false
+): DistanceOutput {
+	// Initialize the simplex.
+	val simplex = Simplex(cache, proxyA, transformA, proxyB, transformB)
+
+	// Get simplex vertices as an array.
+	val vertices = simplex.vertices
+
+	// These store the vertices of the last simplex so that we
+	// can check for duplicates and prevent cycling.
+	val saveA = IntArray(3)
+	val saveB = IntArray(3)
+	var saveCount: Int
+
+	var iter = 0
+	while (iter < k_maxIters) {
+		// Copy simplex so we can identify duplicates.
+		saveCount = simplex.count
+
+		for (i in 0 until saveCount) {
+			saveA[i] = vertices[i].indexA
+			saveB[i] = vertices[i].indexB
+		}
+
+		when (simplex.count) {
+			1 -> {}
+			2 -> simplex.solve2()
+			3 -> simplex.solve3()
+			else -> throw IllegalStateException(simplex.count.toString())
+		}
+
+		if (simplex.count == 3) {
+			// If we have 3 points, then the origin is in the corresponding triangle.
+			break
+		}
+
+		// Get search direction.
+		val d = simplex.getSearchDirection()
+
+		// Ensure the search direction is numerically fit.
+		if (d.lengthSquared < b2_epsilon * b2_epsilon) {
+			// The origin is probably contained by a line segment
+			// or triangle. Thus the shapes are overlapped.
+
+			// We can't return zero here even though there may be overlap.
+			// In case the simplex is a point, segment, or triangle it is difficult
+			// to determine if the origin is contained in the CSO or very close to it.
+			break
+		}
+
+		// Compute a tentative new simplex vertex using support points.
+		val vertex = vertices[simplex.count]
+		vertex.indexA = proxyA.getSupport(transformA.q.timesT(-d))
+		vertex.wA = transformA.times(proxyA.vertices[vertex.indexA])
+		vertex.indexB = proxyB.getSupport(transformB.q.timesT(d))
+		vertex.wB = transformB.times(proxyB.vertices[vertex.indexB])
+		vertex.w = vertex.wB - vertex.wA
+
+		// Iteration count is equated to the number of support point calls.
+		iter++
+		b2_gjkIters++
+
+		// Check for duplicate support points. This is the main termination criteria.
+		var duplicate = false
+
+		for (i in 0 until saveCount) {
+			if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i]) {
+				duplicate = true
+				break
+			}
+		}
+
+		// If we found a duplicate support point we must exit to avoid cycling.
+		if (duplicate) {
+			break
+		}
+
+		// New vertex is ok and needed.
+		simplex.count++
+	}
+
+	b2_gjkMaxIters = b2_gjkMaxIters.coerceAtLeast(iter)
+
+	// Prepare output.
+	var (pointA, pointB) = simplex.getWitnessPoints()
+	// Cache the simplex.
+	val newCache = simplex.writeCache()
+
+	var distance = pointA.distance(pointB)
+
+	// Apply radii if requested
+	if (useRadii) {
+		if (distance < b2_epsilon) {
+			// Shapes are too close to safely compute normal
+			val p = 0.5 * (pointA + pointB)
+			pointA = p
+			pointB = p
+			distance = 0.0
+		} else {
+			// Keep closest points on perimeter even if overlapped, this way
+			// the points move smoothly.
+			val rA = proxyA.radius;
+			val rB = proxyB.radius;
+			val normal = (pointB - pointA).normalized;
+			distance = (distance - rA - rB).coerceAtLeast(0.0)
+			pointA += rA * normal
+			pointB -= rB * normal
+		}
+	}
+
+	return DistanceOutput(
+		pointA = pointA,
+		pointB = pointB,
+		distance = distance,
+		newCache = newCache,
+		iterations = iter,
+	)
+}
+
+fun b2Distance(
+	cache: SimplexCache,
+	input: DistanceInput
+): DistanceOutput {
+	val (proxyA, proxyB, transformA, transformB, useRadii) = input
+	return b2Distance(cache, proxyA, proxyB, transformA, transformB, useRadii)
+}
+
+private const val gjk_tolerance = 0.5 * b2_linearSlop
+
+/**
+ * GJK-raycast
+ * Algorithm by Gino van den Bergen.
+ * "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010
+ */
+fun b2ShapeCast(
+	output: ShapeCastOutput,
+	proxyA: IDistanceProxy,
+	proxyB: IDistanceProxy,
+	xfA: Transform,
+	xfB: Transform,
+	r: Vector2d,
+): Boolean {
+	output.iterations = 0
+	output.lambda = 1.0
+	output.normal = Vector2d.ZERO
+	output.point = Vector2d.ZERO
+
+	val radiusA = proxyA.radius.coerceAtLeast(b2_polygonRadius)
+	val radiusB = proxyB.radius.coerceAtLeast(b2_polygonRadius)
+
+	val radius = radiusA + radiusB
+	var n = Vector2d.ZERO
+	var lambda = 0.0
+
+	// Initial simplex
+	val simplex = Simplex()
+
+	// Get simplex vertices as an array.
+	val vertices = simplex.vertices
+
+	// Get support point in -r direction
+	var indexA = proxyA.getSupport(xfA.q.timesT(-r))
+	var wA = xfA.times(proxyA.vertices[indexA])
+
+	var indexB = proxyB.getSupport(xfB.q.timesT(r))
+	var wB = xfB.times(proxyB.vertices[indexB])
+	var v = wA - wB
+
+	// Sigma is the target distance between polygons
+	val sigma = b2_polygonRadius.coerceAtLeast(radius - b2_polygonRadius)
+
+	// Main iteration loop.
+	var iter = 0
+
+	while (iter < k_maxIters && v.length - sigma > gjk_tolerance) {
+		check(simplex.count < 3) { simplex.count }
+
+		output.iterations++
+
+		// Support in direction -v (A - B)
+		indexA = proxyA.getSupport(xfA.q.timesT(-v))
+		wA = xfA.times(proxyA.vertices[indexA])
+
+		indexB = proxyB.getSupport(xfB.q.timesT(v))
+		wB = xfB.times(proxyB.vertices[indexB])
+
+		val p = wA - wB
+
+		// -v is a normal at p
+		v = v.normalized
+
+		// Intersect ray with plane
+		val vp = v.dotProduct(p)
+		val vr = v.dotProduct(r)
+
+		if (vp - sigma > lambda * vr) {
+			if (vr <= 0.0) {
+				return false
+			}
+
+			lambda = (vp - sigma) / vr
+
+			if (lambda >= 1.0) {
+				return false
+			}
+
+			n = -v
+			simplex.count = 0
+		}
+
+		// Reverse simplex since it works with B - A.
+		// Shift by lambda * r because we want the closest point to the current clip point.
+		// Note that the support point p is not shifted because we want the plane equation
+		// to be formed in unshifted space.
+		val vertex = vertices[simplex.count]
+		vertex.indexA = indexB
+		vertex.wA = wB + lambda * r
+		vertex.indexB = indexA
+		vertex.wB = wA
+		vertex.w = vertex.wB - vertex.wA
+		vertex.a = 1.0
+		simplex.count++
+
+		when (simplex.count) {
+			1 -> {}
+			2 -> simplex.solve2()
+			3 -> simplex.solve3()
+			else -> throw IllegalStateException(simplex.count.toString())
+		}
+
+		// If we have 3 points, then the origin is in the corresponding triangle.
+		if (simplex.count == 3) {
+			// Overlap
+			return false
+		}
+
+		// Get search direction.
+		v = simplex.getClosestPoint()
+
+		// Iteration count is equated to the number of support point calls.
+		iter++
+	}
+
+	if (iter == 0) {
+		// Initial overlap
+		return false
+	}
+
+	// Prepare output.
+	val (_, pointA) = simplex.getWitnessPoints()
+
+	if (v.lengthSquared > 0.0) {
+		n = (-v).normalized
+	}
+
+	output.point = pointA + radiusA * n
+	output.normal = n
+	output.lambda = lambda
+	output.iterations = iter
+
+	return true
+}
+
+fun b2ShapeCast(output: ShapeCastOutput, input: ShapeCastInput): Boolean {
+	val (proxyA, proxyB, transformA, transformB, translationB) = input
+	return b2ShapeCast(output, proxyA, proxyB, transformA, transformB, translationB)
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/DynamicTree.kt

@@ -0,0 +1,715 @@
+package ru.dbotthepony.kbox2d.collision
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+import kotlin.math.absoluteValue
+
+const val b2_nullNode = -1
+
+class DynamicTree : IDynamicTree {
+	private var root: Int = b2_nullNode
+
+	private val nodeCapacity get() = nodes.size
+	private var nodeCount = 0
+	private var freeList = 0
+	private var insertionCount = 0
+
+	internal var nodes = Array(16) { TreeNode(this) }
+
+	init {
+		// Build a linked list for the free list.
+		for (i in 0 until nodeCapacity - 1) {
+			nodes[i].next = i + 1
+			nodes[i].height = -1
+		}
+
+		nodes[nodeCapacity - 1].next = b2_nullNode
+		nodes[nodeCapacity - 1].height = -1
+	}
+
+	private fun allocateNode(): Int {
+		// Expand the node pool as needed.
+		if (freeList == b2_nullNode) {
+			check(nodeCount == nodeCapacity) { "$nodeCount != $nodeCapacity" }
+
+			// The free list is empty. Rebuild a bigger pool.
+			nodes = Array(nodeCapacity * 2) memcopy@{
+				if (it >= nodeCapacity)
+					return@memcopy TreeNode(this)
+				else
+					return@memcopy nodes[it]
+			}
+
+			// Build a linked list for the free list. The parent
+			// pointer becomes the "next" pointer.
+			for (i in nodeCount until nodeCapacity - 1) {
+				nodes[i].next = i + 1
+				nodes[i].height = -1
+			}
+
+			nodes[nodeCapacity - 1].next = b2_nullNode
+			nodes[nodeCapacity - 1].height = -1
+
+			freeList = nodeCount
+		}
+
+		// Peel a node off the free list.
+		val nodeId = freeList
+		freeList = nodes[nodeId].next
+		val node = nodes[nodeId]
+
+		node.parent = b2_nullNode
+		node.child1 = b2_nullNode
+		node.child2 = b2_nullNode
+		node.height = 0
+		node.userData = null
+		node.moved = false
+
+		nodeCount++
+		return nodeId
+	}
+
+	private fun freeNode(nodeId: Int) {
+		require(nodeId in 0 until nodeCapacity) { "$nodeId is out of range (0 to ${nodeCapacity - 1})" }
+		check(0 < nodeCount) { "We have $nodeCount nodes" }
+
+		val node = nodes[nodeId]
+
+		node.next = freeList
+		node.height = -1
+		// node.aabb = null
+		// node.userData = null
+
+		freeList = nodeId
+		nodeCount--
+	}
+
+	override fun createProxy(aabb: AABB, userData: Any?): Int {
+		val proxyId = allocateNode()
+
+		// Fatten the aabb.
+		val node = nodes[proxyId]
+		node.aabb = AABB(aabb.mins - R, aabb.maxs + R)
+		node.userData = userData
+		node.height = 0
+		node.moved = true
+
+		insertLeaf(proxyId)
+
+		return proxyId
+	}
+
+	override fun destroyProxy(proxyID: Int) {
+		check(nodes[proxyID].isLeaf) { "Can't chop whole branch" }
+
+		removeLeaf(proxyID)
+		freeNode(proxyID)
+	}
+
+	override fun moveProxy(proxyID: Int, aabb: AABB, displacement: Vector2d): Boolean {
+		check(nodes[proxyID].isLeaf) { "Can't move whole branch" }
+
+		// Extend AABB
+		val mins = aabb.mins.toMutableVector() - R
+		val maxs = aabb.maxs.toMutableVector() + R
+
+		// Predict AABB movement
+		val d = displacement * b2_aabbMultiplier
+
+		if (d.x < 0.0) {
+			mins.x += d.x
+		} else {
+			maxs.x += d.x
+		}
+
+		if (d.y < 0.0) {
+			mins.y += d.y
+		} else {
+			maxs.y += d.y
+		}
+
+		val fatAABB = AABB(mins.toVector(), maxs.toVector())
+		val treeAABB = checkNotNull(nodes[proxyID].aabb) { "Node at $proxyID has null AABB" }
+
+		if (treeAABB.contains(fatAABB)) {
+			// The tree AABB still contains the object, but it might be too large.
+			// Perhaps the object was moving fast but has since gone to sleep.
+			// The huge AABB is larger than the new fat AABB.
+			val hugeAABB = AABB(
+				fatAABB.mins - R * 4.0,
+				fatAABB.maxs + R * 4.0,
+			)
+
+			if (treeAABB.contains(hugeAABB)) {
+				// The tree AABB contains the object AABB and the tree AABB is
+				// not too large. No tree update needed.
+				return false
+			}
+
+			// Otherwise the tree AABB is huge and needs to be shrunk
+		}
+
+		removeLeaf(proxyID)
+		nodes[proxyID].aabb = fatAABB
+		insertLeaf(proxyID)
+		nodes[proxyID].moved = true
+
+		return true
+	}
+
+	override fun getUserData(proxyID: Int): Any? {
+		return nodes[proxyID].userData
+	}
+
+	override fun wasMoved(proxyID: Int): Boolean {
+		return nodes[proxyID].moved
+	}
+
+	override fun clearMoved(proxyID: Int) {
+		nodes[proxyID].moved = false
+	}
+
+	override fun getFatAABB(proxyID: Int): AABB {
+		return checkNotNull(nodes[proxyID].aabb) { "Tree node has null aabb. This can be either by a bug, or $proxyID is not a valid proxy" }
+	}
+
+	private fun insertLeaf(leaf: Int) {
+		insertionCount++
+
+		if (root == b2_nullNode) {
+			root = leaf
+			nodes[leaf].parent = b2_nullNode
+			return
+		}
+
+		// Find the best sibling for this node
+		val leafAABB = checkNotNull(nodes[leaf].aabb) { "Leaf at $leaf has null aabb" }
+		var index = root
+
+		while (!nodes[index].isLeaf) {
+			val child1 = nodes[index].child1
+			val child2 = nodes[index].child2
+
+			val area = checkNotNull(nodes[index].aabb) { "Node at $index has null aabb" }.perimeter
+			val combined = nodes[index].aabb?.combine(leafAABB) ?: throw ConcurrentModificationException()
+
+			val combinedArea = combined.perimeter
+
+			// Cost of creating a new parent for this node and the new leaf
+			val cost = 2.0 * combinedArea
+
+			// Minimum cost of pushing the leaf further down the tree
+			val inheritanceCost = 2.0 * (combinedArea - area)
+
+			// Cost of descending into child1
+			val cost1: Double
+
+			if (nodes[child1].isLeaf) {
+				val aabb = leafAABB.combine(checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" })
+				cost1 = aabb.perimeter + inheritanceCost
+			} else {
+				val aabb = leafAABB.combine(checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" })
+				val oldArea = nodes[child1].aabb?.perimeter ?: throw ConcurrentModificationException()
+				val newArea = aabb.perimeter
+				cost1 = (newArea - oldArea) + inheritanceCost
+			}
+
+			// Cost of descending into child2
+			val cost2: Double
+
+			if (nodes[child2].isLeaf) {
+				val aabb = leafAABB.combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" })
+				cost2 = aabb.perimeter + inheritanceCost
+			} else {
+				val aabb = leafAABB.combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" })
+				val oldArea = nodes[child2].aabb?.perimeter ?: throw ConcurrentModificationException()
+				val newArea = aabb.perimeter
+				cost2 = (newArea - oldArea) + inheritanceCost
+			}
+
+			// Descend according to the minimum cost.
+			if (cost < cost1 && cost < cost2) {
+				break
+			}
+
+			// Descend
+			if (cost1 < cost2) {
+				index = child1
+			} else {
+				index = child2
+			}
+		}
+
+		val sibling = index
+
+		// Create a new parent.
+		val oldParent = nodes[sibling].parent
+		val newParent = allocateNode()
+
+		nodes[newParent].parent = oldParent
+		nodes[newParent].userData = null
+		nodes[newParent].aabb = leafAABB.combine(checkNotNull(nodes[sibling].aabb) { "Node at $sibling has null aabb" })
+		nodes[newParent].height = nodes[sibling].height + 1
+
+		if (oldParent != b2_nullNode) {
+			// The sibling was not the root.
+			if (nodes[oldParent].child1 == sibling) {
+				nodes[oldParent].child1 = newParent
+			} else {
+				nodes[oldParent].child2 = newParent
+			}
+
+			nodes[newParent].child1 = sibling
+			nodes[newParent].child2 = leaf
+
+			nodes[sibling].parent = newParent
+			nodes[leaf].parent = newParent
+		} else {
+			// The sibling was the root.
+			nodes[newParent].child1 = sibling
+			nodes[newParent].child2 = leaf
+
+			nodes[sibling].parent = newParent
+			nodes[leaf].parent = newParent
+			root = newParent
+		}
+
+		// Walk back up the tree fixing heights and AABBs
+		index = nodes[leaf].parent
+
+		while (index != b2_nullNode) {
+			index = balance(index)
+
+			val child1 = nodes[index].child1
+			val child2 = nodes[index].child2
+
+			check(child1 != b2_nullNode) { "Node at $index is supposed to have child1, but it does not" }
+			check(child2 != b2_nullNode) { "Node at $index is supposed to have child2, but it does not" }
+
+			nodes[index].height = 1 + nodes[child1].height.coerceAtLeast(nodes[child2].height)
+			nodes[index].aabb =
+				checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" }
+				.combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" })
+
+			index = nodes[index].parent
+		}
+
+		// validate()
+	}
+
+	private fun removeLeaf(leaf: Int) {
+		if (leaf == root) {
+			root = b2_nullNode
+			return
+		}
+
+		val parent = nodes[leaf].parent
+		val grandParent = nodes[parent].parent
+		val sibling: Int
+
+		if (nodes[parent].child1 == leaf) {
+			sibling = nodes[parent].child2
+		} else {
+			sibling = nodes[parent].child1
+		}
+
+		if (grandParent != b2_nullNode) {
+			// Destroy parent and connect sibling to grandParent.
+			if (nodes[grandParent].child1 == parent) {
+				nodes[grandParent].child1 = sibling
+			} else {
+				nodes[grandParent].child2 = sibling
+			}
+
+			nodes[sibling].parent = grandParent
+			freeNode(parent)
+
+			// Adjust ancestor bounds.
+			var index = grandParent
+
+			while (index != b2_nullNode) {
+				index = balance(index)
+
+				val child1 = nodes[index].child1
+				val child2 = nodes[index].child2
+
+				nodes[index].aabb =
+					checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" }
+					.combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" })
+
+				nodes[index].height = 1 + nodes[child1].height.coerceAtLeast(nodes[child2].height)
+
+				index = nodes[index].parent
+			}
+		} else {
+			root = sibling
+			nodes[sibling].parent = b2_nullNode
+			freeNode(parent)
+		}
+
+		// validate()
+	}
+
+	/**
+	 * Perform a left or right rotation if node A is imbalanced.
+	 * Returns the new root index.
+	 */
+	private fun balance(iA: Int): Int {
+		require(iA != b2_nullNode) { "iA is a null node" }
+
+		val A = nodes[iA]
+
+		if (A.isLeaf || A.height < 2) {
+			return iA
+		}
+
+		val iB = A.child1
+		val iC = A.child2
+
+		val B = nodes[iB]
+		val C = nodes[iC]
+
+		val balance = C.height - B.height
+
+		// Rotate C up
+		if (balance > 1) {
+			val iF = C.child1
+			val iG = C.child2
+
+			val F = nodes[iF]
+			val G = nodes[iG]
+
+			// Swap A and C
+			C.child1 = iA
+			C.parent = A.parent
+			A.parent = iC
+
+			// A's old parent should point to C
+			if (C.parent != b2_nullNode) {
+				if (nodes[C.parent].child1 == iA) {
+					nodes[C.parent].child1 = iC
+				} else {
+					check(nodes[C.parent].child2 == iA) { "${nodes[C.parent].child2} != $iA" }
+					nodes[C.parent].child2 = iC
+				}
+			} else {
+				root = iC
+			}
+
+			// Rotate
+			if (F.height > G.height) {
+				C.child2 = iF
+				A.child2 = iG
+				G.parent = iA
+				A.aabb = checkNotNull(B.aabb) { "Node at $iB has null aabb" }.combine(checkNotNull(G.aabb) { "Node at $iG has null aabb" })
+				C.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(F.aabb) { "Node at $iF has null aabb" })
+
+				A.height = 1 + b2Max(B.height, G.height)
+				C.height = 1 + b2Max(A.height, F.height)
+			} else {
+				C.child2 = iG
+				A.child2 = iF
+				F.parent = iA
+
+				A.aabb = checkNotNull(B.aabb) { "Node at $iB has null aabb" }.combine(checkNotNull(F.aabb) { "Node at $iF has null aabb" })
+				C.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(G.aabb) { "Node at $iG has null aabb" })
+
+				A.height = 1 + b2Max(B.height, F.height)
+				C.height = 1 + b2Max(A.height, G.height)
+			}
+
+			return iC
+		}
+
+		// rotate B up
+		if (balance < -1) {
+			val iD = B.child1
+			val iE = B.child2
+
+			val D = nodes[iD]
+			val E = nodes[iE]
+
+			// Swap A and B
+			B.child1 = iA
+			B.parent = A.parent
+			A.parent = iB
+
+			// A's old parent should point to B
+			if (B.parent != b2_nullNode) {
+				if (nodes[B.parent].child1 == iA) {
+					nodes[B.parent].child1 = iB
+				} else {
+					check(nodes[B.parent].child2 == iA) { "${nodes[B.parent].child2} != $iA" }
+					nodes[B.parent].child2 = iB
+				}
+			} else {
+				root = iB
+			}
+
+			// Rotate
+			if (D.height > E.height) {
+				B.child2 = iD
+				A.child1 = iE
+				E.parent = iA
+
+				A.aabb = checkNotNull(C.aabb) { "Node at $iC has null aabb" }.combine(checkNotNull(E.aabb) { "Node at $iE has null aabb" })
+				B.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(D.aabb) { "Node at $iD has null aabb" })
+
+				A.height = 1 + b2Max(C.height, E.height)
+				B.height = 1 + b2Max(A.height, D.height)
+			} else {
+				B.child2 = iE
+				A.child1 = iD
+				D.parent = iA
+
+				A.aabb = checkNotNull(C.aabb) { "Node at $iC has null aabb" }.combine(checkNotNull(D.aabb) { "Node at $iD has null aabb" })
+				B.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(E.aabb) { "Node at $iE has null aabb" })
+
+				A.height = 1 + b2Max(C.height, D.height)
+				B.height = 1 + b2Max(A.height, E.height)
+			}
+
+			return iB
+		}
+
+		return iA
+	}
+
+	override val height: Int get() = if (root == b2_nullNode) 0 else nodes[root].height
+
+	override val getAreaRatio: Double get() {
+		if (root == b2_nullNode)
+			return 0.0
+
+		val root = nodes[root]
+		val rootArea = checkNotNull(root.aabb) { "Node at ${this.root} has null aabb" }.perimeter
+
+		var totalArea = 0.0
+
+		for ((i, node) in nodes.withIndex())
+			if (node.height >= 0)
+				totalArea += checkNotNull(node.aabb) { "Node at $i has null aabb" }.perimeter
+
+		return totalArea / rootArea
+	}
+
+	private fun computeHeight(nodeId: Int): Int {
+		val node = nodes[nodeId]
+
+		if (node.isLeaf)
+			return 0
+
+		val height1 = computeHeight(node.child1)
+		val height2 = computeHeight(node.child2)
+		return 1 + height1.coerceAtLeast(height2)
+	}
+
+	private fun computeHeight() = computeHeight(root)
+
+	private fun validateStructure(index: Int) {
+		if (index == b2_nullNode)
+			return
+
+		if (index == root)
+			check(nodes[index].parent == b2_nullNode)
+
+		val node = nodes[index]
+
+		val child1 = node.child1
+		val child2 = node.child2
+
+		if (node.isLeaf) {
+			check(child1 == b2_nullNode)
+			check(child2 == b2_nullNode)
+			check(node.height == 0)
+			return
+		}
+
+		check(nodes[child1].parent == index)
+		check(nodes[child2].parent == index)
+
+		validateStructure(child1)
+		validateStructure(child2)
+	}
+
+	private fun validateMetrics(index: Int) {
+		if (index == b2_nullNode)
+			return
+
+		val node = nodes[index]
+
+		val child1 = node.child1
+		val child2 = node.child2
+
+		if (node.isLeaf) {
+			check(child1 == b2_nullNode)
+			check(child2 == b2_nullNode)
+			check(node.height == 0)
+			return
+		}
+
+		val height1 = nodes[child1].height
+		val height2 = nodes[child2].height
+
+		val height = 1 + height1.coerceAtLeast(height2)
+		check(node.height == height1)
+
+		val combined = nodes[child1].aabb!!.combine(nodes[child2].aabb!!)
+		check(combined == node.aabb)
+
+		validateMetrics(child1)
+		validateMetrics(child2)
+	}
+
+	override fun validate() {
+		validateStructure(root)
+		validateMetrics(root)
+
+		var freeCount = 0
+		var freeIndex = freeList
+
+		while (freeIndex != b2_nullNode) {
+			freeIndex = nodes[freeIndex].next
+			freeCount++
+		}
+
+		check(height == computeHeight()) { "Height $height does not match checked height ${computeHeight()}" }
+		check(nodeCount + freeCount == nodeCapacity)
+	}
+
+	override val maxBalance: Int get() {
+		var maxBalance = 0
+
+		for (node in nodes) {
+			if (node.height <= 1)
+				continue
+
+			check(!node.isLeaf)
+
+			val child1 = node.child1
+			val child2 = node.child2
+			val balance = (nodes[child2].height - nodes[child1].height).absoluteValue
+			maxBalance = balance.coerceAtLeast(maxBalance)
+		}
+
+		return maxBalance
+	}
+
+	override fun rebuildBottomUp() {
+		TODO("Not Yet Implemented")
+	}
+
+	override fun shiftOrigin(newOrigin: Vector2d) {
+		// Build array of leaves. Free the rest.
+		for (node in nodes) {
+			if (node.aabb != null) {
+				node.aabb = node.aabb?.minus(newOrigin) ?: throw ConcurrentModificationException()
+			}
+		}
+	}
+
+	override fun query(aabb: AABB, callback: ProxyQueryCallback): Boolean {
+		val stack = ArrayDeque<Int>(256)
+		stack.add(root)
+
+		while (stack.isNotEmpty()) {
+			val nodeId = stack.removeLast()
+
+			if (nodeId == b2_nullNode) {
+				continue
+			}
+
+			val node = nodes[nodeId]
+			val nodeAABB = checkNotNull(node.aabb) { "Tree node at $nodeId has null aabb" }
+
+			if (nodeAABB.intersectWeak(aabb)) {
+				if (node.isLeaf) {
+					if (!callback.invoke(nodeId, node.userData)) {
+						return true
+					}
+				} else {
+					stack.add(node.child1)
+					stack.add(node.child2)
+				}
+			}
+		}
+
+		return false
+	}
+
+	override fun rayCast(input: RayCastInput, callback: ProxyRayCastCallback) {
+		val p1 = input.p1
+		val p2 = input.p2
+		var r = p2 - p1
+		val diff = r
+		require(r.lengthSquared > 0.0) { "Start and end points match: $p1 $p2" }
+		r = r.normalized
+
+		// v is perpendicular to the segment.
+		val v = b2Cross(1.0, r)
+		val abs_v = v.absoluteVector
+
+		// Separating axis for segment (Gino, p80).
+		// |dot(v, p1 - c)| > dot(|v|, h)
+
+		var maxFraction = input.maxFraction
+
+		// Build a bounding box for the segment.
+
+		var t = p1 + diff * maxFraction
+
+		var segmentAABB = AABB(
+			mins = p1.minimumPerComponent(t),
+			maxs = p1.maximumPerComponent(t)
+		)
+
+		val stack = ArrayDeque<Int>(256)
+		stack.add(root)
+
+		while (stack.isNotEmpty()) {
+			val nodeId = stack.removeLast()
+
+			if (nodeId == b2_nullNode) {
+				continue
+			}
+
+			val node = nodes[nodeId]
+			val nodeAABB = checkNotNull(node.aabb) { "Tree node at $nodeId has null aabb" }
+
+			if (!nodeAABB.intersectWeak(segmentAABB)) {
+				continue
+			}
+
+			// Separating axis for segment (Gino, p80).
+			// |dot(v, p1 - c)| > dot(|v|, h)
+			if (v.dotProduct(p1 - nodeAABB.centre).absoluteValue > abs_v.dotProduct(nodeAABB.extents)) {
+				continue
+			}
+
+			if (node.isLeaf) {
+				val value = callback.invoke(RayCastInput(p1, p2, maxFraction), nodeId, node.userData)
+
+				if (value == 0.0) {
+					// The client has terminated the ray cast.
+					return
+				} else if (value > 0.0) {
+					// Update segment bounding box.
+					maxFraction = value
+					t = p1 + diff * maxFraction
+					segmentAABB = AABB(
+						mins = p1.minimumPerComponent(t),
+						maxs = p1.maximumPerComponent(t)
+					)
+				}
+			} else {
+				stack.add(node.child1)
+				stack.add(node.child2)
+			}
+		}
+	}
+
+	companion object {
+		private val R = Vector2d(b2_aabbExtension, b2_aabbExtension)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/TimeOfImpact.kt

@@ -0,0 +1,407 @@
+package ru.dbotthepony.kbox2d.collision
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+import kotlin.math.absoluteValue
+
+var b2_toiCalls = 0
+	private set
+
+var b2_toiRootIters = 0
+	private set
+
+var b2_toiMaxRootIters = 0
+	private set
+
+var b2_toiIters = 0
+	private set
+
+var b2_toiMaxIters = 0
+	private set
+
+var b2_toiTime = 0L
+	private set
+
+var b2_toiMaxTime = 0L
+	private set
+
+private data class MinSeparationResult(
+	val indexA: Int,
+	val indexB: Int,
+	val separation: Double
+)
+
+private class SeparationFunction(
+	cache: SimplexCache,
+	val proxyA: IDistanceProxy,
+	val proxyB: IDistanceProxy,
+	val sweepA: Sweep,
+	val sweepB: Sweep,
+	t1: Double
+) {
+	enum class Type {
+		POINTS,
+		FACE_A,
+		FACE_B,
+	}
+
+	val type: Type
+	val localPoint: Vector2d
+	val axis: Vector2d
+
+	init {
+		require(cache.count in 1 .. 2) { cache.count }
+
+		val xfA = sweepA.getTransform(t1)
+		val xfB = sweepB.getTransform(t1)
+
+		if (cache.count == 1) {
+			type = Type.POINTS
+			val localPointA = proxyA.vertices[cache.indexA[0]]
+			val localPointB = proxyB.vertices[cache.indexB[0]]
+			val pointA = b2Mul(xfA, localPointA)
+			val pointB = b2Mul(xfB, localPointB)
+			axis = pointB - pointA
+
+			localPoint = Vector2d.ZERO
+		} else if (cache.indexA[0] == cache.indexA[1]) {
+			// Two points on B and one on A.
+			type = Type.FACE_B
+			val localPointB1 = proxyB.vertices[cache.indexB[0]]
+			val localPointB2 = proxyB.vertices[cache.indexB[1]]
+
+			val axis = b2Cross(localPointB2 - localPointB1, 1.0).normalized
+			val normal = b2Mul(xfB.q, axis)
+
+			localPoint = 0.5 * (localPointB1 + localPointB2)
+			val pointB = b2Mul(xfB, localPoint)
+
+			val localPointA = proxyA.vertices[cache.indexA[0]]
+			val pointA = b2Mul(xfA, localPointA)
+
+			val s = b2Dot(pointA - pointB, normal)
+
+			if (s < 0.0) {
+				this.axis = -axis
+			} else {
+				this.axis = axis
+			}
+		} else {
+			// Two points on A and one or two points on B.
+			type = Type.FACE_A;
+			val localPointA1 = proxyA.vertices[cache.indexA[0]]
+			val localPointA2 = proxyA.vertices[cache.indexA[1]]
+
+			val axis = b2Cross(localPointA2 - localPointA1, 1.0).normalized
+			val normal = b2Mul(xfA.q, axis)
+
+			localPoint = 0.5 * (localPointA1 + localPointA2)
+			val pointA = b2Mul(xfA, localPoint)
+
+			val localPointB = proxyB.vertices[cache.indexB[0]]
+			val pointB = b2Mul(xfB, localPointB)
+
+			val s = b2Dot(pointB - pointA, normal)
+
+			if (s < 0.0) {
+				this.axis = -axis
+			} else {
+				this.axis = axis
+			}
+		}
+	}
+
+	fun findMinSeparation(t: Double): MinSeparationResult {
+		val xfA = sweepA.getTransform(t)
+		val xfB = sweepB.getTransform(t)
+
+		when (type) {
+			Type.POINTS -> {
+				val axisA = xfA.q.timesT( axis)
+				val axisB = xfB.q.timesT(-axis)
+
+				val indexA = proxyA.getSupport(axisA)
+				val indexB = proxyB.getSupport(axisB)
+
+				val localPointA = proxyA.vertices[indexA]
+				val localPointB = proxyB.vertices[indexB]
+
+				val pointA = xfA.times(localPointA)
+				val pointB = xfB.times(localPointB)
+
+				return MinSeparationResult(
+					indexA = indexA,
+					indexB = indexB,
+					separation = (pointB - pointA).dotProduct(axis)
+				)
+			}
+
+			Type.FACE_A -> {
+				val normal = xfA.q.times(axis)
+				val pointA = xfA.times(localPoint)
+
+				val axisB = xfB.q.timesT(-normal)
+
+				val indexA = -1
+				val indexB = proxyB.getSupport(axisB)
+
+				val localPointB = proxyB.vertices[indexB]
+				val pointB = xfB.times(localPointB)
+
+				val separation = (pointB - pointA).dotProduct(normal)
+				return MinSeparationResult(
+					indexA = indexA,
+					indexB = indexB,
+					separation = separation
+				)
+			}
+
+			Type.FACE_B -> {
+				val normal = xfB.q.times(axis)
+				val pointB = xfB.times(localPoint)
+
+				val axisA = xfA.q.timesT(-normal)
+
+				val indexB = -1
+				val indexA = proxyA.getSupport(axisA)
+
+				val localPointA = proxyA.vertices[indexA]
+				val pointA = xfA.times(localPointA)
+
+				val separation = (pointA - pointB).dotProduct(normal)
+				return MinSeparationResult(
+					indexA = indexA,
+					indexB = indexB,
+					separation = separation
+				)
+			}
+		}
+	}
+
+	fun evaluate(indexA: Int, indexB: Int, t: Double): Double {
+		val xfA = sweepA.getTransform(t)
+		val xfB = sweepB.getTransform(t)
+
+		when (type) {
+			Type.POINTS -> {
+				val localPointA = proxyA.vertices[indexA]
+				val localPointB = proxyB.vertices[indexB]
+
+				val pointA = xfA.times(localPointA)
+				val pointB = xfB.times(localPointB)
+
+				return (pointB - pointA).dotProduct(axis)
+			}
+
+			Type.FACE_A -> {
+				val normal = xfA.q.times(axis);
+				val pointA = xfA.times(localPoint);
+
+				val localPointB = proxyB.vertices[indexB]
+				val pointB = xfB.times(localPointB)
+
+				return (pointB - pointA).dotProduct(normal)
+			}
+
+			Type.FACE_B -> {
+				val normal = xfB.q.times(axis)
+				val pointB = xfB.times(localPoint)
+
+				val localPointA = proxyA.vertices[indexA]
+				val pointA = xfA.times(localPointA)
+
+				return (pointA - pointB).dotProduct(normal)
+			}
+		}
+	}
+}
+
+const val k_maxIterations = 20
+
+/**
+ * Compute the upper bound on time before two shapes penetrate. Time is represented as
+ * a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
+ * non-tunneling collisions. If you change the time interval, you should call this function
+ * again.
+ *
+ * Note: use [b2Distance] to compute the contact point and normal at the time of impact.
+ */
+fun b2TimeOfImpact(
+	proxyA: IDistanceProxy,
+	proxyB: IDistanceProxy,
+	_sweepA: Sweep,
+	_sweepB: Sweep,
+	tMax: Double,		// defines sweep interval [0, tMax]
+): TOIOutput {
+	var timer = System.nanoTime()
+
+	b2_toiCalls++
+
+	var state = TOIOutput.State.UNKNOWN
+	var t = tMax
+
+	// TODO
+	val sweepA = _sweepA.copy()
+	val sweepB = _sweepB.copy()
+
+	sweepA.normalize()
+	sweepB.normalize()
+
+	val totalRadius = proxyA.radius + proxyB.radius
+	val target = b2_linearSlop.coerceAtLeast(totalRadius - 3.0 * b2_linearSlop)
+	val tolerance = 0.25 * b2_linearSlop
+
+	check(target > tolerance) { "$target <= $tolerance" }
+
+	var t1 = 0.0
+	var iter = 0
+
+	// Prepare input for distance query.
+	var cache = SimplexCache()
+	val distanceInput = DistanceInput(
+		proxyA = proxyA,
+		proxyB = proxyB,
+		useRadii = false
+	)
+
+	// The outer loop progressively attempts to compute new separating axes.
+	// This loop terminates when an axis is repeated (no progress is made).
+	while (true) {
+		val xfA = sweepA.getTransform(t1)
+		val xfB = sweepB.getTransform(t1)
+
+		// Get the distance between shapes. We can also use the results
+		// to get a separating axis.
+		distanceInput.transformA = xfA
+		distanceInput.transformB = xfB
+
+		val distanceOutput = b2Distance(cache, distanceInput)
+		cache = distanceOutput.newCache
+
+		// If the shapes are overlapped, we give up on continuous collision.
+		if (distanceOutput.distance <= 0.0) {
+			// Failure!
+			state = TOIOutput.State.OVERLAPPED
+			t = 0.0
+			break
+		}
+
+		// Initialize the separating axis.
+		val fcn = SeparationFunction(cache, proxyA, proxyB, sweepA, sweepB, t1)
+
+		// Compute the TOI on the separating axis. We do this by successively
+		// resolving the deepest point. This loop is bounded by the number of vertices.
+		var done = false
+		var t2 = tMax
+		var pushBackIter = 0
+
+		while (true) {
+			// Find the deepest point at t2. Store the witness point indices.
+			var (indexA, indexB, s2) = fcn.findMinSeparation(t2)
+
+			// Is the final configuration separated?
+			if (s2 > target + tolerance) {
+				// Victory!
+				state = TOIOutput.State.SEPARATED
+				t = tMax
+				done = true
+				break
+			}
+
+			// Has the separation reached tolerance?
+			if (s2 > target - tolerance) {
+				// Advance the sweeps
+				t1 = t2
+				break
+			}
+
+			// Compute the initial separation of the witness points.
+			var s1 = fcn.evaluate(indexA, indexB, t1)
+
+			// Check for initial overlap. This might happen if the root finder
+			// runs out of iterations.
+			if (s1 < target - tolerance) {
+				state = TOIOutput.State.FAILED
+				t = t1
+				done = true
+				break
+			}
+
+			// Check for touching
+			if (s1 <= target + tolerance) {
+				// Victory! t1 should hold the TOI (could be 0.0).
+				state = TOIOutput.State.TOUCHING
+				t = t1
+				done = true
+				break
+			}
+
+			// Compute 1D root of: f(x) - target = 0
+			var rootIterCount = 0
+			var a1 = t1
+			var a2 = t2
+
+			while (rootIterCount < 50) {
+				// Use a mix of the secant rule and bisection.
+				val t: Double
+
+				if (rootIterCount and 1 == 1) {
+					// Secant rule to improve convergence.
+					t = a1 + (target - s1) * (a2 - a1) / (s2 - s1)
+				} else {
+					// Bisection to guarantee progress.
+					t = 0.5 * (a1 + a2)
+				}
+
+				rootIterCount++
+				b2_toiRootIters++
+
+				val s = fcn.evaluate(indexA, indexB, t)
+
+				if ((s - target).absoluteValue < tolerance) {
+					// t2 holds a tentative value for t1
+					t2 = t
+					break
+				}
+
+				// Ensure we continue to bracket the root.
+				if (s > target) {
+					a1 = t
+					s1 = s
+				} else {
+					a2 = t
+					s2 = s
+				}
+			}
+
+			b2_toiMaxRootIters = b2_toiMaxRootIters.coerceAtLeast(rootIterCount)
+			pushBackIter++
+
+			if (pushBackIter == b2_maxPolygonVertices) {
+				break
+			}
+		}
+
+		iter++
+		b2_toiIters++
+
+		if (done) {
+			break
+		}
+
+		if (iter == k_maxIterations) {
+			// Root finder got stuck. Semi-victory.
+			state = TOIOutput.State.FAILED
+			t = t1
+			break
+		}
+	}
+
+	b2_toiMaxIters = b2_toiMaxIters.coerceAtLeast(iter)
+	val spent = System.nanoTime() - timer
+	b2_toiMaxTime = b2_toiMaxTime.coerceAtLeast(spent)
+	b2_toiTime += spent
+
+	return TOIOutput(state, t)
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideCircle.kt

@@ -0,0 +1,149 @@
+package ru.dbotthepony.kbox2d.collision.handler
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.b2Dot
+import ru.dbotthepony.kbox2d.api.b2Mul
+import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+import ru.dbotthepony.kstarbound.math.times
+
+internal fun b2CollideCircles(
+	circleA: CircleShape,
+	xfA: Transform,
+	circleB: CircleShape,
+	xfB: Transform
+): Manifold {
+	val pA = b2Mul(xfA, circleA.p)
+	val pB = b2Mul(xfB, circleB.p)
+
+	val d = pB - pA
+	val distSqr = b2Dot(d, d)
+	val rA = circleA.radius
+	val rB = circleB.radius
+	val radius = rA + rB
+
+	if (distSqr > radius * radius) {
+		return Manifold.EMPTY
+	}
+
+	return Manifold(
+		type = Manifold.Type.CIRCLES,
+		localPoint = circleA.p,
+		points = listOf(
+			ManifoldPoint(
+				localPoint = circleB.p,
+				id = ContactID(key = 0)
+			)
+		)
+	)
+}
+
+internal fun b2CollidePolygonAndCircle(
+	polygonA: PolygonShape,
+	xfA: Transform,
+	circleB: CircleShape,
+	xfB: Transform
+): Manifold {
+	// Compute circle position in the frame of the polygon.
+	val c = b2Mul(xfB, circleB.p)
+	val cLocal = b2MulT(xfA, c)
+
+	// Find the min separating edge.
+	var normalIndex = 0
+	var separation = -Double.MAX_VALUE
+	val radius = polygonA.radius + circleB.radius
+	val vertexCount = polygonA.count
+	val vertices = polygonA.vertices
+	val normals = polygonA.normals
+
+	for (i in 0 until vertexCount) {
+		val s = b2Dot(normals[i], cLocal - vertices[i])
+
+		if (s > radius) {
+			// Early out.
+			return Manifold.EMPTY
+		}
+
+		if (s > separation) {
+			separation = s
+			normalIndex = i
+		}
+	}
+
+	// Vertices that subtend the incident face.
+	val vertIndex1 = normalIndex
+	val vertIndex2 = if (vertIndex1 + 1 < vertexCount) vertIndex1 + 1 else 0
+	val v1 = vertices[vertIndex1]
+	val v2 = vertices[vertIndex2]
+
+	// If the center is inside the polygon ...
+	if (separation < b2_epsilon) {
+		return Manifold(
+			type = Manifold.Type.FACE_A,
+			localNormal = normals[normalIndex],
+			localPoint = 0.5 * (v1 + v2),
+			points = listOf(
+				ManifoldPoint(
+					localPoint = circleB.p,
+					id = ContactID(key = 0)
+				)
+			)
+		)
+	}
+
+	// Compute barycentric coordinates
+	val u1 = b2Dot(cLocal - v1, v2 - v1)
+	val u2 = b2Dot(cLocal - v2, v1 - v2)
+
+	if (u1 <= 0.0f) {
+		if (b2DistanceSquared(cLocal, v1) > radius * radius) {
+			return Manifold.EMPTY
+		}
+
+		return Manifold(
+			type = Manifold.Type.FACE_A,
+			localNormal = (cLocal - v1).normalized,
+			localPoint = v1,
+			points = listOf(
+				ManifoldPoint(
+					localPoint = circleB.p,
+					id = ContactID(key = 0)
+				)
+			)
+		)
+	} else if (u2 <= 0.0f) {
+		if (b2DistanceSquared(cLocal, v2) > radius * radius) {
+			return Manifold.EMPTY
+		}
+
+		return Manifold(
+			type = Manifold.Type.FACE_A,
+			localNormal = (cLocal - v2).normalized,
+			localPoint = v2,
+			points = listOf(
+				ManifoldPoint(
+					localPoint = circleB.p,
+					id = ContactID(key = 0)
+				)
+			)
+		)
+	} else {
+		val faceCenter = 0.5 * (v1 + v2)
+		val s = b2Dot(cLocal - faceCenter, normals[vertIndex1])
+		if (s > radius) {
+			return Manifold.EMPTY
+		}
+
+		return Manifold(
+			type = Manifold.Type.FACE_A,
+			localNormal = normals[vertIndex1],
+			localPoint = faceCenter,
+			points = listOf(
+				ManifoldPoint(
+					localPoint = circleB.p,
+					id = ContactID(key = 0)
+				)
+			)
+		)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideEdge.kt

@@ -0,0 +1,469 @@
+package ru.dbotthepony.kbox2d.collision.handler
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.b2Mul
+import ru.dbotthepony.kbox2d.api.b2MulT
+import ru.dbotthepony.kbox2d.collision.b2ClipSegmentToLine
+import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
+import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+import java.util.*
+import kotlin.collections.ArrayList
+
+internal fun b2CollideEdgeAndCircle(
+	edgeA: EdgeShape,
+	xfA: Transform,
+	circleB: CircleShape,
+	xfB: Transform
+): Manifold {
+	// Compute circle in frame of edge
+	val Q = b2MulT(xfA, b2Mul(xfB, circleB.p))
+
+	val A = edgeA.vertex1
+	val B = edgeA.vertex2
+	val e = B - A
+
+	// Normal points to the right for a CCW winding
+	var n = Vector2d(e.y, -e.x)
+	val offset = b2Dot(n, Q - A)
+
+	if (edgeA.oneSided && offset < 0.0) {
+		return Manifold.EMPTY
+	}
+
+	// Barycentric coordinates
+	val u = b2Dot(e, B - Q)
+	val v = b2Dot(e, Q - A)
+
+	val radius = edgeA.radius + circleB.radius
+	val cf = ContactFeature(
+		indexB = 0,
+		typeB = ContactFeature.Type.VERTEX
+	)
+
+	// Region A
+	if (v <= 0.0) {
+		val P = A
+		val d = Q - P
+		val dd = b2Dot(d, d)
+		if (dd > radius * radius) {
+			return Manifold.EMPTY
+		}
+
+		// Is there an edge connected to A?
+		if (edgeA.oneSided) {
+			val A1 = edgeA.vertex0
+			val B1 = A
+			val e1 = B1 - A1
+			val u1 = b2Dot(e1, B1 - Q)
+
+			// Is the circle in Region AB of the previous edge?
+			if (u1 > 0.0) {
+				return Manifold.EMPTY
+			}
+		}
+
+		cf.indexA = 0
+		cf.typeA = ContactFeature.Type.VERTEX
+
+		return Manifold(
+			type = Manifold.Type.CIRCLES,
+			localPoint = P,
+			points = listOf(
+				ManifoldPoint(
+					id = ContactID(key = 0, cf = cf),
+					localPoint = circleB.p
+				)
+			)
+		)
+	}
+
+	// Region B
+	if (u <= 0.0) {
+		val P = B
+		val d = Q - P
+		val dd = b2Dot(d, d)
+		if (dd > radius * radius) {
+			return Manifold.EMPTY
+		}
+
+		// Is there an edge connected to B?
+		if (edgeA.oneSided) {
+			val B2 = edgeA.vertex3
+			val A2 = B
+			val e2 = B2 - A2
+			val v2 = b2Dot(e2, Q - A2)
+
+			// Is the circle in Region AB of the next edge?
+			if (v2 > 0.0) {
+				return Manifold.EMPTY
+			}
+		}
+
+		cf.indexA = 1
+		cf.typeA = ContactFeature.Type.VERTEX
+
+		return Manifold(
+			type = Manifold.Type.CIRCLES,
+			localPoint = P,
+			points = listOf(
+				ManifoldPoint(
+					id = ContactID(key = 0, cf = cf),
+					localPoint = circleB.p
+				)
+			)
+		)
+	}
+
+	// Region AB
+	val den = b2Dot(e, e)
+	check(den > 0.0) { den }
+	val P = (1.0 / den) * (u * A + v * B)
+	val d = Q - P
+	val dd = b2Dot(d, d)
+	if (dd > radius * radius) {
+		return Manifold.EMPTY
+	}
+
+	if (offset < 0.0) {
+		n = Vector2d(-n.x, -n.y)
+	}
+
+	n = n.normalized
+
+	cf.indexA = 0
+	cf.typeA = ContactFeature.Type.FACE
+
+	return Manifold(
+		type = Manifold.Type.FACE_A,
+		localNormal = n,
+		localPoint = A,
+		points = listOf(
+			ManifoldPoint(
+				id = ContactID(key = 0, cf = cf),
+				localPoint = circleB.p
+			)
+		)
+	)
+}
+
+// This structure is used to keep track of the best separating axis.
+private data class EPAxis(
+	var normal: Vector2d = Vector2d.ZERO,
+	var type: Type = Type.UNKNOWN,
+	var index: Int = -1,
+	var separation: Double = -Double.MAX_VALUE
+) {
+	enum class Type {
+		UNKNOWN,
+		EDGE_A,
+		EDGE_B
+	}
+}
+
+// This holds polygon B expressed in frame A.
+private data class TempPolygon(
+	val vertices: ArrayList<Vector2d> = ArrayList(),
+	val normals: ArrayList<Vector2d> = ArrayList(),
+)
+
+// Reference face used for clipping
+private data class ReferenceFace(
+	var i1: Int = 0,
+	var i2: Int = 0,
+	var v1: Vector2d = Vector2d.ZERO,
+	var v2: Vector2d = Vector2d.ZERO,
+	var normal: Vector2d = Vector2d.ZERO,
+
+	var sideNormal1: Vector2d = Vector2d.ZERO,
+	var sideOffset1: Double = 0.0,
+
+	var sideNormal2: Vector2d = Vector2d.ZERO,
+	var sideOffset2: Double = 0.0,
+)
+
+// Reference face used for clipping
+private fun b2ComputeEdgeSeparation(
+	polygonB: TempPolygon,
+	v1: Vector2d,
+	normal1: Vector2d
+): EPAxis {
+	val axis = EPAxis(type = EPAxis.Type.EDGE_A)
+	val axes = arrayOf(normal1, -normal1)
+
+	// Find axis with least overlap (min-max problem)
+	for (j in axes.indices) {
+		var sj = Double.MAX_VALUE
+
+		// Find deepest polygon vertex along axis j
+		for (v in polygonB.vertices) {
+			val si = b2Dot(axes[j], v - v1)
+
+			if (si < sj) {
+				sj = si
+			}
+		}
+
+		if (sj > axis.separation) {
+			axis.index = j
+			axis.separation = sj
+			axis.normal = axes[j]
+		}
+	}
+
+	return axis
+}
+
+private fun b2ComputePolygonSeparation(
+	polygonB: TempPolygon,
+	v1: Vector2d,
+	v2: Vector2d
+): EPAxis {
+	val axis = EPAxis()
+
+	for (i in polygonB.vertices.indices) {
+		val n = -polygonB.normals[i]
+
+		val s1 = b2Dot(n, polygonB.vertices[i] - v1)
+		val s2 = b2Dot(n, polygonB.vertices[i] - v2)
+		val s = b2Min(s1, s2)
+
+		if (s > axis.separation) {
+			axis.type = EPAxis.Type.EDGE_B
+			axis.index = i
+			axis.separation = s
+			axis.normal = n
+		}
+	}
+
+	return axis
+}
+
+private const val k_relativeTol = 0.98
+private const val k_absoluteTol = 0.001
+private const val sinTol = 0.1
+
+internal fun b2CollideEdgeAndPolygon(
+	edgeA: EdgeShape,
+	xfA: Transform,
+	polygonB: PolygonShape,
+	xfB: Transform
+): Manifold {
+	val xf = b2MulT(xfA, xfB)
+
+	val centroidB = b2Mul(xf, polygonB.centroid)
+
+	val v1 = edgeA.vertex1
+	val v2 = edgeA.vertex2
+
+	val edge1 = (v2 - v1).normalized
+
+	// Normal points to the right for a CCW winding
+	var normal1 = Vector2d(edge1.y, -edge1.x)
+	val offset1 = b2Dot(normal1, centroidB - v1)
+
+	if (edgeA.oneSided && offset1 < 0.0) {
+		return Manifold.EMPTY
+	}
+
+	// Get polygonB in frameA
+	val tempPolygonB = TempPolygon(ArrayList(polygonB.count), ArrayList(polygonB.count))
+
+	for (i in 0 until polygonB.count) {
+		tempPolygonB.vertices.add(b2Mul(xf, polygonB.vertices[i]))
+		tempPolygonB.normals.add(b2Mul(xf.q, polygonB.normals[i]))
+	}
+
+	val radius = polygonB.radius + edgeA.radius
+
+	val edgeAxis = b2ComputeEdgeSeparation(tempPolygonB, v1, normal1)
+	if (edgeAxis.separation > radius) {
+		return Manifold.EMPTY
+	}
+
+	val polygonAxis = b2ComputePolygonSeparation(tempPolygonB, v1, v2)
+	if (polygonAxis.separation > radius) {
+		return Manifold.EMPTY
+	}
+
+	// Use hysteresis for jitter reduction.
+	var primaryAxis: EPAxis
+
+	if (polygonAxis.separation - radius > k_relativeTol * (edgeAxis.separation - radius) + k_absoluteTol) {
+		primaryAxis = polygonAxis
+	} else {
+		primaryAxis = edgeAxis
+	}
+
+	if (edgeA.oneSided) {
+		// Smooth collision
+		// See https://box2d.org/posts/2020/06/ghost-collisions/
+
+		val edge0 = (v1 - edgeA.vertex0).normalized
+		val normal0 = Vector2d(edge0.y, -edge0.x)
+		val convex1 = b2Cross(edge0, edge1) >= 0.0f
+
+		val edge2 = (edgeA.vertex3 - v2).normalized
+		val normal2 = Vector2d(edge2.y, -edge2.x)
+		val convex2 = b2Cross(edge1, edge2) >= 0.0f
+
+		val side1 = b2Dot(primaryAxis.normal, edge1) <= 0.0
+
+		// Check Gauss Map
+		if (side1) {
+			if (convex1) {
+				if (b2Cross(primaryAxis.normal, normal0) > sinTol) {
+					// Skip region
+					return Manifold.EMPTY
+				}
+
+				// Admit region
+			} else {
+				// Snap region
+				primaryAxis = edgeAxis
+			}
+		} else {
+			if (convex2) {
+				if (b2Cross(normal2, primaryAxis.normal) > sinTol) {
+					// Skip region
+					return Manifold.EMPTY
+				}
+
+				// Admit region
+			} else {
+				// Snap region
+				primaryAxis = edgeAxis
+			}
+		}
+	}
+
+	val ref = ReferenceFace()
+	var manifoldType: Manifold.Type
+	val clipPoints = Array(2) { ClipVertex() }
+
+	if (primaryAxis.type == EPAxis.Type.EDGE_A) {
+		manifoldType = Manifold.Type.FACE_A
+
+		// Search for the polygon normal that is most anti-parallel to the edge normal.
+		var bestIndex = 0
+		var bestValue = b2Dot(primaryAxis.normal, tempPolygonB.normals[0])
+
+		for (i in 1 until tempPolygonB.normals.size) {
+			val value = b2Dot(primaryAxis.normal, tempPolygonB.normals[i])
+
+			if (value < bestValue) {
+				bestValue = value
+				bestIndex = i
+			}
+		}
+
+		val i1 = bestIndex
+		val i2 = if (i1 + 1 < tempPolygonB.normals.size) i1 + 1 else 0
+
+		clipPoints[0].v = tempPolygonB.vertices[i1]
+		clipPoints[0].id.cf.indexA = 0
+		clipPoints[0].id.cf.indexB = i1
+		clipPoints[0].id.cf.typeA = ContactFeature.Type.FACE
+		clipPoints[0].id.cf.typeB = ContactFeature.Type.VERTEX
+
+		clipPoints[1].v = tempPolygonB.vertices[i2]
+		clipPoints[1].id.cf.indexA = 0
+		clipPoints[1].id.cf.indexB = i2
+		clipPoints[1].id.cf.typeA = ContactFeature.Type.FACE
+		clipPoints[1].id.cf.typeB = ContactFeature.Type.VERTEX
+
+		ref.i1 = 0
+		ref.i2 = 1
+		ref.v1 = v1
+		ref.v2 = v2
+		ref.normal = primaryAxis.normal
+		ref.sideNormal1 = -edge1
+		ref.sideNormal2 = edge1
+	} else {
+		manifoldType = Manifold.Type.FACE_B
+
+		clipPoints[0].v = v2
+		clipPoints[0].id.cf.indexA = 1
+		clipPoints[0].id.cf.indexB = primaryAxis.index
+		clipPoints[0].id.cf.typeA = ContactFeature.Type.VERTEX
+		clipPoints[0].id.cf.typeB = ContactFeature.Type.FACE
+
+		clipPoints[1].v = v1
+		clipPoints[1].id.cf.indexA = 0
+		clipPoints[1].id.cf.indexB = primaryAxis.index
+		clipPoints[1].id.cf.typeA = ContactFeature.Type.VERTEX
+		clipPoints[1].id.cf.typeB = ContactFeature.Type.FACE
+
+		ref.i1 = primaryAxis.index;
+		ref.i2 = if (ref.i1 + 1 < tempPolygonB.normals.size) ref.i1 + 1 else 0
+		ref.v1 = tempPolygonB.vertices[ref.i1]
+		ref.v2 = tempPolygonB.vertices[ref.i2]
+		ref.normal = tempPolygonB.normals[ref.i1]
+
+		// CCW winding
+		ref.sideNormal1 = Vector2d(ref.normal.y, -ref.normal.x)
+		ref.sideNormal2 = -ref.sideNormal1
+	}
+
+	ref.sideOffset1 = b2Dot(ref.sideNormal1, ref.v1)
+	ref.sideOffset2 = b2Dot(ref.sideNormal2, ref.v2)
+
+	// Clip incident edge against reference face side planes
+
+	// Clip to side 1
+	val clipPoints1 = b2ClipSegmentToLine(clipPoints, ref.sideNormal1, ref.sideOffset1, ref.i1)
+
+	if (clipPoints1.size < b2_maxManifoldPoints) {
+		return Manifold.EMPTY
+	}
+
+	// Clip to side 2
+	val clipPoints2 = b2ClipSegmentToLine(clipPoints1, ref.sideNormal2, ref.sideOffset2, ref.i2)
+
+	if (clipPoints2.size < b2_maxManifoldPoints) {
+		return Manifold.EMPTY
+	}
+
+	val localNormal: Vector2d
+	val localPoint: Vector2d
+
+	// Now clipPoints2 contains the clipped points.
+	if (primaryAxis.type == EPAxis.Type.EDGE_A) {
+		localNormal = ref.normal
+		localPoint = ref.v1
+	} else {
+		localNormal = polygonB.normals[ref.i1]
+		localPoint = polygonB.vertices[ref.i1]
+	}
+
+	val points = ArrayList<ManifoldPoint>(b2_maxManifoldPoints)
+
+	for (i in 0 until b2_maxManifoldPoints) {
+		val separation = b2Dot(ref.normal, clipPoints2[i].v - ref.v1)
+
+		if (separation <= radius) {
+			val cp = ManifoldPoint()
+
+			if (primaryAxis.type == EPAxis.Type.EDGE_A) {
+				cp.localPoint = b2MulT(xf, clipPoints2[i].v)
+				cp.id = clipPoints2[i].id
+			} else {
+				cp.localPoint = clipPoints2[i].v
+				cp.id.cf.typeA = clipPoints2[i].id.cf.typeB
+				cp.id.cf.typeB = clipPoints2[i].id.cf.typeA
+				cp.id.cf.indexA = clipPoints2[i].id.cf.indexB
+				cp.id.cf.indexB = clipPoints2[i].id.cf.indexA
+			}
+
+			points.add(cp)
+		}
+	}
+
+	return Manifold(
+		localNormal = localNormal,
+		localPoint = localPoint,
+		type = manifoldType,
+		points = Collections.unmodifiableList(points))
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollidePolygon.kt

@@ -0,0 +1,250 @@
+package ru.dbotthepony.kbox2d.collision.handler
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.b2Dot
+import ru.dbotthepony.kbox2d.api.b2Mul
+import ru.dbotthepony.kbox2d.api.b2MulT
+import ru.dbotthepony.kbox2d.collision.b2ClipSegmentToLine
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+import ru.dbotthepony.kstarbound.math.times
+import java.util.*
+import kotlin.collections.ArrayList
+
+internal data class FindMaxSeparationResult(
+	val edgeIndex: Int,
+	val maxSeparation: Double
+)
+
+// Find the max separation between poly1 and poly2 using edge normals from poly1.
+internal fun b2FindMaxSeparation(
+	poly1: PolygonShape,
+	xf1: Transform,
+	poly2: PolygonShape,
+	xf2: Transform
+): FindMaxSeparationResult {
+	val count1 = poly1.vertices.size
+	val count2 = poly2.vertices.size
+	val n1s = poly1.normals
+	val v1s = poly1.vertices
+	val v2s = poly2.vertices
+
+	var bestIndex = 0
+	var maxSeparation = -Double.MAX_VALUE
+
+	val xf = b2MulT(xf2, xf1)
+
+	for (i in 0 until count1) {
+		// Get poly1 normal in frame2.
+		val n = b2Mul(xf.q, n1s[i])
+		val v1 = b2Mul(xf, v1s[i])
+
+		// Find deepest point for normal i.
+		var si = Double.MAX_VALUE
+		for (j in 0 until count2) {
+			val sij = b2Dot(n, v2s[j] - v1)
+
+			if (sij < si) {
+				si = sij
+			}
+		}
+
+		if (si > maxSeparation) {
+			maxSeparation = si
+			bestIndex = i
+		}
+	}
+
+	return FindMaxSeparationResult(
+		edgeIndex = bestIndex,
+		maxSeparation = maxSeparation
+	)
+}
+
+internal fun b2FindIncidentEdge(
+	poly1: PolygonShape,
+	xf1: Transform,
+	edge1: Int,
+	poly2: PolygonShape,
+	xf2: Transform
+): Array<ClipVertex> {
+	val normals1 = poly1.normals
+	val count2 = poly2.count
+	val vertices2 = poly2.vertices
+	val normals2 = poly2.normals
+
+	require(edge1 in 0 until poly1.count) { "$edge1 is not in range of 0 until ${poly1.count}" }
+
+	// Get the normal of the reference edge in poly2's frame.
+	val normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]))
+
+	// Find the incident edge on poly2.
+	var index = 0
+	var minDot = Double.MAX_VALUE
+
+	for (i in 0 until count2) {
+		val dot = b2Dot(normal1, normals2[i])
+		if (dot < minDot) {
+			minDot = dot
+			index = i
+		}
+	}
+
+	// Build the clip vertices for the incident edge.
+	val i1 = index
+	val i2 = if (i1 + 1 < count2) i1 + 1 else 0
+
+	val c0 = ClipVertex(
+		v = b2Mul(xf2, vertices2[i1]),
+		id = ContactID(
+			cf = ContactFeature(
+				indexA = edge1,
+				indexB = i1,
+				typeA = ContactFeature.Type.FACE,
+				typeB = ContactFeature.Type.VERTEX,
+			)
+		)
+	)
+
+	val c1 = ClipVertex(
+		v = b2Mul(xf2, vertices2[i2]),
+		id = ContactID(
+			cf = ContactFeature(
+				indexA = edge1,
+				indexB = i2,
+				typeA = ContactFeature.Type.FACE,
+				typeB = ContactFeature.Type.VERTEX
+			)
+		)
+	)
+
+	return arrayOf(c0, c1)
+}
+
+// Find edge normal of max separation on A - return if separating axis is found
+// Find edge normal of max separation on B - return if separation axis is found
+// Choose reference edge as min(minA, minB)
+// Find incident edge
+// Clip
+
+private const val k_tol = 0.1 * b2_linearSlop
+
+// The normal points from 1 to 2
+internal fun b2CollidePolygons(
+	polyA: PolygonShape,
+	xfA: Transform,
+	polyB: PolygonShape,
+	xfB: Transform
+): Manifold {
+	val totalRadius = polyA.radius + polyB.radius
+
+	val (edgeA, separationA) = b2FindMaxSeparation(polyA, xfA, polyB, xfB)
+
+	if (separationA > totalRadius) {
+		return Manifold.EMPTY
+	}
+
+	val (edgeB, separationB) = b2FindMaxSeparation(polyB, xfB, polyA, xfA)
+
+	if (separationB > totalRadius) {
+		return Manifold.EMPTY
+	}
+
+	val poly1: PolygonShape // reference polygon
+	val poly2: PolygonShape // incident polygon
+	val xf1: Transform
+	val xf2: Transform
+	val edge1: Int // reference edge
+	val flip: Boolean
+
+	val type: Manifold.Type
+
+	if (separationB > separationA + k_tol) {
+		poly1 = polyB
+		poly2 = polyA
+		xf1 = xfB
+		xf2 = xfA
+		edge1 = edgeB
+		type = Manifold.Type.FACE_B
+		flip = true
+	} else {
+		poly1 = polyA
+		poly2 = polyB
+		xf1 = xfA
+		xf2 = xfB
+		edge1 = edgeA
+		type = Manifold.Type.FACE_A
+		flip = false
+	}
+
+	val incidentEdge = b2FindIncidentEdge(poly1, xf1, edge1, poly2, xf2)
+	val count1 = poly1.count
+	val vertices1 = poly1.vertices
+
+	val iv1 = edge1
+	val iv2 = if (edge1 + 1 < count1) edge1 + 1 else 0
+
+	var v11 = vertices1[iv1]
+	var v12 = vertices1[iv2]
+
+	val localTangent = (v12 - v11).normalized
+
+	val localNormal = b2Cross(localTangent, 1.0);
+	val planePoint = 0.5 * (v11 + v12)
+
+	val tangent = b2Mul(xf1.q, localTangent)
+	val normal = b2Cross(tangent, 1.0)
+
+	v11 = b2Mul(xf1, v11)
+	v12 = b2Mul(xf1, v12)
+
+	// Face offset.
+	val frontOffset = b2Dot(normal, v11)
+
+	// Side offsets, extended by polytope skin thickness.
+	val sideOffset1 = -b2Dot(tangent, v11) + totalRadius
+	val sideOffset2 = b2Dot(tangent, v12) + totalRadius
+
+	// Clip incident edge against extruded edge1 side edges.
+	val clipPoints1 = b2ClipSegmentToLine(incidentEdge, -tangent, sideOffset1, iv1)
+
+	if (clipPoints1.size < 2)
+		return Manifold(type = type)
+
+	// Clip to negative box side 1
+	val clipPoints2 = b2ClipSegmentToLine(clipPoints1, tangent, sideOffset2, iv2)
+
+	if (clipPoints2.size < 2)
+		return Manifold(type = type)
+
+	// Now clipPoints2 contains the clipped points.
+	val points = ArrayList<ManifoldPoint>()
+
+	for (i in 0 until b2_maxManifoldPoints) {
+		val separation = b2Dot(normal, clipPoints2[i].v) - frontOffset
+
+		if (separation <= totalRadius) {
+			val cp = ManifoldPoint(
+				localPoint = b2MulT(xf2, clipPoints2[i].v),
+				id = clipPoints2[i].id
+			)
+
+			if (flip) {
+				// Swap features
+				val cf = cp.id.cf
+				cp.id.cf.indexA = cf.indexB
+				cp.id.cf.indexB = cf.indexA
+				cp.id.cf.typeA = cf.typeB
+				cp.id.cf.typeB = cf.typeA
+			}
+
+			points.add(cp)
+		}
+	}
+
+	return Manifold(
+		localPoint = planePoint,
+		localNormal = localNormal,
+		points = Collections.unmodifiableList(points),
+		type = type
+	)
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/ChainShape.kt

@@ -0,0 +1,191 @@
+package ru.dbotthepony.kbox2d.collision.shapes
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+/**
+ * A chain shape is a free form sequence of line segments.
+ * The chain has one-sided collision, with the surface normal pointing to the right of the edge.
+ * This provides a counter-clockwise winding like the polygon shape.
+ * Connectivity information is used to create smooth collisions.
+ * @warning the chain will not collide properly if there are self-intersections.
+ */
+class ChainShape : IShape<ChainShape> {
+	override fun copy(): ChainShape {
+		return ChainShape().also {
+			it.vertices.addAll(vertices)
+			it.prevVertex = prevVertex
+			it.nextVertex = nextVertex
+			it.edgeCache = arrayOfNulls(vertices.size - 1)
+			it.rayEdgeCache = arrayOfNulls(vertices.size)
+		}
+	}
+
+	private fun validateInput(vertices: List<Vector2d>) {
+		for (i in 1 until vertices.size) {
+			val v1 = vertices[i - 1]
+			val v2 = vertices[i]
+
+			v1.isFiniteOrThrow { "Vertex at ${i - 1} is invalid" }
+			v2.isFiniteOrThrow { "Vertex at $i is invalid" }
+
+			require(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop) {
+				"Vertices are too close together, at indices ${i - 1} ($v1) and $i ($v2)"
+			}
+		}
+	}
+
+	/**
+	 * Create a loop. This automatically adjusts connectivity.
+	 * @param vertices an array of vertices, these are copied
+	 * @param count the vertex count
+	 */
+	fun createLoop(vertices: List<Vector2d>) {
+		// b2Assert(m_vertices == nullptr && m_count == 0);
+		// KBox2D: not required because we are much more flexible at memory than C++
+
+		require(vertices.size >= 3) { "Can not create loop with ${vertices.size} vertices" }
+
+		validateInput(vertices)
+		this.vertices.clear()
+
+		this.vertices.addAll(vertices)
+		this.vertices.add(vertices[0])
+
+		prevVertex = vertices[vertices.size - 1]
+		nextVertex = vertices[1]
+
+		edgeCache = arrayOfNulls(this.vertices.size - 1)
+		rayEdgeCache = arrayOfNulls(this.vertices.size)
+	}
+
+	/**
+	 * Create a chain with ghost vertices to connect multiple chains together.
+	 * @param vertices an array of vertices, these are copied
+	 * @param count the vertex count
+	 * @param prevVertex previous vertex from chain that connects to the start
+	 * @param nextVertex next vertex from chain that connects to the end
+	 */
+	fun createChain(vertices: List<Vector2d>, prevVertex: Vector2d, nextVertex: Vector2d) {
+		// b2Assert(m_vertices == nullptr && m_count == 0);
+		// KBox2D: not required because we are much more flexible at memory than C++
+
+		require(vertices.size >= 2) { "Can not create chain with ${vertices.size} vertices" }
+
+		validateInput(vertices)
+		this.vertices.clear()
+		this.vertices.addAll(vertices)
+
+		this.prevVertex = prevVertex
+		this.nextVertex = nextVertex
+
+		edgeCache = arrayOfNulls(this.vertices.size - 1)
+		rayEdgeCache = arrayOfNulls(this.vertices.size)
+	}
+
+	internal val vertices = ArrayList<Vector2d>()
+	internal var prevVertex: Vector2d? = null
+	internal var nextVertex: Vector2d? = null
+
+	override val type: IShape.Type = IShape.Type.CHAIN
+	// edge count = vertex count - 1
+	override val childCount: Int get() = vertices.size - 1
+
+	private var edgeCache: Array<EdgeShape?> = arrayOfNulls(0)
+	private var rayEdgeCache: Array<EdgeShape?> = arrayOfNulls(0)
+
+	/**
+	 * Get a child edge.
+	 */
+	fun getChildEdge(index: Int): EdgeShape {
+		val getEdge = edgeCache[index]
+
+		if (getEdge != null) {
+			return getEdge
+		}
+
+		val edge = EdgeShape()
+
+		edge.vertex1 = vertices[index]
+		edge.vertex2 = vertices[index + 1]
+		edge.oneSided = true
+
+		if (index > 0) {
+			edge.vertex0 = vertices[index - 1]
+		} else {
+			edge.vertex0 = prevVertex ?: throw NullPointerException("prevVertex is null")
+		}
+
+		if (index < vertices.size - 2) {
+			edge.vertex3 = vertices[index + 2]
+		} else {
+			edge.vertex3 = nextVertex ?: throw NullPointerException("nextVertex is null")
+		}
+
+		edgeCache[index] = edge
+		return edge
+	}
+
+	override fun rayCast(input: RayCastInput, transform: Transform, childIndex: Int): RayCastOutput {
+		val getEdge = rayEdgeCache[childIndex]
+
+		if (getEdge != null) {
+			return getEdge.rayCast(input, transform, 0)
+		}
+
+		val edgeShape = EdgeShape()
+
+		val i1 = childIndex
+		var i2 = childIndex + 1
+
+		if (i2 == vertices.size) {
+			i2 = 0
+		}
+
+		edgeShape.vertex1 = vertices[i1]
+		edgeShape.vertex2 = vertices[i2]
+
+		rayEdgeCache[childIndex] = edgeShape
+		return edgeShape.rayCast(input, transform, 0)
+	}
+
+	override fun computeAABB(transform: Transform, childIndex: Int): AABB {
+		val i1 = childIndex
+		var i2 = childIndex + 1
+
+		if (i2 == vertices.size) {
+			i2 = 0
+		}
+
+		val v1 = b2Mul(transform, vertices[i1])
+		val v2 = b2Mul(transform, vertices[i2])
+
+		val lower = b2Min(v1, v2)
+		val upper = b2Max(v1, v2)
+
+		val r = Vector2d(radius, radius)
+
+		return AABB(
+			mins = lower - r,
+			maxs = upper + r
+		)
+	}
+
+	override fun computeMass(density: Double): MassData {
+		return MASS
+	}
+
+	override var radius: Double = b2_polygonRadius
+		set(value) {
+			if (value == b2_polygonRadius) {
+				field = value
+			}
+
+			throw IllegalArgumentException("")
+		}
+
+	companion object {
+		private val MASS = MassData(mass = 0.0, center = Vector2d.ZERO, inertia = 0.0)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/CircleShape.kt

@@ -0,0 +1,79 @@
+package ru.dbotthepony.kbox2d.collision.shapes
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+import kotlin.math.PI
+import kotlin.math.sqrt
+
+class CircleShape(
+	override var radius: Double = 0.0,
+	var position: Vector2d = Vector2d.ZERO,
+) : IShape<CircleShape> {
+	var p by this::position
+
+	override fun copy(): CircleShape {
+		return CircleShape().also {
+			it.position = this.position
+			it.radius = this.radius
+		}
+	}
+
+	override val type: IShape.Type = IShape.Type.CIRCLE
+	override val childCount: Int = 1
+
+	override fun testPoint(transform: Transform, p: Vector2d): Boolean {
+		val center = transform.p + b2Mul(transform.q, position)
+		val d = p - center
+		return b2Dot(d, d) <= radius * radius
+	}
+
+	override fun rayCast(input: RayCastInput, transform: Transform, childIndex: Int): RayCastOutput {
+		val position = transform.p + b2Mul(transform.q, position)
+		val s = input.p1 - position
+		val b = b2Dot(s, s) - radius * radius
+
+		// Solve quadratic equation.
+		// Solve quadratic equation.
+		val r = input.p2 - input.p1
+		val c =  b2Dot(s, r)
+		val rr = b2Dot(r, r)
+		val sigma = c * c - rr * b
+
+		// Check for negative discriminant and short segment.
+		if (sigma < 0.0 || rr < b2_epsilon) {
+			return RayCastOutput.MISS
+		}
+
+		// Find the point of intersection of the line with the circle.
+		var a = -(c + sqrt(sigma))
+
+		// Is the intersection point on the segment?
+		if (a in 0.0 .. input.maxFraction * rr) {
+			a /= rr
+			return RayCastOutput(hit = true, fraction = a, normal = (s + a * r).normalized)
+		}
+
+		return RayCastOutput.MISS
+	}
+
+	override fun computeAABB(transform: Transform, childIndex: Int): AABB {
+		val p = transform.p + b2Mul(transform.q, position)
+
+		return AABB(
+			mins = Vector2d(p.x - radius, p.y - radius),
+			maxs = Vector2d(p.x + radius, p.y + radius)
+		)
+	}
+
+	override fun computeMass(density: Double): MassData {
+		val mass = density * PI * radius * radius
+
+		return MassData(
+			mass = mass,
+			center = position,
+			inertia = mass * (0.5 * radius * radius + b2Dot(position, position))
+		)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/EdgeShape.kt

@@ -0,0 +1,188 @@
+package ru.dbotthepony.kbox2d.collision.shapes
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+
+/**
+ * A line segment (edge) shape. These can be connected in chains or loops
+ * to other edge shapes. Edges created independently are two-sided and do
+ * no provide smooth movement across junctions.
+ */
+class EdgeShape : IShape<EdgeShape> {
+	override fun copy(): EdgeShape {
+		return EdgeShape().also {
+			it.oneSided = oneSided
+			it.vertex0 = vertex0
+			it.vertex1 = vertex1
+			it.vertex2 = vertex2
+			it.vertex3 = vertex3
+		}
+	}
+
+	override val type: IShape.Type = IShape.Type.EDGE
+	override val childCount: Int = 1
+
+	override fun rayCast(input: RayCastInput, transform: Transform, childIndex: Int): RayCastOutput {
+		// Put the ray into the edge's frame of reference.
+		val p1 = b2MulT(transform.q, input.p1 - transform.p)
+		val p2 = b2MulT(transform.q, input.p2 - transform.p)
+		val d = p2 - p1
+
+		val v1 = vertex1
+		val v2 = vertex2
+		val e = v2 - v1
+
+		// Normal points to the right, looking from v1 at v2
+		val normal = Vector2d(e.y, -e.x).normalized
+
+		// q = p1 + t * d
+		// dot(normal, q - v1) = 0
+		// dot(normal, p1 - v1) + t * dot(normal, d) = 0
+		val numerator = b2Dot(normal, v1 - p1)
+
+		if (oneSided && numerator > 0.0) {
+			return RayCastOutput.MISS
+		}
+
+		val denominator = b2Dot(normal, d)
+
+		if (denominator == 0.0) {
+			return RayCastOutput.MISS
+		}
+
+		val t = numerator / denominator
+
+		if (t < 0.0 || input.maxFraction < t) {
+			return RayCastOutput.MISS
+		}
+
+		val q = p1 + t * d
+
+		// q = v1 + s * r
+		// s = dot(q - v1, r) / dot(r, r)
+		val r = v2 - v1
+		val rr = b2Dot(r, r)
+
+		if (rr == 0.0) {
+			return RayCastOutput.MISS
+		}
+
+		val s = b2Dot(q - v1, r) / rr
+
+		if (s < 0.0 || 1.0 < s) {
+			return RayCastOutput.MISS
+		}
+
+		val outputNormal: Vector2d
+
+		if (numerator > 0.0) {
+			outputNormal = -b2Mul(transform.q, normal)
+		} else {
+			outputNormal = b2Mul(transform.q, normal)
+		}
+
+		return RayCastOutput(
+			hit = true,
+			normal = outputNormal,
+			fraction = t
+		)
+	}
+
+	override fun computeAABB(transform: Transform, childIndex: Int): AABB {
+		val v1 = b2Mul(transform, vertex1)
+		val v2 = b2Mul(transform, vertex2)
+
+		val lower = b2Min(v1, v2)
+		val upper = b2Max(v1, v2)
+
+		val r = Vector2d(radius, radius)
+
+		return AABB(
+			mins = lower - r,
+			maxs = upper + r
+		)
+	}
+
+	override fun computeMass(density: Double): MassData {
+		return MassData(
+			center = 0.5 * (vertex1 + vertex2),
+			mass = 0.0,
+			inertia = 0.0
+		)
+	}
+
+	override var radius: Double = b2_polygonRadius
+		set(value) {
+			if (value == b2_polygonRadius) {
+				field = value
+				return
+			}
+
+			throw UnsupportedOperationException("For polygonal shapes this must be b2_polygonRadius")
+		}
+
+	/**
+	 * Set this as a part of a sequence. Vertex [v0] precedes the edge and vertex [v3]
+	 * follows. These extra vertices are used to provide smooth movement
+	 * across junctions. This also makes the collision one-sided. The edge
+	 * normal points to the right looking from [v1] to [v2].
+	 */
+	fun setOneSided(v0: Vector2d, v1: Vector2d, v2: Vector2d, v3: Vector2d) {
+		oneSided = true
+		vertex0 = v0
+		vertex1 = v1
+		vertex2 = v2
+		vertex3 = v3
+	}
+
+	/**
+	 * Set this as an isolated edge. Collision is two-sided.
+	 */
+	fun setTwoSided(v1: Vector2d, v2: Vector2d) {
+		oneSided = false
+		vertex1 = v1
+		vertex2 = v2
+	}
+
+	/// These are the edge vertices
+	var vertex1 = Vector2d.ZERO
+		set(value) {
+			if (!value.isFinite) {
+				throw IllegalArgumentException("Tried to set illegal vertex $value")
+			}
+
+			field = value
+		}
+
+	var vertex2 = Vector2d.ZERO
+		set(value) {
+			if (!value.isFinite) {
+				throw IllegalArgumentException("Tried to set illegal vertex $value")
+			}
+
+			field = value
+		}
+
+	/// Optional adjacent vertices. These are used for smooth collision.
+	var vertex0 = Vector2d.ZERO
+		set(value) {
+			if (!value.isFinite) {
+				throw IllegalArgumentException("Tried to set illegal vertex $value")
+			}
+
+			field = value
+		}
+
+	var vertex3 = Vector2d.ZERO
+		set(value) {
+			if (!value.isFinite) {
+				throw IllegalArgumentException("Tried to set illegal vertex $value")
+			}
+
+			field = value
+		}
+
+	var oneSided = false
+}

src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/PolygonShape.kt

@@ -0,0 +1,449 @@
+package ru.dbotthepony.kbox2d.collision.shapes
+
+import it.unimi.dsi.fastutil.ints.IntArrayList
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+
+private const val inv3 = 1.0 / 3.0
+
+private fun computeCentroid(vs: List<Vector2d>): Vector2d {
+	require(vs.size >= 3) { "Got only ${vs.size} vertices" }
+
+	var c = Vector2d.ZERO
+	var area = 0.0
+
+	// Get a reference point for forming triangles.
+	// Use the first vertex to reduce round-off errors.
+	val s = vs[0]
+
+	for (i in vs.indices) {
+		// Triangle vertices.
+		val p1 = vs[0] - s;
+		val p2 = vs[i] - s;
+		val p3 = if (i + 1 < vs.size) vs[i + 1] - s else vs[0] - s;
+
+		val e1 = p2 - p1;
+		val e2 = p3 - p1;
+
+		val D = b2Cross(e1, e2);
+
+		val triangleArea = 0.5 * D;
+		area += triangleArea;
+
+		// Area weighted centroid
+		c += triangleArea * inv3 * (p1 + p2 + p3);
+	}
+
+	// Centroid
+	check(area > b2_epsilon) { area }
+
+	return (1.0 / area) * c + s
+}
+
+class PolygonShape : IShape<PolygonShape> {
+	override fun copy(): PolygonShape {
+		return PolygonShape().also {
+			it.centroid = centroid
+			it.vertices.addAll(vertices)
+			it.normals.addAll(normals)
+		}
+	}
+
+	/**
+	 * Create a convex hull from the given array of local points.
+	 * The count must be in the range [3, b2_maxPolygonVertices].
+	 * @warning the points may be re-ordered, even if they form a convex polygon
+	 * @warning collinear points are handled but not removed. Collinear points
+	 * may lead to poor stacking behavior.
+	 */
+	fun set(vertices: List<Vector2d>) {
+		this.vertices.clear()
+		this.normals.clear()
+
+		require(vertices.size >= 3) { "Got only ${vertices.size} points" }
+
+		// Perform welding and copy vertices into local buffer.
+		val ps = ArrayList<Vector2d>()
+
+		for ((i, v) in vertices.withIndex()) {
+			var unique = true
+
+			for ((j, other) in ps.withIndex()) {
+				if (v.distanceSquared(other) < ((0.5 * b2_linearSlop) * (0.5 * b2_linearSlop))) {
+					unique = false
+					break
+				}
+			}
+
+			if (unique) {
+				ps.add(v)
+			}
+		}
+
+		check(ps.size >= 3) { "Polygon is degenerate" }
+
+		// Create the convex hull using the Gift wrapping algorithm
+		// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
+
+		// Find the right most point on the hull
+		var i0 = 0
+		var x0 = ps[0].x
+
+		for (i in 1 until ps.size) {
+			val x = ps[i].x
+
+			if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) {
+				i0 = i
+				x0 = x
+			}
+		}
+
+		val hull = IntArrayList()
+		var ih = i0
+
+		while (true) {
+			hull.add(ih)
+
+			var ie = 0
+			for (j in 1 until ps.size) {
+				if (ie == ih) {
+					ie = j
+					continue
+				}
+
+				val r = ps[ie] - ps[ih]
+				val v = ps[j] - ps[ih]
+
+				val c = b2Cross(r, v)
+
+				if (c < 0.0) {
+					ie = j
+				}
+
+				// Collinearity check
+				if (c == 0.0 && v.lengthSquared > r.lengthSquared) {
+					ie = j
+				}
+			}
+
+			ih = ie
+
+			if (ie == i0) {
+				break
+			}
+		}
+
+		check(hull.size >= 3) { "Polygon is degenerate" }
+
+		// Copy vertices.
+		for (i in hull.indices) {
+			this.vertices.add(ps[hull.getInt(i)])
+		}
+
+		// Compute normals. Ensure the edges have non-zero length.
+		for (i in hull.indices) {
+			val i2 = if (i + 1 < hull.size) i + 1 else 0
+			val edge = this.vertices[i2] - this.vertices[i]
+			check(edge.lengthSquared > b2_epsilon * b2_epsilon)
+			this.normals.add(b2Cross(edge, 1.0).normalized)
+		}
+
+		// Compute the polygon centroid.
+		centroid = computeCentroid(this.vertices)
+	}
+
+	/**
+	 * Build vertices to represent an axis-aligned box centered on the local origin.
+	 * @param hx the half-width.
+	 * @param hy the half-height.
+	 */
+	fun setAsBox(hx: Double, hy: Double) {
+		vertices.clear()
+		normals.clear()
+
+		vertices.add(Vector2d(-hx, -hy))
+		vertices.add(Vector2d(hx, -hy))
+		vertices.add(Vector2d(hx, hy))
+		vertices.add(Vector2d(-hx, hy))
+
+		normals.add(Vector2d.DOWN)
+		normals.add(Vector2d.RIGHT)
+		normals.add(Vector2d.UP)
+		normals.add(Vector2d.LEFT)
+
+		centroid = Vector2d.ZERO
+	}
+
+	/**
+	 * Build vertices to represent an oriented box.
+	 * @param hx the half-width.
+	 * @param hy the half-height.
+	 * @param center the center of the box in local coordinates.
+	 * @param angle the rotation of the box in local coordinates.
+	 */
+	fun setAsBox(hx: Double, hy: Double, center: Vector2d, angle: Double) {
+		vertices.clear()
+		normals.clear()
+
+		vertices.add(Vector2d(-hx, -hy))
+		vertices.add(Vector2d(hx, -hy))
+		vertices.add(Vector2d(hx, hy))
+		vertices.add(Vector2d(-hx, hy))
+
+		normals.add(Vector2d.DOWN)
+		normals.add(Vector2d.RIGHT)
+		normals.add(Vector2d.UP)
+		normals.add(Vector2d.LEFT)
+
+		centroid = center
+
+		val xf = Transform(center, angle)
+
+		// Transform vertices and normals.
+		for (i in 0 until 4) {
+			vertices[i] = b2Mul(xf, vertices[i])
+			normals[i] = b2Mul(xf.q, normals[i])
+		}
+	}
+
+	override fun testPoint(transform: Transform, p: Vector2d): Boolean {
+		val pLocal = b2MulT(transform.q, p - transform.p)
+
+		for (i in 0 until vertices.size) {
+			val dot = b2Dot(normals[i], pLocal - vertices[i])
+
+			if (dot > 0.0) {
+				return false
+			}
+		}
+
+		return true
+	}
+
+	override val type: IShape.Type = IShape.Type.POLYGON
+	override val childCount: Int = 1
+
+	override fun rayCast(input: RayCastInput, transform: Transform, childIndex: Int): RayCastOutput {
+		// Put the ray into the polygon's frame of reference.
+		val p1 = b2MulT(transform.q, input.p1 - transform.p)
+		val p2 = b2MulT(transform.q, input.p2 - transform.p)
+		val d = p2 - p1
+
+		var lower = 0.0
+		var upper = input.maxFraction
+
+		var index = -1
+
+		for (i in 0 until vertices.size) {
+			// p = p1 + a * d
+			// dot(normal, p - v) = 0
+			// dot(normal, p1 - v) + a * dot(normal, d) = 0
+
+			val numerator = b2Dot(normals[i], vertices[i] - p1)
+			val denominator = b2Dot(normals[i], d)
+
+			if (denominator == 0.0) {
+				if (numerator < 0.0) {
+					return RayCastOutput.MISS
+				}
+			} else {
+				// Note: we want this predicate without division:
+				// lower < numerator / denominator, where denominator < 0
+				// Since denominator < 0, we have to flip the inequality:
+				// lower < numerator / denominator <==> denominator * lower > numerator.
+
+				if (denominator < 0.0 && numerator < lower * denominator) {
+					// Increase lower.
+					// The segment enters this half-space.
+					lower = numerator / denominator
+					index = i
+				} else if (denominator > 0.0 && numerator < upper * denominator) {
+					// Decrease upper.
+					// The segment exits this half-space.
+					upper = numerator / denominator
+				}
+			}
+
+			// The use of epsilon here causes the assert on lower to trip
+			// in some cases. Apparently the use of epsilon was to make edge
+			// shapes work, but now those are handled separately.
+			//if (upper < lower - b2_epsilon)
+
+			if (upper < lower) {
+				return RayCastOutput.MISS
+			}
+		}
+
+		check(lower in 0.0 .. input.maxFraction) { "$lower <=> ${input.maxFraction}!" }
+
+		if (index >= 0) {
+			return RayCastOutput(
+				hit = true,
+				fraction = lower,
+				normal = b2Mul(transform.q, normals[index])
+			)
+		}
+
+		return RayCastOutput.MISS
+	}
+
+	override fun computeAABB(transform: Transform, childIndex: Int): AABB {
+		var lower = b2Mul(transform, vertices[0])
+		var upper = lower
+
+		for (i in 1 until vertices.size) {
+			val v = b2Mul(transform, vertices[i])
+			lower = b2Min(lower, v)
+			upper = b2Max(upper, v)
+		}
+
+		val r = Vector2d(radius, radius)
+
+		return AABB(
+			mins = lower - r,
+			maxs = upper + r
+		)
+	}
+
+	override fun computeMass(density: Double): MassData {
+		// Polygon mass, centroid, and inertia.
+		// Let rho be the polygon density in mass per unit area.
+		// Then:
+		// mass = rho * int(dA)
+		// centroid.x = (1/mass) * rho * int(x * dA)
+		// centroid.y = (1/mass) * rho * int(y * dA)
+		// I = rho * int((x*x + y*y) * dA)
+		//
+		// We can compute these integrals by summing all the integrals
+		// for each triangle of the polygon. To evaluate the integral
+		// for a single triangle, we make a change of variables to
+		// the (u,v) coordinates of the triangle:
+		// x = x0 + e1x * u + e2x * v
+		// y = y0 + e1y * u + e2y * v
+		// where 0 <= u && 0 <= v && u + v <= 1.
+		//
+		// We integrate u from [0,1-v] and then v from [0,1].
+		// We also need to use the Jacobian of the transformation:
+		// D = cross(e1, e2)
+		//
+		// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
+		//
+		// The rest of the derivation is handled by computer algebra.
+
+		check(vertices.size >= 3) { vertices.size }
+
+		var center = Vector2d.ZERO
+		var area = 0.0
+		var I = 0.0
+
+		// Get a reference point for forming triangles.
+		// Use the first vertex to reduce round-off errors.
+		var s = vertices[0]
+
+		for (i in 0 until vertices.size) {
+			// Triangle vertices.
+			val e1 = vertices[i] - s;
+			val e2 = if (i + 1 < vertices.size) vertices[i+1] - s else vertices[0] - s
+
+			val D = b2Cross(e1, e2)
+
+			val triangleArea = 0.5f * D;
+			area += triangleArea;
+
+			// Area weighted centroid
+			center += triangleArea * inv3 * (e1 + e2);
+
+			val ex1 = e1.x
+			val ey1 = e1.y
+			val ex2 = e2.x
+			val ey2 = e2.y
+
+			val intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
+			val inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
+
+			I += (0.25f * inv3 * D) * (intx2 + inty2);
+		}
+
+		check(area > b2_epsilon) { "Area is too small: $area" }
+
+		center *= 1.0 / area
+		val center2 = center + s
+		val mass = density * area
+
+		return MassData(
+			// Total mass
+			mass = mass,
+			// Center of mass
+			center = center2,
+			// Inertia tensor relative to the local origin (point s).
+			// Shift to center of mass then to original body origin.
+			inertia = density * I + mass * (b2Dot(center2, center2) - b2Dot(center, center)),
+		)
+	}
+
+	/**
+	 * Validate convexity. This is a very time consuming operation.
+	 * @returns true if valid
+	 */
+	fun validate(doThrow: Boolean = false): Boolean {
+		for (i in 0 until vertices.size) {
+			val i1 = i
+			val i2 = if (i < vertices.size - 1) i1 + 1 else 0
+			val p = vertices[i1]
+			val e = vertices[i2] - p
+
+			if (!p.isFinite) {
+				if (doThrow) {
+					throw IllegalStateException("Vertex at $i1 is not finite")
+				}
+
+				return false
+			}
+
+			if (!e.isFinite) {
+				if (doThrow) {
+					throw IllegalStateException("Vertex at $i2 is not finite")
+				}
+
+				return false
+			}
+
+			for (j in 0 until vertices.size) {
+				if (j == i1 || j == i2) {
+					continue
+				}
+
+				val v = vertices[j] - p
+				val c = b2Cross(e, v)
+
+				if (c < 0.0) {
+					if (doThrow) {
+						throw IllegalStateException("Vertex at $j form concave shape")
+					}
+
+					return false
+				}
+			}
+		}
+
+		return true
+	}
+
+	internal var centroid = Vector2d.ZERO
+	internal val vertices = ArrayList<Vector2d>()
+	internal val normals = ArrayList<Vector2d>()
+
+	val count: Int get() = vertices.size
+
+	override var radius: Double = b2_polygonRadius
+		set(value) {
+			if (value == b2_polygonRadius) {
+				field = value
+				return
+			}
+
+			throw UnsupportedOperationException("For polygonal shapes this must be b2_polygonRadius")
+		}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/B2World.kt

@@ -0,0 +1,916 @@
+package ru.dbotthepony.kbox2d.dynamics
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.collision.DistanceProxy
+import ru.dbotthepony.kbox2d.collision.b2TimeOfImpact
+import ru.dbotthepony.kbox2d.collision.shapes.ChainShape
+import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
+import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+import ru.dbotthepony.kbox2d.dynamics.contact.AbstractContact
+import ru.dbotthepony.kbox2d.dynamics.internal.Island
+import ru.dbotthepony.kbox2d.dynamics.joint.AbstractJoint
+import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kstarbound.util.Color
+
+class B2World(override var gravity: Vector2d) : IB2World {
+	override var bodyCount: Int = 0
+		private set
+	override var jointCount: Int = 0
+		private set
+
+	override val contactManager: IContactManager = ContactManager()
+
+	override var destructionListener: IDestructionListener? = null
+	override var contactFilter: IContactFilter? by contactManager::contactFilter
+	override var contactListener: IContactListener? by contactManager::contactListener
+
+	override var debugDraw: IDebugDraw? = null
+
+	override var bodyList: IBody? = null
+		private set
+	override var jointList: IJoint? = null
+		private set
+
+	override var warmStarting: Boolean = true
+	override var continuousPhysics: Boolean = true
+	override var enableSubStepping: Boolean = false
+
+	override var allowAutoSleep: Boolean = true
+		set(value) {
+			if (value == field)
+				return
+
+			field = value
+
+			if (!value) {
+				for (body in bodyListIterator) {
+					body.isAwake = true
+				}
+			}
+		}
+
+	override var isLocked: Boolean = false
+		private set
+
+	override var autoClearForces: Boolean = true
+
+	private var stepComplete = true
+	private var newContacts = false
+
+	private val profile = ProfileData()
+
+	override fun notifyNewContacts() {
+		newContacts = true
+	}
+
+	override fun createBody(bodyDef: BodyDef): IBody {
+		if (isLocked)
+			throw ConcurrentModificationException()
+
+		val body = Body(bodyDef, this)
+
+		body.next = bodyList
+		(bodyList as Body?)?.prev = body
+		bodyList = body
+		bodyCount++
+
+		return body
+	}
+
+	override fun destroyBody(body: IBody) {
+		if (isLocked)
+			throw ConcurrentModificationException()
+
+		check(body.world == this) { "$body does not belong to $this" }
+		check(bodyCount > 0) { "I have ${bodyCount} bodies, can't remove one" }
+
+		// Delete the attached joints.
+		for (jointEdge in body.jointIterator) {
+			destructionListener?.sayGoodbye(jointEdge.joint)
+			destroyJoint(jointEdge.joint)
+		}
+
+		// Delete the attached contacts.
+		for (contactEdge in body.contactEdgeIterator) {
+			contactManager.destroy(contactEdge.contact)
+		}
+
+		// Delete the attached fixtures. This destroys broad-phase proxies.
+		for (fixture in body.fixtureIterator) {
+			destructionListener?.sayGoodbye(fixture)
+			(fixture as Fixture).destroyProxies(contactManager.broadPhase)
+		}
+
+		// Remove world body list.
+		val prev = body.prev as Body?
+		val next = body.next as Body?
+
+		prev?.next = next
+		next?.prev = prev
+
+		if (body == bodyList) {
+			bodyList = next ?: prev
+		}
+
+		bodyCount--
+		(body as Body).unlink()
+	}
+
+	override fun createJoint(jointDef: IJointDef): AbstractJoint {
+		if (isLocked)
+			throw ConcurrentModificationException()
+
+		val joint = AbstractJoint.create(jointDef.type, jointDef)
+
+		// Connect to the world list.
+		joint.next = jointList
+		(jointList as AbstractJoint?)?.prev = joint
+		jointList = joint
+		jointCount++
+
+		if (joint.hasTwoBodies) {
+			val bodyA = joint.bodyA
+			val bodyB = joint.bodyB
+
+			// If the joint prevents collisions, then flag any contacts for filtering.
+			if (!joint.collideConnected) {
+				for (edge in bodyB.contactEdgeIterator) {
+					if (edge.other == bodyA) {
+						edge.contact.flagForFiltering()
+					}
+				}
+			}
+		}
+
+		// Note: creating a joint doesn't wake the bodies.
+
+		return joint
+	}
+
+	override fun destroyJoint(joint: IJoint) {
+		if (isLocked)
+			throw ConcurrentModificationException()
+
+		check(jointCount > 0) { "No joints tracked to remove" }
+		joint as AbstractJoint
+		require((joint.nullableBodyA?.world == this || joint.nullableBodyA == null) && (joint.nullableBodyB?.world == this || joint.nullableBodyB == null)) { "$joint does not belong to $this" }
+
+		if (!joint.isValid) {
+			throw IllegalStateException("Joint $joint is already destroyed")
+		}
+
+		// Remove from the doubly linked list.
+		this.run {
+			val prev = joint.prev as AbstractJoint?
+			val next = joint.next as AbstractJoint?
+
+			prev?.next = next
+			next?.prev = prev
+
+			if (joint == this.jointList) {
+				this.jointList = next ?: prev
+			}
+		}
+
+		// Disconnect from island graph.
+		val bodyA = joint.nullableBodyA
+		val bodyB = joint.nullableBodyB
+
+		// Wake up connected bodies.
+		bodyA?.isAwake = true
+		bodyB?.isAwake = true
+
+		// Remove from body 1.
+		this.run {
+			val edgeA = joint.edgeA
+
+			val prev = edgeA.prev
+			val next = edgeA.next
+
+			prev?.next = next
+			next?.prev = prev
+
+			if (bodyA?.jointList == edgeA) {
+				bodyA.jointList = next ?: prev
+			}
+		}
+
+		// Remove from body 2
+		this.run {
+			val edgeB = joint.edgeB
+
+			val prev = edgeB.prev
+			val next = edgeB.next
+
+			prev?.next = next
+			next?.prev = prev
+
+			if (bodyB?.jointList == edgeB) {
+				bodyB.jointList = next ?: prev
+			}
+		}
+
+		jointCount--
+		joint.unlink()
+
+		if (!joint.collideConnected && bodyB != null && bodyA != null) {
+			for (edge in bodyB.contactEdgeIterator) {
+				if (edge.other == bodyA) {
+					edge.contact.flagForFiltering()
+				}
+			}
+		}
+	}
+
+	private val _profile = ProfileData()
+
+	/**
+	 * Find islands, integrate and solve constraints, solve position constraints
+	 */
+	internal fun solve(step: B2TimeStep) {
+		_profile.solveInit = 0L
+		_profile.solveVelocity = 0L
+		_profile.solvePosition = 0L
+
+		// Size the island for the worst case.
+		// TODO: Kotlin: or do we size it??
+		val island = Island(listener = contactListener)
+
+		// Clear all the island flags.
+		for (body in bodyListIterator) {
+			body as Body
+			body.isOnIsland = false
+		}
+
+		for (contact in contactListIterator) {
+			contact as AbstractContact
+			contact.isOnIsland = false
+		}
+
+		for (joint in jointListIterator) {
+			joint as AbstractJoint
+			check(joint.isValid) { "$joint is no longer valid, but present in linked list" }
+			joint.isOnIsland = false
+		}
+
+		// Build and simulate all awake islands.
+		for (seed in bodyListIterator) {
+			seed as Body
+
+			if (seed.type == BodyType.STATIC || !seed.isAwake || !seed.isEnabled || seed.isOnIsland) {
+				continue
+			}
+
+			// Reset island and stack.
+			island.clear()
+			val stack = ArrayDeque<Body>(32)
+			stack.add(seed)
+			seed.isOnIsland = true
+
+			// Perform a depth first search (DFS) on the constraint graph.
+			while (stack.isNotEmpty()) {
+				// Grab the next body off the stack and add it to the island.
+				val body = stack.removeLast()
+				check(body.isEnabled)
+				island.add(body)
+
+				// To keep islands as small as possible, we don't
+				// propagate islands across static bodies.
+				if (body.type == BodyType.STATIC)
+					continue
+
+				// Make sure the body is awake (without resetting sleep timer).
+				body.flags = BodyFlags.AWAKE.or(body.flags)
+
+				// Search all contacts connected to this body.
+				for (ce in body.contactEdgeIterator) {
+					val contact = ce.contact as AbstractContact
+
+					// Has this contact already been added to an island?
+					if (contact.isOnIsland)
+						continue
+
+					// Is this contact solid and touching?
+					if (!contact.isEnabled || !contact.isTouching)
+						continue
+
+					// Skip sensors.
+					if (contact.fixtureA.isSensor || contact.fixtureB.isSensor)
+						continue
+
+					island.add(contact)
+					contact.isOnIsland = true
+
+					val other = ce.other as Body
+
+					// Was the other body already added to this island?
+					if (!other.isOnIsland) {
+						stack.add(other)
+						other.isOnIsland = true
+					}
+				}
+
+				// Search all joints connect to this body.
+				for (je in body.jointIterator) {
+					val joint = je.joint as AbstractJoint
+					check(joint.isValid) { "$joint is no longer valid, but present in linked list of $body" }
+
+					if (joint.isOnIsland)
+						continue
+
+					val other = je.otherNullable as Body?
+
+					// Don't simulate joints connected to disabled bodies.
+					if (other != null && !other.isEnabled)
+						continue
+
+					island.add(joint)
+					joint.isOnIsland = true
+
+					if (other != null && !other.isOnIsland) {
+						stack.add(other)
+						other.isOnIsland = true
+					}
+				}
+			}
+
+			val profile = ProfileData()
+			island.solve(profile, step, gravity, allowAutoSleep)
+			this.profile.solveInit += profile.solveInit
+			this.profile.solveVelocity += profile.solveVelocity
+			this.profile.solvePosition += profile.solvePosition
+			this.profile.integratePositions += profile.integratePositions
+
+			// Post solve cleanup.
+			for (body in island.bodiesAccess) {
+				// Allow static bodies to participate in other islands.
+				if (body.type == BodyType.STATIC) {
+					body.isOnIsland = false
+				}
+			}
+		}
+
+		val timer = System.nanoTime()
+
+		// Synchronize fixtures, check for out of range bodies.
+		for (body in bodyListIterator) {
+			body as Body
+
+			// If a body was not in an island then it did not move.
+			if (!body.isOnIsland || body.type == BodyType.STATIC) {
+				continue
+			}
+
+			// Update fixtures (for broad-phase).
+			body.synchronizeFixtures()
+		}
+
+		// Look for new contacts.
+		contactManager.findNewContacts()
+		profile.broadphase = System.nanoTime() - timer
+	}
+
+	/**
+	 * Find TOI contacts and solve them.
+	 */
+	fun solveTOI(step: B2TimeStep) {
+		val island = Island(listener = contactListener)
+
+		if (stepComplete) {
+			for (body in bodyListIterator) {
+				body as Body
+				body.isOnIsland = false
+				body.sweep.alpha0 = 0.0
+			}
+
+			for (c in contactManager.contactListIterator) {
+				// Invalidate TOI
+				c as AbstractContact
+				c.isOnIsland = false
+				c.toiFlag = false
+				c.toiCount = 0
+				c.toi = 1.0
+			}
+		}
+
+		// Find TOI events and solve them.
+		while (true) {
+			var minContact: AbstractContact? = null
+			var minAlpha = 1.0
+
+			for (c in contactManager.contactListIterator) {
+				if (!c.isEnabled) {
+					continue
+				}
+
+				c as AbstractContact
+
+				if (c.toiCount > b2_maxSubSteps) {
+					continue
+				}
+
+				var alpha = 1.0
+
+				if (c.toiFlag) {
+					// This contact has a valid cached TOI.
+					alpha = c.toi
+				} else {
+					val fA = c.fixtureA
+					val fB = c.fixtureB
+
+					// Is there a sensor?
+					if (fA.isSensor || fB.isSensor) {
+						continue
+					}
+
+					val bA = fA.body as Body
+					val bB = fB.body as Body
+
+					val typeA = bA.type
+					val typeB = bB.type
+
+					check(typeA == BodyType.DYNAMIC || typeB == BodyType.DYNAMIC)
+
+					val activeA = bA.isAwake && typeA != BodyType.STATIC
+					val activeB = bB.isAwake && typeB != BodyType.STATIC
+
+					// Is at least one body active (awake and dynamic or kinematic)?
+					if (!activeA && !activeB) {
+						continue
+					}
+
+					val collideA = bA.isBullet || typeA != BodyType.DYNAMIC
+					val collideB = bB.isBullet || typeB != BodyType.DYNAMIC
+
+					// Are these two non-bullet dynamic bodies?
+					if (!collideA && !collideB) {
+						continue
+					}
+
+					// Compute the TOI for this contact.
+					// Put the sweeps onto the same time interval.
+					var alpha0 = bA.sweep.alpha0
+
+					if (bA.sweep.alpha0 < bB.sweep.alpha0) {
+						alpha0 = bB.sweep.alpha0
+						bA.sweep.advance(alpha0)
+					} else if (bA.sweep.alpha0 > bB.sweep.alpha0) {
+						alpha0 = bA.sweep.alpha0
+						bB.sweep.advance(alpha0)
+					}
+
+					check(alpha0 < 1.0) { alpha0 }
+
+					val indexA = c.childIndexA
+					val indexB = c.childIndexB
+
+					// Compute the time of impact in interval [0, minTOI]
+					val output = b2TimeOfImpact(
+						proxyA = DistanceProxy(fA.shape, indexA),
+						proxyB = DistanceProxy(fB.shape, indexB),
+						_sweepA = bA.sweep,
+						_sweepB = bB.sweep,
+						tMax = 1.0
+					)
+
+					// Beta is the fraction of the remaining portion of the .
+					val beta = output.t
+
+					if (output.state == TOIOutput.State.TOUCHING) {
+						alpha = 1.0.coerceAtMost(alpha0 + (1.0f - alpha0) * beta)
+					} else {
+						alpha = 1.0
+					}
+
+					c.toi = alpha
+					c.toiFlag = true
+				}
+
+				if (alpha < minAlpha) {
+					// This is the minimum TOI found so far.
+					minContact = c
+					minAlpha = alpha
+				}
+			}
+
+			if (minContact == null || 1.0 - 10.0 * b2_epsilon < minAlpha) {
+				// No more TOI events. Done!
+				stepComplete = true
+				break
+			}
+
+			// Advance the bodies to the TOI.
+			val fA = minContact.fixtureA
+			val fB = minContact.fixtureB
+			val bA = fA.body as Body
+			val bB = fB.body as Body
+
+			val backup1 = bA.sweep.copy()
+			val backup2 = bB.sweep.copy()
+
+			bA.advance(minAlpha)
+			bB.advance(minAlpha)
+
+			// The TOI contact likely has some new contact points.
+			minContact.update(contactManager.contactListener)
+			minContact.toiFlag = false
+			minContact.toiCount++
+
+			// Is the contact solid?
+			if (!minContact.isEnabled || !minContact.isTouching) {
+				// Restore the sweeps.
+				minContact.isEnabled = false
+				bA.sweep.load(backup1)
+				bB.sweep.load(backup2)
+				bA.synchronizeTransform()
+				bB.synchronizeTransform()
+				continue
+			}
+
+			bA.isAwake = true
+			bB.isAwake = true
+
+			// Build the island
+			island.clear()
+			island.add(bA)
+			island.add(bB)
+			island.add(minContact)
+
+			bA.isOnIsland = true
+			bB.isOnIsland = true
+			minContact.isOnIsland = true
+
+			// Get contacts on bodyA and bodyB.
+			val bodies = arrayOf(bA, bB)
+			for (i in 0 .. 1) {
+				val body = bodies[i]
+
+				if (body.type == BodyType.DYNAMIC) {
+					for (ce in body.contactEdgeIterator) {
+						val contact = ce.contact as AbstractContact
+
+						// Has this contact already been added to the island?
+						if (contact.isOnIsland) {
+							continue
+						}
+
+						// Only add static, kinematic, or bullet bodies.
+						val other = ce.other
+
+						if (other.type == BodyType.DYNAMIC && !body.isBullet && !other.isBullet) {
+							continue
+						}
+
+						// Skip sensors.
+						if (contact.fixtureA.isSensor || contact.fixtureB.isSensor) {
+							continue
+						}
+
+						other as Body
+
+						// Tentatively advance the body to the TOI.
+						val backup = other.sweep
+						if (!other.isOnIsland) {
+							other.advance(minAlpha)
+						}
+
+						// Update the contact points
+						contact.update(contactManager.contactListener)
+
+						// Was the contact disabled by the user?
+						if (!contact.isEnabled) {
+							other.sweep.load(backup)
+							other.synchronizeTransform()
+							continue
+						}
+
+						// Are there contact points?
+						if (!contact.isTouching) {
+							other.sweep.load(backup)
+							other.synchronizeTransform()
+							continue
+						}
+
+						// Add the contact to the island
+						contact.isOnIsland = true
+						island.add(contact)
+
+						// Has the other body already been added to the island?
+						if (other.isOnIsland) {
+							continue
+						}
+
+						// Add the other body to the island.
+						other.isOnIsland = true
+
+						if (other.type != BodyType.STATIC) {
+							other.isAwake = true
+						}
+
+						island.add(other)
+					}
+				}
+			}
+
+			val subStepDT = (1.0 - minAlpha) * step.dt
+
+			val subStep = B2TimeStep(
+				dt = subStepDT,
+				inv_dt = 1.0 / subStepDT,
+				dtRatio = 1.0,
+				positionIterations = 20,
+				velocityIterations = step.velocityIterations,
+				warmStarting = false,
+			)
+
+			island.solveTOI(subStep, bA.islandIndex, bB.islandIndex)
+
+			// Reset island flags and synchronize broad-phase proxies.
+			for (body in island.bodiesAccess) {
+				body.isOnIsland = false
+				if (body.type != BodyType.DYNAMIC) {
+					continue
+				}
+
+				body.synchronizeFixtures()
+
+				// Invalidate all contact TOIs on this displaced body.
+				for (ce in body.contactEdgeIterator) {
+					val contact = ce.contact as AbstractContact
+					contact.toiFlag = false
+					contact.isOnIsland = false
+				}
+			}
+
+			// Commit fixture proxy movements to the broad-phase so that new contacts are created.
+			// Also, some contacts can be destroyed.
+			contactManager.findNewContacts()
+
+			if (enableSubStepping) {
+				stepComplete = false
+				break
+			}
+		}
+	}
+
+	private var m_inv_dt0 = 0.0
+
+	override fun step(dt: Double, velocityIterations: Int, positionIterations: Int) {
+		var stepTimer = System.nanoTime()
+
+		// If new fixtures were added, we need to find the new contacts.
+		if (newContacts) {
+			contactManager.findNewContacts()
+			newContacts = false
+		}
+
+		isLocked = true
+
+		try {
+			val inv_dt: Double
+
+			if (dt > 0.0) {
+				inv_dt = 1.0 / dt
+			} else {
+				inv_dt = 0.0
+			}
+
+			val step = B2TimeStep(
+				dt = dt,
+				inv_dt = inv_dt,
+				dtRatio = m_inv_dt0 * dt,
+				warmStarting = warmStarting,
+				velocityIterations = velocityIterations,
+				positionIterations = positionIterations
+			)
+
+			// Update contacts. This is where some contacts are destroyed.
+			this.run {
+				val timer = System.nanoTime()
+				this.contactManager.collide()
+				this.profile.collide = System.nanoTime() - timer
+			}
+
+			// Integrate velocities, solve velocity constraints, and integrate positions.
+			if (stepComplete && dt > 0.0) {
+				val timer = System.nanoTime()
+				solve(step)
+				profile.solve = System.nanoTime() - timer
+			}
+
+			// Handle TOI events.
+			if (continuousPhysics && dt > 0.0) {
+				val timer = System.nanoTime()
+				solveTOI(step)
+				profile.solveTOI = System.nanoTime() - timer
+			}
+
+			if (dt > 0.0) {
+				m_inv_dt0 = step.inv_dt
+			}
+
+			if (autoClearForces) {
+				clearForces()
+			}
+		} catch(err: Throwable) {
+			throw RuntimeException("Caught an exception simulating physics world", err)
+		} finally {
+			isLocked = false
+		}
+
+		profile.step = System.nanoTime() - stepTimer
+	}
+
+	override fun clearForces() {
+		for (body in bodyListIterator) {
+			body as Body
+			body.force = Vector2d.ZERO
+			body.torque = 0.0
+		}
+	}
+
+	override fun queryAABB(aabb: AABB, callback: IQueryCallback) {
+		contactManager.broadPhase.query(aabb) { nodeId, userData -> callback.reportFixture((userData as FixtureProxy).fixture) }
+	}
+
+	override fun rayCast(point1: Vector2d, point2: Vector2d, callback: IRayCastCallback) {
+		val input = RayCastInput(point1, point2, 1.0)
+
+		contactManager.broadPhase.rayCast(input, object : ProxyRayCastCallback {
+			override fun invoke(subInput: RayCastInput, nodeId: Int, userData: Any?): Double {
+				val proxy = userData as FixtureProxy
+				val fixture = proxy.fixture
+				val index = proxy.childIndex
+				val output = fixture.rayCast(subInput, index)
+
+				if (output.hit) {
+					val point = (1.0 - output.fraction) * subInput.p1 + output.fraction * subInput.p2
+					return callback.reportFixture(fixture, point, output.normal, output.fraction)
+				}
+
+				return subInput.maxFraction
+			}
+		})
+	}
+
+	private fun drawShape(fixture: IFixture, xf: Transform, color: Color) {
+		when (fixture.type) {
+			IShape.Type.CIRCLE -> {
+				val circle = fixture.shape as CircleShape
+				val center = b2Mul(xf, circle.p)
+				val radius = circle.radius
+				val axis = b2Mul(xf.q, Vector2d.RIGHT)
+
+				debugDraw?.drawSolidCircle(center, radius, axis, color)
+			}
+
+			IShape.Type.EDGE -> {
+				val edge = fixture.shape as EdgeShape
+				val v1 = b2Mul(xf, edge.vertex1)
+				val v2 = b2Mul(xf, edge.vertex2)
+
+				debugDraw?.drawSegment(v1, v2, color)
+
+				if (!edge.oneSided) {
+					debugDraw?.drawPoint(v1, 4.0, color)
+					debugDraw?.drawPoint(v2, 4.0, color)
+				}
+			}
+
+			IShape.Type.POLYGON -> {
+				val poly = fixture.shape as PolygonShape
+				val vertices = poly.vertices.map { b2Mul(xf, it) }
+				debugDraw?.drawSolidPolygon(vertices, color)
+			}
+
+			IShape.Type.CHAIN -> {
+				val chain = fixture.shape as ChainShape
+				var v1 = b2Mul(xf, chain.vertices[0])
+
+				for (i in 1 until chain.vertices.size) {
+					val v2 = b2Mul(xf, chain.vertices[i])
+					debugDraw?.drawSegment(v1, v2, color)
+					v1 = v2
+				}
+			}
+		}
+	}
+
+	override fun debugDraw() {
+		val debugDraw = debugDraw ?: return
+
+		if (debugDraw.drawShapes) {
+			for (body in bodyListIterator) {
+				val xf = body.transform
+
+				for (f in body.fixtureIterator) {
+					if (body.type == BodyType.DYNAMIC && body.mass == 0.0) {
+						// Bad body
+						drawShape(f, xf, BAD_BODY_COLOR)
+					} else if (!body.isEnabled) {
+						drawShape(f, xf, DISABLED_BODY_COLOR)
+					} else if (body.type == BodyType.STATIC) {
+						drawShape(f, xf, STATIC_BODY_COLOR)
+					} else if (body.type == BodyType.KINEMATIC) {
+						drawShape(f, xf, KINEMATIC_BODY_COLOR)
+					} else if (!body.isAwake) {
+						drawShape(f, xf, SLEEPING_BODY_COLOR)
+					} else {
+						drawShape(f, xf, NORMAL_BODY_COLOR)
+					}
+				}
+			}
+		}
+
+		if (debugDraw.drawJoints) {
+			for (joint in jointListIterator) {
+				joint.draw(debugDraw)
+			}
+		}
+
+		if (debugDraw.drawPairs) {
+			for (c in contactManager.contactListIterator) {
+				val fixtureA = c.fixtureA
+				val fixtureB = c.fixtureB
+				val indexA = c.childIndexA
+				val indexB = c.childIndexB
+				val cA = fixtureA.getAABB(indexA).centre
+				val cB = fixtureB.getAABB(indexB).centre
+
+				debugDraw.drawSegment(cA, cB, PAIR_COLOR)
+			}
+		}
+
+		if (debugDraw.drawAABB) {
+			for (body in bodyListIterator) {
+				if (!body.isEnabled) {
+					continue
+				}
+
+				for (f in body.fixtureIterator) {
+					for (proxy in f.proxies) {
+						val aabb = contactManager.broadPhase.getFatAABB(proxy.proxyId)
+
+						debugDraw.drawPolygon(listOf(
+							aabb.A, aabb.B, aabb.C, aabb.D
+						), AABB_COLOR
+						)
+					}
+				}
+			}
+		}
+
+		if (debugDraw.drawCenterOfMess) {
+			for (body in bodyListIterator) {
+				val xf = Transform(body.transform.position, body.transform.rotation.angle)
+				xf.p = body.worldCenter
+				debugDraw.drawTransform(xf)
+			}
+		}
+	}
+
+	companion object {
+		private val BAD_BODY_COLOR = Color(1f, 0f, 0f)
+		private val DISABLED_BODY_COLOR = Color(0.5f, 0.5f, 0.3f)
+		private val STATIC_BODY_COLOR = Color(0.5f, 0.9f, 0.5f)
+		private val KINEMATIC_BODY_COLOR = Color(0.5f, 0.5f, 0.9f)
+		private val SLEEPING_BODY_COLOR = Color(0.6f, 0.6f, 0.6f)
+		private val NORMAL_BODY_COLOR = Color(0.9f, 0.7f, 0.7f)
+		private val PAIR_COLOR = Color(0.3f, 0.9f, 0.9f)
+		private val AABB_COLOR = Color(0.9f, 0.3f, 0.9f)
+	}
+
+	override fun shiftOrigin(newOrigin: Vector2d) {
+		if (isLocked)
+			throw ConcurrentModificationException()
+
+		isLocked = true
+
+		try {
+			for (body in bodyListIterator) {
+				body as Body
+				body.transform.p -= newOrigin
+				body.sweep.c0 -= newOrigin
+				body.sweep.c -= newOrigin
+			}
+
+			for (joint in jointListIterator) {
+				joint.shiftOrigin(newOrigin)
+			}
+
+			contactManager.broadPhase.shiftOrigin(newOrigin)
+		} finally {
+			isLocked = false
+		}
+	}
+
+	override val profileData: IProfileData
+		get() = profile.snapshot()
+
+	override fun dump() {
+		TODO("Not yet implemented")
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/Body.kt

@@ -0,0 +1,622 @@
+package ru.dbotthepony.kbox2d.dynamics
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kstarbound.math.Vector2d
+
+open class Body(def: BodyDef, world: IB2World) : IBody {
+	private var _world: IB2World? = world
+
+	final override val world: IB2World
+		get() = _world ?: throw IllegalStateException("Tried to use removed body")
+
+	internal var flags: Int = 0
+	internal var isOnIsland = false
+	internal var islandIndex: Int = 0
+
+	internal fun unlink() {
+		_world = null
+	}
+
+	init {
+		def.validate()
+
+		if (def.bullet) {
+			flags = BodyFlags.BULLET.or(flags)
+		}
+
+		if (def.fixedRotation) {
+			flags = BodyFlags.FIXED_ROTATION.or(flags)
+		}
+
+		if (def.allowSleep) {
+			flags = BodyFlags.AUTO_SLEEP.or(flags)
+		}
+
+		if (def.awake && def.type != BodyType.STATIC) {
+			flags = BodyFlags.AWAKE.or(flags)
+		}
+
+		if (def.enabled) {
+			flags = BodyFlags.ENABLED.or(flags)
+		}
+	}
+
+	internal val sweep: Sweep = Sweep()
+
+	final override val transform: Transform = Transform(def.position, def.angle)
+	val xf by this::transform
+
+	override val position: Vector2d
+		get() = transform.position
+	override val angle: Double
+		get() = sweep.a
+
+	override val userData: Any? = def.userData
+
+	internal var sleepTime: Double = 0.0
+	internal var torque: Double = 0.0
+	internal var force: Vector2d = Vector2d.ZERO
+
+	override var linearVelocity: Vector2d = def.linearVelocity
+		set(value) {
+			if (type == BodyType.STATIC)
+				return
+
+			if (value.dotProduct(value) > 0.0)
+				isAwake = true
+
+			field = value
+		}
+
+	override var angularVelocity: Double = def.angularVelocity
+		set(value) {
+			if (type == BodyType.STATIC)
+				return
+
+			if (value * value > 0.0)
+				isAwake = true
+
+			field = value
+		}
+
+	override var fixtureList: Fixture? = null
+		protected set
+
+	protected var fixtureCount: Int = 0
+
+	override var jointList: JointEdge? = null
+		internal set
+	override var contactEdge: ContactEdge? = null
+		internal set
+
+	override var next: IBody? = null
+		internal set
+	override var prev: IBody? = null
+		internal set
+
+	override var linearDamping: Double = def.linearDamping
+	override var angularDamping: Double = def.angularDamping
+	override var gravityScale: Double = def.gravityScale
+
+	override var mass: Double = 0.0
+		protected set
+
+	internal var invMass: Double = 0.0
+
+	/**
+	 * Rotational inertia about the center of mass.
+ 	 */
+	internal var rotInertia: Double = 0.0
+
+	/**
+	 * Rotational inertia about the center of mass.
+	 */
+	internal var rotInertiaInv: Double = 0.0
+
+	internal var I by this::rotInertia
+	internal var invI by this::rotInertiaInv
+
+	override var isBullet: Boolean
+		get() = BodyFlags.BULLET.isit(flags)
+		set(value) { flags = BodyFlags.BULLET.update(flags, value) }
+
+	override var isAwake: Boolean
+		get() = BodyFlags.AWAKE.isit(flags)
+		set(value) {
+			if (type == BodyType.STATIC || BodyFlags.AWAKE.isit(flags) == value)
+				return
+
+			if (value) {
+				flags = flags or BodyFlags.AWAKE.bitmask
+				sleepTime = 0.0
+			} else {
+				flags = flags and BodyFlags.AWAKE.bitmask.inv()
+				sleepTime = 0.0
+				linearVelocity = Vector2d.ZERO
+				angularVelocity = 0.0
+				force = Vector2d.ZERO
+				torque = 0.0
+			}
+		}
+
+	override var isEnabled: Boolean
+		get() = BodyFlags.ENABLED.isit(flags)
+		set(value) {
+			if (world.isLocked)
+				throw ConcurrentModificationException()
+
+			if (value == isEnabled)
+				return
+
+			if (value) {
+				flags = BodyFlags.ENABLED.not(flags)
+
+				// Create all proxies.
+				val broadPhase = world.contactManager.broadPhase
+
+				for (fixture in fixtureIterator) {
+					(fixture as Fixture).createProxies(broadPhase, transform)
+				}
+
+				world.notifyNewContacts()
+			} else {
+				flags = BodyFlags.ENABLED.or(flags)
+
+				// Destroy all proxies.
+				val broadPhase = world.contactManager.broadPhase
+
+				for (fixture in fixtureIterator) {
+					(fixture as Fixture).destroyProxies(broadPhase)
+				}
+
+				// Destroy the attached contacts.
+				for (edge in contactEdgeIterator) {
+					world.contactManager.destroy(edge.contact)
+				}
+
+				contactEdge = null
+			}
+		}
+
+	override var isFixedRotation: Boolean
+		get() = BodyFlags.FIXED_ROTATION.isit(flags)
+		set(value) {
+			if (value == isFixedRotation)
+				return
+
+			flags = BodyFlags.FIXED_ROTATION.update(flags, value)
+
+			angularVelocity = 0.0
+			resetMassData()
+		}
+
+	override var allowAutoSleep: Boolean
+		get() = BodyFlags.AUTO_SLEEP.isit(flags)
+		set(value) {
+			flags = BodyFlags.AUTO_SLEEP.update(flags, value)
+
+			if (!value) {
+				isAwake = true
+			}
+		}
+
+	override var type: BodyType = def.type
+		set(value) {
+			if (world.isLocked)
+				throw ConcurrentModificationException()
+
+			field = value
+
+			resetMassData()
+
+			if (value == BodyType.STATIC) {
+				linearVelocity = Vector2d.ZERO
+				angularVelocity = 0.0
+				sweep.a0 = sweep.a
+				sweep.c0 = sweep.c
+				flags = BodyFlags.AWAKE.not(flags)
+				synchronizeFixtures()
+			}
+
+			isAwake = true
+
+			force = Vector2d.ZERO
+			torque = 0.0
+
+			var edge = contactEdge
+
+			while (edge != null) {
+				// TODO: проверить, что делает destroy
+				world.contactManager.destroy(edge.contact)
+				edge = edge.next
+			}
+
+			contactEdge = null
+
+			val broadPhase = world.contactManager.broadPhase
+			var f: IFixture? = fixtureList
+
+			while (f != null) {
+				for (proxy in f.proxies) {
+					broadPhase.touchProxy(proxy.proxyId)
+				}
+
+				f = f.next
+			}
+		}
+
+	override fun createFixture(def: FixtureDef): IFixture {
+		if (world.isLocked)
+			throw ConcurrentModificationException()
+
+		val fixture = Fixture(this, def)
+
+		if (isEnabled) {
+			fixture.createProxies(world.contactManager.broadPhase, transform)
+		}
+
+		fixture.next = fixtureList
+		fixtureList = fixture
+		fixtureCount++
+
+		// Adjust mass properties if needed.
+		if (fixture.density > 0.0) {
+			resetMassData()
+		}
+
+		// Let the world know we have a new fixture. This will cause new contacts
+		// to be created at the beginning of the next time step.
+		world.notifyNewContacts()
+
+		return fixture
+	}
+
+	override fun destroyFixture(fixture: IFixture) {
+		if (world.isLocked)
+			throw ConcurrentModificationException()
+
+		require(fixture.body == this) { "$fixture does not belong to $this (belongs to ${fixture.body})" }
+		check(fixtureCount > 0) { "Having no tracked fixtures, but $fixture belongs to us" }
+
+		var node: IFixture? = fixtureList
+		var found = false
+		var previous: IFixture? = null
+
+		while (node != null) {
+			if (node == fixture) {
+				// TODO: Это должно работать
+				(previous as Fixture?)?.next = node.next
+				found = true
+				break
+			} else {
+				previous = node
+				node = node.next
+			}
+		}
+
+		check(found) { "Can't find $fixture in linked list of fixtures" }
+
+		val density = node!!.density
+
+		// Destroy any contacts associated with the fixture.
+		var edge = contactEdge
+
+		while (edge != null) {
+			val contact = edge.contact
+			edge = edge.next
+
+			val fixtureA = contact.fixtureA
+			val fixtureB = contact.fixtureB
+
+			if (fixtureA == fixture || fixtureB == fixture) {
+				// This destroys the contact and removes it from
+				// this body's contact list.
+				world.contactManager.destroy(contact)
+			}
+		}
+
+		fixture as Fixture
+
+		if (isEnabled) {
+			fixture.destroyProxies(world.contactManager.broadPhase)
+		}
+
+		fixture.unlink()
+		fixtureCount--
+
+		// Reset the mass data
+		if (density > 0.0) {
+			resetMassData()
+		}
+	}
+
+	override fun resetMassData() {
+		// Compute mass data from shapes. Each shape has its own density.
+		mass = 0.0
+		invMass = 0.0
+		rotInertia = 0.0
+		rotInertiaInv = 0.0
+		sweep.localCenter = Vector2d.ZERO
+
+		if (type == BodyType.STATIC || type == BodyType.KINEMATIC) {
+			sweep.c0 = transform.position
+			sweep.c = transform.position
+			sweep.a0 = sweep.a
+			return
+		}
+
+		check(type == BodyType.DYNAMIC)
+
+		// Accumulate mass over all fixtures.
+		var localCenter = Vector2d.ZERO
+
+		for (fixture in fixtureIterator) {
+			if (fixture.density == 0.0) {
+				continue
+			}
+
+			val massData = fixture.getMassData()
+			mass += massData.mass
+			localCenter += massData.center * massData.mass
+			rotInertia += massData.inertia
+		}
+
+		// Compute center of mass.
+		if (mass > 0.0) {
+			invMass = 1.0 / mass
+			localCenter *= invMass
+		}
+
+		if (rotInertia > 0.0 && !isFixedRotation) {
+			// Center the inertia about the center of mass.
+			rotInertia -= mass * localCenter.dotProduct(localCenter)
+			check(rotInertia > 0.0)
+			rotInertiaInv = 1.0 / rotInertia
+		} else {
+			rotInertia = 0.0
+			rotInertiaInv = 0.0
+		}
+
+		// Move center of mass.
+		val oldCenter = sweep.c
+		sweep.localCenter = localCenter
+		sweep.c = transform.times(localCenter)
+		sweep.c0 = sweep.c
+
+		// Update center of mass velocity.
+		linearVelocity += b2Cross(angularVelocity, sweep.c - oldCenter)
+	}
+
+	override var massData: MassData
+		get() = MassData(
+			mass = mass,
+			inertia = inertia,
+			center = sweep.localCenter
+		)
+		set(value) {
+			if (world.isLocked)
+				throw ConcurrentModificationException()
+
+			if (type != BodyType.DYNAMIC)
+				return
+
+			invMass = 0.0
+			rotInertia = 0.0
+			rotInertiaInv = 0.0
+
+			mass = value.mass
+
+			if (mass <= 0.0) {
+				mass = 1.0
+			}
+
+			invMass = 1.0 / mass
+
+			if (value.inertia > 0.0 && !isFixedRotation) {
+				rotInertia = value.inertia - mass * value.center.dotProduct(value.center)
+				check(rotInertia > 0.0)
+				rotInertiaInv = 1.0 / rotInertia
+			}
+
+			// Move center of mass.
+			val oldCenter = sweep.c
+			sweep.localCenter = value.center
+			sweep.c = transform.times(sweep.localCenter)
+			sweep.c0 = sweep.c
+
+			// Update center of mass velocity.
+			linearVelocity += b2Cross(angularVelocity, sweep.c - oldCenter)
+		}
+
+	internal fun shouldCollide(other: IBody): Boolean {
+		// At least one body should be dynamic.
+		if (type != BodyType.DYNAMIC && other.type != BodyType.DYNAMIC)
+			return false
+
+		// Does a joint prevent collision?
+		var joint = jointList
+
+		while (joint != null) {
+			if (joint.otherNullable == other && !joint.joint.collideConnected) {
+				return false
+			}
+
+			joint = joint.next
+		}
+
+		return true
+	}
+
+	init {
+		sweep.c0 = transform.position
+		sweep.c = transform.position
+		sweep.a0 = def.angle
+		sweep.a = def.angle
+	}
+
+	override fun setTransform(position: Vector2d, angle: Double) {
+		if (world.isLocked)
+			throw ConcurrentModificationException()
+
+		transform.rotation.set(angle)
+		transform.position = position
+
+		sweep.c = transform.times(sweep.localCenter)
+		sweep.a = angle
+
+		sweep.c0 = sweep.c
+		sweep.a0 = angle
+
+		val broadPhase = world.contactManager.broadPhase
+
+		for (fixture in fixtureIterator) {
+			(fixture as Fixture?)?.synchronize(broadPhase, transform, transform)
+		}
+
+		// Check for new contacts the next step
+		world.notifyNewContacts()
+	}
+
+	override val inertia: Double
+		get() = rotInertia + mass * sweep.localCenter.dotProduct(sweep.localCenter)
+
+	override val localCenter: Vector2d
+		get() = sweep.localCenter
+
+	override val worldCenter: Vector2d
+		get() = sweep.c
+
+	override fun getWorldPoint(localPoint: Vector2d): Vector2d {
+		return transform.times(localPoint)
+	}
+
+	override fun getWorldVector(localPoint: Vector2d): Vector2d {
+		return transform.rotation.times(localPoint)
+	}
+
+	override fun getLocalPoint(worldPoint: Vector2d): Vector2d {
+		return transform.timesT(worldPoint)
+	}
+
+	override fun getLocalVector(worldVector: Vector2d): Vector2d {
+		return transform.rotation.timesT(worldVector)
+	}
+
+	override fun getLinearVelocityFromWorldPoint(worldPoint: Vector2d): Vector2d {
+		return linearVelocity + b2Cross(angularVelocity, worldPoint - sweep.c)
+	}
+
+	override fun getLinearVelocityFromLocalPoint(localPoint: Vector2d): Vector2d {
+		return getLinearVelocityFromWorldPoint(getWorldPoint(localPoint))
+	}
+
+	override fun applyForce(force: Vector2d, point: Vector2d, wake: Boolean) {
+		if (type != BodyType.DYNAMIC)
+			return
+
+		if (wake && !isAwake)
+			isAwake = true
+
+		// Don't accumulate a force if the body is sleeping.
+		if (isAwake) {
+			this.force += force
+			this.torque += b2Cross(point - sweep.c, force)
+		}
+	}
+
+	override fun applyForceToCenter(force: Vector2d, wake: Boolean) {
+		if (type != BodyType.DYNAMIC)
+			return
+
+		if (wake && !isAwake)
+			isAwake = true
+
+		// Don't accumulate a force if the body is sleeping.
+		if (isAwake) {
+			this.force += force
+		}
+	}
+
+	override fun applyTorque(torque: Double, wake: Boolean) {
+		if (type != BodyType.DYNAMIC)
+			return
+
+		if (wake && !isAwake)
+			isAwake = true
+
+		if (isAwake) {
+			this.torque += torque
+		}
+	}
+
+	override fun applyLinearImpulse(impulse: Vector2d, point: Vector2d, wake: Boolean) {
+		if (type != BodyType.DYNAMIC)
+			return
+
+		if (wake && !isAwake)
+			isAwake = true
+
+		if (isAwake) {
+			linearVelocity += impulse * invMass
+			angularVelocity += rotInertiaInv *  b2Cross(point - sweep.c, impulse)
+		}
+	}
+
+	override fun applyLinearImpulseToCenter(impulse: Vector2d, wake: Boolean) {
+		if (type != BodyType.DYNAMIC)
+			return
+
+		if (wake && !isAwake)
+			isAwake = true
+
+		if (isAwake) {
+			linearVelocity += impulse * invMass
+		}
+	}
+
+	override fun applyAngularImpulse(impulse: Double, wake: Boolean) {
+		if (type != BodyType.DYNAMIC)
+			return
+
+		if (wake && !isAwake)
+			isAwake = true
+
+		if (isAwake) {
+			angularVelocity += impulse * rotInertiaInv
+		}
+	}
+
+	internal fun synchronizeTransform() {
+		transform.rotation.set(sweep.a)
+		transform.position = sweep.c - transform.rotation.times(sweep.localCenter)
+	}
+
+	internal fun advance(alpha: Double) {
+		sweep.advance(alpha)
+		sweep.c = sweep.c0
+		sweep.a = sweep.a0
+		synchronizeTransform()
+	}
+
+	internal fun synchronizeFixtures() {
+		val broadPhase = world.contactManager.broadPhase
+
+		if (isAwake) {
+			val transform1 = Transform()
+			transform1.rotation.set(sweep.a0)
+			transform1.position = sweep.c0 - transform1.rotation.times(sweep.localCenter)
+
+			for (fixture in fixtureIterator) {
+				(fixture as Fixture).synchronize(broadPhase, transform1, transform)
+			}
+		} else {
+			for (fixture in fixtureIterator) {
+				(fixture as Fixture).synchronize(broadPhase, transform, transform)
+			}
+		}
+	}
+
+	override fun dump() {
+		TODO("Not yet implemented")
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/ContactManager.kt

@@ -0,0 +1,193 @@
+package ru.dbotthepony.kbox2d.dynamics
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.collision.BroadPhase
+import ru.dbotthepony.kbox2d.dynamics.contact.AbstractContact
+
+class ContactManager : IContactManager {
+	override val broadPhase: IBroadPhase = BroadPhase()
+	override var contactList: IContact? = null
+	override var contactCount: Int = 0
+		private set
+
+	override var contactFilter: IContactFilter? = null
+	override var contactListener: IContactListener? = null
+
+	override fun destroy(contact: IContact) {
+		contact as AbstractContact
+
+		val fixtureA = contact.fixtureA
+		val fixtureB = contact.fixtureB
+		val bodyA = fixtureA.body as Body
+		val bodyB = fixtureB.body as Body
+
+		if (contact.isTouching) {
+			contactListener?.endContact(contact)
+		}
+
+		// Remove from the world.
+		run {
+			val prev = contact.prev as AbstractContact?
+			val next = contact.next as AbstractContact?
+
+			prev?.next = next
+			next?.prev = prev
+
+			if (contactList == contact) {
+				contactList = next ?: prev
+			}
+		}
+
+		// Remove from body 1
+		run {
+			val prev = contact.nodeA.prev
+			val next = contact.nodeA.next
+
+			prev?.next = next
+			next?.prev = prev
+
+			if (contact.nodeA == bodyA.contactEdge) {
+				bodyA.contactEdge = next ?: prev
+			}
+		}
+
+		// Remove from body 2
+		run {
+			val prev = contact.nodeB.prev
+			val next = contact.nodeB.next
+
+			prev?.next = next
+			next?.prev = prev
+
+			if (contact.nodeB == bodyB.contactEdge) {
+				bodyB.contactEdge = next ?: prev
+			}
+		}
+
+		contactCount--
+	}
+
+	/**
+	 * This is the top level collision call for the time step. Here
+	 * all the narrow phase collision is processed for the world
+	 * contact list.
+	 */
+	override fun collide() {
+		// Update awake contacts.
+
+		for (c in contactListIterator) {
+			c as AbstractContact
+			val fixtureA = c.fixtureA
+			val fixtureB = c.fixtureB
+			val indexA = c.childIndexA
+			val indexB = c.childIndexB
+			val bodyA = fixtureA.body as Body
+			val bodyB = fixtureB.body as Body
+
+			// Is this contact flagged for filtering?
+			if (c.isFlaggedForFiltering) {
+				// Should these bodies collide?
+				if (!bodyB.shouldCollide(bodyA)) {
+					destroy(c)
+					continue
+				}
+
+				// Check user filtering.
+				if (contactFilter?.shouldCollide(fixtureA, fixtureB) == false) {
+					destroy(c)
+					continue
+				}
+
+				// Clear the filtering flag.
+				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
+
+			// At least one body must be awake and it must be dynamic or kinematic.
+			if (!activeA && !activeB) {
+				continue
+			}
+
+			val proxyIdA = fixtureA.proxies[indexA].proxyId
+			val proxyIdB = fixtureB.proxies[indexB].proxyId
+			val overlap = broadPhase.testOverlap(proxyIdA, proxyIdB)
+
+			if (!overlap) {
+				destroy(c)
+				continue
+			}
+
+			c.update(contactListener)
+		}
+	}
+
+	override fun findNewContacts() {
+		broadPhase.updatePairs(this::addPair)
+	}
+
+	override fun addPair(proxyUserDataA: Any?, proxyUserDataB: Any?) {
+		val proxyA = proxyUserDataA as FixtureProxy
+		val proxyB = proxyUserDataB as FixtureProxy
+
+		val fixtureA = proxyA.fixture
+		val fixtureB = proxyB.fixture
+
+		val indexA = proxyA.childIndex
+		val indexB = proxyB.childIndex
+
+		val bodyA = fixtureA.body as Body
+		val bodyB = fixtureB.body as Body
+
+		// Are the fixtures on the same body?
+		if (bodyA === bodyB) {
+			return
+		}
+
+		// TODO_ERIN use a hash table to remove a potential bottleneck when both
+		// bodies have a lot of contacts.
+		// Does a contact already exist?
+		for (edge in bodyB.contactEdgeIterator) {
+			if (edge.other === bodyA) {
+				val fA = edge.contact.fixtureA
+				val fB = edge.contact.fixtureB
+				val iA = edge.contact.childIndexA
+				val iB = edge.contact.childIndexB
+
+				if (fA === fixtureA && fB === fixtureB && iA == indexA && iB == indexB) {
+					// A contact already exists.
+					return
+				}
+
+				if (fA === fixtureB && fB === fixtureA && iA == indexB && iB == indexA) {
+					// A contact already exists.
+					return
+				}
+			}
+		}
+
+		// Does a joint override collision? Is at least one body dynamic?
+		if (!bodyB.shouldCollide(bodyA)) {
+			return
+		}
+
+		// Check user filtering.
+		if (contactFilter?.shouldCollide(fixtureA, fixtureB) == false) {
+			return
+		}
+
+		// Call the factory.
+		val c = AbstractContact.create(fixtureA, indexA, fixtureB, indexB)
+
+		// Contact creation may swap fixtures.
+		// Insert into the world.
+		c.next = contactList
+		(contactList as AbstractContact?)?.prev = c
+		contactList = c
+
+		// Connect to island graph.
+		// (connection is done in AbstractContact initializer)
+		contactCount++
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/Fixture.kt

@@ -0,0 +1,147 @@
+package ru.dbotthepony.kbox2d.dynamics
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.collision.shapes.ChainShape
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+import java.util.*
+import kotlin.collections.ArrayList
+
+open class Fixture(
+	body: ru.dbotthepony.kbox2d.api.IBody,
+	def: FixtureDef
+) : IFixture {
+	final override var body: ru.dbotthepony.kbox2d.api.IBody? = body
+		protected set
+
+	final override var next: IFixture? = null
+
+	override val userData: Any? = def.userData
+	override var friction: Double = def.friction
+	override var restitution: Double = def.restitution
+	override var restitutionThreshold: Double = def.restitutionThreshold
+
+	override val type: IShape.Type get() = shape.type
+
+	override var isSensor: Boolean = def.isSensor
+		set(value) {
+			if (field != value) {
+				checkNotNull(body) { "Already destroyed" }.isAwake = true
+				field = value
+			}
+		}
+
+	final override val shape: IShape<*> = requireNotNull(def.shape) { "null shape provided" }.copy()
+
+	init {
+		if (shape is PolygonShape) {
+			shape.validate(true)
+		}
+
+		if (shape.childCount < 0) {
+			throw IllegalArgumentException("Shape $shape has ${shape.childCount} children")
+		}
+	}
+
+	protected val internalProxies = ArrayList<FixtureProxy>(shape.childCount)
+	final override val proxies: List<FixtureProxy> = Collections.unmodifiableList(internalProxies)
+
+	override var density: Double = def.density
+		set(value) {
+			require(value.isFinite()) { "Infinite density" }
+			require(!value.isNaN()) { "NaN density" }
+			require(value >= 0.0) { "Negative density of $value" }
+			field = value
+		}
+
+	override var filter: IFilter = def.filter.immutable()
+		set(value) {
+			if (value is ImmutableFilter)
+				field = value
+			else
+				field = (value as Filter).immutable()
+
+			refilter()
+		}
+
+	fun destroy() {
+		checkNotNull(body) { "Already destroyed" }.destroyFixture(this)
+	}
+
+	internal fun unlink() {
+		check(body != null) { "Already destroyed" }
+		check(internalProxies.isEmpty()) { "Still having proxies" }
+		body = null
+	}
+
+	/**
+	 * These support body activation/deactivation.
+	 */
+	internal fun createProxies(broadPhase: IBroadPhase, xf: Transform) {
+		check(body != null) { "Already destroyed" }
+		check(internalProxies.isEmpty()) { "Already having proxies" }
+
+		for (i in 0 until shape.childCount) {
+			val aabb = shape.computeAABB(xf, i)
+
+			val proxy = FixtureProxy(
+				fixture = this,
+				childIndex = i,
+				aabb = aabb,
+			)
+
+			proxy.proxyId = broadPhase.createProxy(aabb, proxy)
+			internalProxies.add(proxy)
+		}
+	}
+
+	/**
+	 * These support body activation/deactivation.
+	 */
+	internal fun destroyProxies(broadPhase: IBroadPhase) {
+		check(body != null) { "Already destroyed" }
+
+		// Destroy proxies in the broad-phase.
+		for (proxy in internalProxies) {
+			broadPhase.destroyProxy(proxy.proxyId)
+		}
+
+		internalProxies.clear()
+	}
+
+	internal fun synchronize(broadPhase: IBroadPhase, transform1: Transform, transform2: Transform) {
+		check(body != null) { "Already destroyed" }
+
+		for (proxy in internalProxies) {
+			val aabb1 = shape.computeAABB(transform1, proxy.childIndex)
+			val aabb2 = shape.computeAABB(transform2, proxy.childIndex)
+
+			proxy.aabb = aabb1.combine(aabb2)
+			val displacement = aabb2.centre - aabb1.centre
+			broadPhase.moveProxy(proxy.proxyId, proxy.aabb, displacement)
+		}
+	}
+
+	override fun refilter() {
+		val body = body
+		check(body != null) { "Already destroyed" }
+		var edge = body.contactEdge
+
+		while (edge != null) {
+			val contact = edge.contact
+			val fixtureA = contact.fixtureA
+			val fixtureB = contact.fixtureB
+
+			if (fixtureA == this || fixtureB == this)
+				contact.flagForFiltering()
+
+			edge = edge.next
+		}
+
+		val world = body.world
+		val broadPhase = world.contactManager.broadPhase
+
+		for (proxy in internalProxies) {
+			broadPhase.touchProxy(proxy.proxyId)
+		}
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/AbstractContact.kt

@@ -0,0 +1,215 @@
+package ru.dbotthepony.kbox2d.dynamics.contact
+
+import it.unimi.dsi.fastutil.objects.Object2ObjectArrayMap
+import it.unimi.dsi.fastutil.objects.Object2ObjectFunction
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.collision.WorldManifold
+import ru.dbotthepony.kbox2d.collision.b2TestOverlap
+import ru.dbotthepony.kbox2d.dynamics.Body
+
+fun interface ContactFactory {
+	fun factorize(fixtureA: IFixture, childIndexA: Int, fixtureB: IFixture, childIndexB: Int): AbstractContact
+}
+
+sealed class AbstractContact(
+	final override val fixtureA: IFixture,
+	final override val childIndexA: Int,
+	final override val fixtureB: IFixture,
+	final override val childIndexB: Int,
+) : IContact {
+	internal var flags: Int = ContactFlags.ENABLED.bitmask
+	internal var isOnIsland = false
+	internal var toiFlag = false
+
+	final override var manifold: Manifold = Manifold()
+		private set
+
+	override var next: IContact? = null
+		internal set
+
+	override var prev: IContact? = null
+		internal set
+
+	internal var isFlaggedForFiltering: Boolean
+		get() = ContactFlags.FILTER.isit(flags)
+		set(value) { flags = ContactFlags.FILTER.update(flags, value) }
+
+	internal val nodeA: ContactEdge = ContactEdge(contact = this, other = fixtureB.body!!)
+	internal val nodeB: ContactEdge = ContactEdge(contact = this, other = fixtureA.body!!)
+
+	init {
+		val bodyA = fixtureA.body as Body
+		val bodyB = fixtureB.body as Body
+
+		nodeA.next = bodyA.contactEdge
+		nodeB.next = bodyB.contactEdge
+
+		bodyA.contactEdge?.prev = nodeA
+		bodyB.contactEdge?.prev = nodeB
+
+		bodyA.contactEdge = nodeA
+		bodyB.contactEdge = nodeB
+	}
+
+	override var friction: Double = b2MixFriction(fixtureA.friction, fixtureB.friction)
+	override var restitution: Double = b2MixRestitution(fixtureA.restitution, fixtureB.restitution)
+	override var restitutionThreshold: Double = b2MixRestitutionThreshold(fixtureA.restitutionThreshold, fixtureB.restitutionThreshold)
+	override var tangentSpeed: Double = 0.0
+
+	internal var toiCount: Int = 0
+	internal var toi: Double = 0.0
+
+	override val worldManifold: IWorldManifold get() {
+		val bodyA = checkNotNull(fixtureA.body) { "FixtureA has no body attached" }
+		val bodyB = checkNotNull(fixtureB.body) { "FixtureB has no body attached" }
+
+		val shapeA = fixtureA.shape
+		val shapeB = fixtureB.shape
+
+		return WorldManifold(manifold, bodyA.transform, shapeA.radius, bodyB.transform, shapeB.radius)
+	}
+
+	override var isTouching: Boolean
+		get() = (flags and ContactFlags.TOUCHING.bitmask) == ContactFlags.TOUCHING.bitmask
+		internal set(value) { flags = ContactFlags.TOUCHING.update(flags, value) }
+
+	override var isEnabled: Boolean
+		get() = (flags and ContactFlags.ENABLED.bitmask) == ContactFlags.ENABLED.bitmask
+		set(value) { flags = ContactFlags.ENABLED.update(flags, value) }
+
+	override fun flagForFiltering() {
+		flags = flags or ContactFlags.FILTER.bitmask
+	}
+
+	/**
+	 * Awakens fixtures this contact belongs to if neither of them are sensors
+	 */
+	fun destroy() {
+		if (manifold.points.isNotEmpty() && !fixtureA.isSensor && !fixtureB.isSensor) {
+			fixtureA.body!!.isAwake = true
+			fixtureB.body!!.isAwake = true
+		}
+	}
+
+	/**
+	 * Update the contact manifold and touching status.
+	 * Note: do not assume the fixture AABBs are overlapping or are valid.
+	 */
+	internal fun update(contactListener: IContactListener?) {
+		// Re-enable this contact.
+		flags = ContactFlags.ENABLED.or(flags)
+
+		val oldManifold = manifold
+
+		val touching: Boolean
+		val wasTouching = isTouching
+
+		val sensor = fixtureA.isSensor || fixtureB.isSensor
+
+		val bodyA = fixtureA.body as Body
+		val bodyB = fixtureB.body as Body
+
+		val xfA = bodyA.transform
+		val xfB = bodyB.transform
+
+		// Is this contact a sensor?
+		if (sensor) {
+			val shapeA = fixtureA.shape
+			val shapeB = fixtureB.shape
+
+			touching = b2TestOverlap(shapeA, childIndexA, shapeB, childIndexB, xfA, xfB)
+			// Sensors don't generate manifolds.
+			manifold = Manifold.EMPTY
+		} else {
+			manifold = evaluate(xfA, xfB)
+			touching = manifold.points.isNotEmpty()
+
+			// Match old contact ids to new contact ids and copy the
+			// stored impulses to warm start the solver.
+			for (mp2 in manifold.points) {
+				mp2.normalImpulse = 0.0
+				mp2.tangentImpulse = 0.0
+				val id2 = mp2.id
+
+				for (mp1 in oldManifold.points) {
+					if (mp1.id.key == id2.key) {
+						mp2.normalImpulse = mp1.normalImpulse
+						mp2.tangentImpulse = mp1.tangentImpulse
+						break
+					}
+				}
+			}
+
+			if (touching != wasTouching) {
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+			}
+		}
+
+		this.isTouching = touching
+
+		if (!wasTouching && touching) {
+			contactListener?.beginContact(this)
+		} else if (wasTouching && !touching) {
+			contactListener?.endContact(this)
+		}
+
+		if (!sensor && touching) {
+			contactListener?.preSolve(this, oldManifold)
+		}
+	}
+
+	companion object {
+		private val registry =
+			Object2ObjectArrayMap<IShape.Type, Object2ObjectArrayMap<IShape.Type, ContactFactory>>()
+
+		internal fun register(type1: IShape.Type, type2: IShape.Type, factory: ContactFactory) {
+			registry.computeIfAbsent(type1, Object2ObjectFunction { Object2ObjectArrayMap() })
+				.put(type2, factory)
+		}
+
+		internal fun create(
+			fixtureA: IFixture,
+			indexA: Int,
+			fixtureB: IFixture,
+			indexB: Int,
+		): AbstractContact {
+			val type1 = fixtureA.type
+			val type2 = fixtureB.type
+
+			return registry[type1]?.get(type2)?.factorize(fixtureA, indexA, fixtureB, indexB)
+				?: registry[type2]?.get(type1)?.factorize(fixtureB, indexB, fixtureA, indexA)
+				?: throw IllegalArgumentException("No collision handler for between $type1 and $type2")
+		}
+
+		init {
+			register(IShape.Type.POLYGON, IShape.Type.POLYGON) { fixtureA: IFixture, _: Int, fixtureB: IFixture, _: Int ->
+				return@register PolygonContact(fixtureA, fixtureB)
+			}
+
+			register(IShape.Type.POLYGON, IShape.Type.CIRCLE) { fixtureA: IFixture, _: Int, fixtureB: IFixture, _: Int ->
+				return@register PolygonCircleContact(fixtureA, fixtureB)
+			}
+
+			register(IShape.Type.CIRCLE, IShape.Type.CIRCLE) { fixtureA: IFixture, _: Int, fixtureB: IFixture, _: Int ->
+				return@register CircleContact(fixtureA, fixtureB)
+			}
+
+			register(IShape.Type.EDGE, IShape.Type.CIRCLE) { fixtureA: IFixture, _: Int, fixtureB: IFixture, _: Int ->
+				return@register EdgeCircleContact(fixtureA, fixtureB)
+			}
+
+			register(IShape.Type.EDGE, IShape.Type.POLYGON) { fixtureA: IFixture, _: Int, fixtureB: IFixture, _: Int ->
+				return@register EdgePolygonContact(fixtureA, fixtureB)
+			}
+
+			register(IShape.Type.CHAIN, IShape.Type.POLYGON) { fixtureA: IFixture, indexA: Int, fixtureB: IFixture, indexB: Int ->
+				return@register ChainPolygonContact(fixtureA, indexA, fixtureB, indexB)
+			}
+
+			register(IShape.Type.CHAIN, IShape.Type.CIRCLE) { fixtureA: IFixture, indexA: Int, fixtureB: IFixture, indexB: Int ->
+				return@register ChainCircleContact(fixtureA, indexA, fixtureB, indexB)
+			}
+		}
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainCircleContact.kt

@@ -0,0 +1,28 @@
+package ru.dbotthepony.kbox2d.dynamics.contact
+
+import ru.dbotthepony.kbox2d.api.IFixture
+import ru.dbotthepony.kbox2d.api.IShape
+import ru.dbotthepony.kbox2d.api.Manifold
+import ru.dbotthepony.kbox2d.api.Transform
+import ru.dbotthepony.kbox2d.collision.handler.b2CollideEdgeAndCircle
+import ru.dbotthepony.kbox2d.collision.shapes.ChainShape
+import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
+
+class ChainCircleContact(
+	fixtureA: IFixture,
+	childIndexA: Int,
+	fixtureB: IFixture,
+	childIndexB: Int,
+) : AbstractContact(fixtureA, childIndexA, fixtureB, childIndexB) {
+	init {
+		require(fixtureA.type == IShape.Type.CHAIN) { "Fixture A is of type ${fixtureA.type}" }
+		require(fixtureB.type == IShape.Type.CIRCLE) { "Fixture B is of type ${fixtureB.type}" }
+	}
+
+	override fun evaluate(xfA: Transform, xfB: Transform): Manifold {
+		val chain = fixtureA.shape as ChainShape
+		val edge = chain.getChildEdge(childIndexA)
+
+		return b2CollideEdgeAndCircle(edge, xfA, fixtureB.shape as CircleShape, xfB)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainPolygonContact.kt

@@ -0,0 +1,28 @@
+package ru.dbotthepony.kbox2d.dynamics.contact
+
+import ru.dbotthepony.kbox2d.api.IFixture
+import ru.dbotthepony.kbox2d.api.IShape
+import ru.dbotthepony.kbox2d.api.Manifold
+import ru.dbotthepony.kbox2d.api.Transform
+import ru.dbotthepony.kbox2d.collision.handler.b2CollideEdgeAndPolygon
+import ru.dbotthepony.kbox2d.collision.shapes.ChainShape
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+
+class ChainPolygonContact(
+	fixtureA: IFixture,
+	childIndexA: Int,
+	fixtureB: IFixture,
+	childIndexB: Int,
+) : AbstractContact(fixtureA, childIndexA, fixtureB, childIndexB) {
+	init {
+		require(fixtureA.type == IShape.Type.CHAIN) { "Fixture A is of type ${fixtureA.type}" }
+		require(fixtureB.type == IShape.Type.POLYGON) { "Fixture B is of type ${fixtureB.type}" }
+	}
+
+	override fun evaluate(xfA: Transform, xfB: Transform): Manifold {
+		val chain = fixtureA.shape as ChainShape
+		val edge = chain.getChildEdge(childIndexA)
+
+		return b2CollideEdgeAndPolygon(edge, xfA, fixtureB.shape as PolygonShape, xfB)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/CircleContact.kt

@@ -0,0 +1,22 @@
+package ru.dbotthepony.kbox2d.dynamics.contact
+
+import ru.dbotthepony.kbox2d.api.IFixture
+import ru.dbotthepony.kbox2d.api.IShape
+import ru.dbotthepony.kbox2d.api.Manifold
+import ru.dbotthepony.kbox2d.api.Transform
+import ru.dbotthepony.kbox2d.collision.handler.b2CollideCircles
+import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
+
+class CircleContact(
+	fixtureA: IFixture,
+	fixtureB: IFixture,
+) : AbstractContact(fixtureA, 0, fixtureB, 0) {
+	init {
+		require(fixtureA.type == IShape.Type.CIRCLE) { "Fixture A is of type ${fixtureA.type}" }
+		require(fixtureB.type == IShape.Type.CIRCLE) { "Fixture B is of type ${fixtureB.type}" }
+	}
+
+	override fun evaluate(xfA: Transform, xfB: Transform): Manifold {
+		return b2CollideCircles(fixtureA.shape as CircleShape, xfA, fixtureB.shape as CircleShape, xfB)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgeCircleContact.kt

@@ -0,0 +1,23 @@
+package ru.dbotthepony.kbox2d.dynamics.contact
+
+import ru.dbotthepony.kbox2d.api.IFixture
+import ru.dbotthepony.kbox2d.api.IShape
+import ru.dbotthepony.kbox2d.api.Manifold
+import ru.dbotthepony.kbox2d.api.Transform
+import ru.dbotthepony.kbox2d.collision.handler.b2CollideEdgeAndCircle
+import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
+import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape
+
+class EdgeCircleContact(
+	fixtureA: IFixture,
+	fixtureB: IFixture,
+) : AbstractContact(fixtureA, 0, fixtureB, 0) {
+	init {
+		require(fixtureA.type == IShape.Type.EDGE) { "Fixture A is of type ${fixtureA.type}, expected EDGE" }
+		require(fixtureB.type == IShape.Type.CIRCLE) { "Fixture B is of type ${fixtureB.type}, expected CIRCLE" }
+	}
+
+	override fun evaluate(xfA: Transform, xfB: Transform): Manifold {
+		return b2CollideEdgeAndCircle(fixtureA.shape as EdgeShape, xfA, fixtureB.shape as CircleShape, xfB)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgePolygonContact.kt

@@ -0,0 +1,23 @@
+package ru.dbotthepony.kbox2d.dynamics.contact
+
+import ru.dbotthepony.kbox2d.api.IFixture
+import ru.dbotthepony.kbox2d.api.IShape
+import ru.dbotthepony.kbox2d.api.Manifold
+import ru.dbotthepony.kbox2d.api.Transform
+import ru.dbotthepony.kbox2d.collision.handler.b2CollideEdgeAndPolygon
+import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+
+class EdgePolygonContact(
+	fixtureA: IFixture,
+	fixtureB: IFixture,
+) : AbstractContact(fixtureA, 0, fixtureB, 0) {
+	init {
+		require(fixtureA.type == IShape.Type.EDGE) { "Fixture A is of type ${fixtureA.type}, expected EDGE" }
+		require(fixtureB.type == IShape.Type.POLYGON) { "Fixture B is of type ${fixtureB.type}, expected POLYGON" }
+	}
+
+	override fun evaluate(xfA: Transform, xfB: Transform): Manifold {
+		return b2CollideEdgeAndPolygon(fixtureA.shape as EdgeShape, xfA, fixtureB.shape as PolygonShape, xfB)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonCircleContact.kt

@@ -0,0 +1,23 @@
+package ru.dbotthepony.kbox2d.dynamics.contact
+
+import ru.dbotthepony.kbox2d.api.IFixture
+import ru.dbotthepony.kbox2d.api.IShape
+import ru.dbotthepony.kbox2d.api.Manifold
+import ru.dbotthepony.kbox2d.api.Transform
+import ru.dbotthepony.kbox2d.collision.handler.b2CollidePolygonAndCircle
+import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+
+class PolygonCircleContact(
+	fixtureA: IFixture,
+	fixtureB: IFixture,
+) : AbstractContact(fixtureA, 0, fixtureB, 0) {
+	init {
+		require(fixtureA.type == IShape.Type.POLYGON) { "Fixture A is of type ${fixtureA.type}, expected POLYGON" }
+		require(fixtureB.type == IShape.Type.CIRCLE) { "Fixture B is of type ${fixtureB.type}, expected CIRCLE" }
+	}
+
+	override fun evaluate(xfA: Transform, xfB: Transform): Manifold {
+		return b2CollidePolygonAndCircle(fixtureA.shape as PolygonShape, xfA, fixtureB.shape as CircleShape, xfB)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonContact.kt

@@ -0,0 +1,26 @@
+package ru.dbotthepony.kbox2d.dynamics.contact
+
+import ru.dbotthepony.kbox2d.api.IFixture
+import ru.dbotthepony.kbox2d.api.IShape
+import ru.dbotthepony.kbox2d.api.Manifold
+import ru.dbotthepony.kbox2d.api.Transform
+import ru.dbotthepony.kbox2d.collision.handler.b2CollidePolygons
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+
+class PolygonContact(
+	fixtureA: IFixture,
+	fixtureB: IFixture,
+) : AbstractContact(fixtureA, 0, fixtureB, 0) {
+	init {
+		require(fixtureA.type == IShape.Type.POLYGON) { "Fixture A has type of ${fixtureA.type}" }
+		require(fixtureB.type == IShape.Type.POLYGON) { "Fixture B has type of ${fixtureB.type}" }
+	}
+
+	override fun evaluate(xfA: Transform, xfB: Transform): Manifold {
+		return b2CollidePolygons(
+			fixtureA.shape as PolygonShape,
+			xfA,
+			fixtureB.shape as PolygonShape,
+			xfB)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/ContactSolver.kt

@@ -0,0 +1,820 @@
+package ru.dbotthepony.kbox2d.dynamics.internal
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.collision.WorldManifold
+import ru.dbotthepony.kbox2d.dynamics.contact.AbstractContact
+import ru.dbotthepony.kbox2d.dynamics.Body
+import ru.dbotthepony.kstarbound.math.MutableMatrix2d
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+import java.lang.IllegalArgumentException
+import kotlin.math.absoluteValue
+
+private const val g_blockSolve = true
+
+// Ensure a reasonable condition number.
+private const val k_maxConditionNumber = 1000.0
+
+private const val k_errorTol = 1E-3
+
+private const val B2_DEBUG_SOLVER = false
+
+internal data class VelocityCostantPoint(
+	var rA: Vector2d = Vector2d.ZERO,
+	var rB: Vector2d = Vector2d.ZERO,
+	var normalImpulse: Double = 0.0,
+	var tangentImpulse: Double = 0.0,
+	var normalMass: Double = 0.0,
+	var tangentMass: Double = 0.0,
+	var velocityBias: Double = 0.0,
+)
+
+internal data class ContactVelocityConstraint(
+	var points: Array<VelocityCostantPoint>,
+	var normal: Vector2d,
+	var normalMass: MutableMatrix2d,
+	var K: MutableMatrix2d,
+	var indexA: Int,
+	var indexB: Int,
+	var invMassA: Double,
+	var invMassB: Double,
+	var invIA: Double,
+	var invIB: Double,
+	var friction: Double,
+	var restitution: Double,
+	var threshold: Double,
+	var tangentSpeed: Double,
+	var contactIndex: Int,
+)
+
+internal data class ContactSolverDef(
+	val step: B2TimeStep,
+	val contacts: List<AbstractContact>,
+	val positions: List<B2Position>,
+	val velocities: List<B2Velocity>,
+)
+
+internal data class ContactPositionConstraint(
+	val localPoints: Array<Vector2d>,
+	var localNormal: Vector2d,
+	var localPoint: Vector2d,
+	var indexA: Int,
+	var indexB: Int,
+	var invMassA: Double,
+	var invMassB: Double,
+	var localCenterA: Vector2d,
+	var localCenterB: Vector2d,
+	var invIA: Double,
+	var invIB: Double,
+	var radiusA: Double,
+	var radiusB: Double,
+	val type: Manifold.Type?
+)
+
+internal class PositionSolverManifold(pc: ContactPositionConstraint, xfA: Transform, xfB: Transform, index: Int) {
+	val normal: Vector2d
+	val point: Vector2d
+	val separation: Double
+
+	operator fun component1() = normal
+	operator fun component2() = point
+	operator fun component3() = separation
+
+	init {
+		check(pc.localPoints.isNotEmpty()) { "localPoints is empty" }
+
+		when (pc.type) {
+			Manifold.Type.CIRCLES -> {
+				val pointA = b2Mul(xfA, pc.localPoint)
+				val pointB = b2Mul(xfB, pc.localPoints[0])
+				normal = (pointB - pointA).normalized
+				point = 0.5 * (pointA + pointB)
+				separation = b2Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB
+			}
+
+			Manifold.Type.FACE_A -> {
+				normal = b2Mul(xfA.q, pc.localNormal)
+				val planePoint = b2Mul(xfA, pc.localPoint)
+
+				val clipPoint = b2Mul(xfB, pc.localPoints[index])
+				separation = b2Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB
+				point = clipPoint
+			}
+
+			Manifold.Type.FACE_B -> {
+				val normal = b2Mul(xfB.q, pc.localNormal)
+				val planePoint = b2Mul(xfB, pc.localPoint)
+
+				val clipPoint = b2Mul(xfA, pc.localPoints[index])
+				separation = b2Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB
+				point = clipPoint
+
+				// Ensure normal points from A to B
+				this.normal = -normal
+			}
+
+			else -> throw IllegalArgumentException("Position Constraint $pc has manifold of null (unknown) type")
+		}
+	}
+}
+
+internal class ContactSolver(
+	val step: B2TimeStep,
+	val positions: List<B2Position>,
+	val velocities: List<B2Velocity>,
+	val contacts: List<AbstractContact>,
+) {
+	constructor(def: ContactSolverDef) : this(
+		def.step,
+		def.positions,
+		def.velocities,
+		def.contacts,
+	)
+
+	val positionConstraints = ArrayList<ContactPositionConstraint>()
+	val velocityConstraints = ArrayList<ContactVelocityConstraint>()
+
+	init {
+		// Initialize position independent portions of the constraints.
+		for ((i, contact) in contacts.withIndex()) {
+			val fixtureA = contact.fixtureA
+			val fixtureB = contact.fixtureB
+			val shapeA = fixtureA.shape
+			val shapeB = fixtureB.shape
+			val radiusA = shapeA.radius
+			val radiusB = shapeB.radius
+			val bodyA = fixtureA.body as Body
+			val bodyB = fixtureB.body as Body
+			val manifold = contact.manifold
+
+			check(manifold.points.isNotEmpty()) { "Manifold points at $i are empty" }
+
+			val vc = ContactVelocityConstraint(
+				friction = contact.friction,
+				restitution = contact.restitution,
+				threshold = contact.restitutionThreshold,
+				tangentSpeed = contact.tangentSpeed,
+				indexA = bodyA.islandIndex,
+				indexB = bodyB.islandIndex,
+				invMassA = bodyA.invMass,
+				invMassB = bodyB.invMass,
+				invIA = bodyA.rotInertiaInv,
+				invIB = bodyB.rotInertiaInv,
+				contactIndex = i,
+				K = MutableMatrix2d(m00 = 0.0, m11 = 0.0),
+				normalMass = MutableMatrix2d(m00 = 0.0, m11 = 0.0),
+				normal = Vector2d.ZERO,
+				points = Array(manifold.points.size) { VelocityCostantPoint() }
+			)
+
+			velocityConstraints.add(vc)
+
+			val pc = ContactPositionConstraint(
+				indexA = bodyA.islandIndex,
+				indexB = bodyB.islandIndex,
+				invMassA = bodyA.invMass,
+				invMassB = bodyB.invMass,
+				localCenterA = bodyA.sweep.localCenter,
+				localCenterB = bodyB.sweep.localCenter,
+				invIA = bodyA.rotInertiaInv,
+				invIB = bodyB.rotInertiaInv,
+				localNormal = manifold.localNormal,
+				localPoint = manifold.localPoint,
+				localPoints = Array(manifold.points.size) { Vector2d.ZERO },
+				radiusA = radiusA,
+				radiusB = radiusB,
+				type = manifold.type,
+			)
+
+			positionConstraints.add(pc)
+
+			for (j in manifold.points.indices) {
+				val cp = manifold.points[j]
+				val vcp = vc.points[j]
+
+				if (step.warmStarting) {
+					vcp.normalImpulse = cp.normalImpulse * step.dtRatio
+					vcp.tangentImpulse = cp.tangentImpulse * step.dtRatio
+				}
+
+				pc.localPoints[j] = cp.localPoint
+			}
+		}
+	}
+
+	/**
+	 * Initialize position dependent portions of the velocity constraints.
+	 */
+	fun initializeVelocityConstraints() {
+		for (i in contacts.indices) {
+			val vc = velocityConstraints[i]
+			val pc = positionConstraints[i]
+
+			val radiusA = pc.radiusA
+			val radiusB = pc.radiusB
+			val manifold = contacts[vc.contactIndex].manifold
+
+			val indexA = vc.indexA
+			val indexB = vc.indexB
+
+			val mA = vc.invMassA
+			val mB = vc.invMassB
+			val iA = vc.invIA
+			val iB = vc.invIB
+
+			val localCenterA = pc.localCenterA
+			val localCenterB = pc.localCenterB
+
+			val cA = positions[indexA].c
+			val aA = positions[indexA].a
+			val vA = velocities[indexA].v
+			val wA = velocities[indexA].w
+
+			val cB = positions[indexB].c
+			val aB = positions[indexB].a
+			val vB = velocities[indexB].v
+			val wB = velocities[indexB].w
+
+			check(manifold.points.isNotEmpty()) { "Manifold at $i is empty" }
+
+			val xfA = Transform()
+			val xfB = Transform()
+
+			xfA.rotation.set(aA)
+			xfB.rotation.set(aB)
+
+			xfA.position = cA - b2Mul(xfA.q, localCenterA)
+			xfB.position = cB - b2Mul(xfB.q, localCenterB)
+
+			val worldManifold = WorldManifold(manifold, xfA, radiusA, xfB, radiusB)
+
+			vc.normal = worldManifold.normal
+
+			for ((j, vcp) in vc.points.withIndex()) {
+				vcp.rA = worldManifold.points[j] - cA
+				vcp.rB = worldManifold.points[j] - cB
+
+				val rnA = b2Cross(vcp.rA, vc.normal)
+				val rnB = b2Cross(vcp.rB, vc.normal)
+
+				val kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB
+
+				vcp.normalMass = if (kNormal > 0.0) 1.0 / kNormal else 0.0
+
+				val tangent = b2Cross(vc.normal, 1.0)
+
+				val rtA = b2Cross(vcp.rA, tangent)
+				val rtB = b2Cross(vcp.rB, tangent)
+
+				val kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB
+
+				vcp.tangentMass = if (kTangent > 0.0) 1.0 / kTangent else 0.0
+
+				// Setup a velocity bias for restitution.
+				// vcp.velocityBias = 0.0
+				val vRel = b2Dot(vc.normal, vB + b2Cross(wB, vcp.rB) - vA - b2Cross(wA, vcp.rA))
+
+				if (vRel < -vc.threshold) {
+					vcp.velocityBias = -vc.restitution * vRel
+				}
+			}
+
+			// If we have two points, then prepare the block solver.
+			if (vc.points.size == 2 && g_blockSolve) {
+				val vcp1 = vc.points[0]
+				val vcp2 = vc.points[1]
+
+				val rn1A = b2Cross(vcp1.rA, vc.normal)
+				val rn1B = b2Cross(vcp1.rB, vc.normal)
+				val rn2A = b2Cross(vcp2.rA, vc.normal)
+				val rn2B = b2Cross(vcp2.rB, vc.normal)
+
+				val k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B
+				val k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B
+				val k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B
+
+				if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) {
+					// K is safe to invert.
+					// k11 k12
+					// k12 k22
+					//
+					// vc->K.ex.Set(k11, k12);
+					// vc->K.ey.Set(k12, k22);
+
+					vc.K.m00 = k11
+					vc.K.m10 = k12
+					vc.K.m01 = k12
+					vc.K.m11 = k22
+					vc.normalMass = vc.K.getInverse().asMutableMatrix()
+				} else {
+					// The constraints are redundant, just use one.
+					// TODO_ERIN use deepest?
+					vc.points = Array(1) { vc.points[it] }
+				}
+			}
+		}
+	}
+
+	/**
+	 * Warm start.
+	 */
+	fun warmStart() {
+		for (vc in velocityConstraints) {
+			val indexA = vc.indexA
+			val indexB = vc.indexB
+			val mA = vc.invMassA
+			val iA = vc.invIA
+			val mB = vc.invMassB
+			val iB = vc.invIB
+
+			var vA = velocities[indexA].v
+			var wA = velocities[indexA].w
+			var vB = velocities[indexB].v
+			var wB = velocities[indexB].w
+
+			val normal = vc.normal
+			val tangent = b2Cross(normal, 1.0)
+
+			for (vcp in vc.points) {
+				val P = normal * vcp.normalImpulse + tangent * vcp.tangentImpulse
+				wA -= iA * b2Cross(vcp.rA, P)
+				vA -= P * mA
+				wB += iB * b2Cross(vcp.rB, P)
+				vB += P * mB
+			}
+
+			velocities[indexA].v = vA
+			velocities[indexA].w = wA
+			velocities[indexB].v = vB
+			velocities[indexB].w = wB
+		}
+	}
+
+	fun solveVelocityConstraints() {
+		for (vc in velocityConstraints) {
+			val indexA = vc.indexA
+			val indexB = vc.indexB
+			val mA = vc.invMassA
+			val iA = vc.invIA
+			val mB = vc.invMassB
+			val iB = vc.invIB
+
+			var vA = velocities[indexA].v
+			var wA = velocities[indexA].w
+			var vB = velocities[indexB].v
+			var wB = velocities[indexB].w
+
+			val normal = vc.normal
+			val tangent = b2Cross(normal, 1.0)
+			val friction = vc.friction
+
+			check(vc.points.size == 1 || vc.points.size == 2) { "Unexpected points amount: ${vc.points.size}" }
+
+			// Solve tangent constraints first because non-penetration is more important
+			// than friction.
+			for (vcp in vc.points) {
+				// Relative velocity at contact
+				val dv = vB + vB + b2Cross(wB, vcp.rB) - vA - b2Cross(wA, vcp.rA)
+
+				// Compute tangent force
+				val vt = b2Dot(dv, tangent) - vc.tangentSpeed
+				var lambda = vcp.tangentMass * (-vt)
+
+				// b2Clamp the accumulated force
+				val maxFriction = friction * vcp.normalImpulse
+				val newImpulse = b2Clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction)
+
+				lambda = newImpulse - vcp.tangentImpulse
+				vcp.tangentImpulse = newImpulse
+
+				// Apply contact impulse
+				val P = lambda * tangent
+
+				vA -= mA * P
+				wA -= iA * b2Cross(vcp.rA, P)
+
+				vB += mB * P
+				wB += iB * b2Cross(vcp.rB, P)
+			}
+
+			// Solve normal constraints
+			if (vc.points.size == 1 || !g_blockSolve) {
+				for (vcp in vc.points) {
+					// Relative velocity at contact
+					val dv = vB + b2Cross(wB, vcp.rB) - vA - b2Cross(wA, vcp.rA)
+
+					// Compute normal impulse
+					val vn = b2Dot(dv, normal)
+					var lambda = -vcp.normalMass * (vn - vcp.velocityBias)
+
+					// b2Clamp the accumulated impulse
+					val newImpulse = b2Max(vcp.normalImpulse + lambda, 0.0)
+					lambda = newImpulse - vcp.normalImpulse
+					vcp.normalImpulse = newImpulse
+
+					// Apply contact impulse
+					val P = lambda * normal
+					vA -= mA * P
+					wA -= iA * b2Cross(vcp.rA, P)
+
+					vB += mB * P
+					wB += iB * b2Cross(vcp.rB, P)
+				}
+			} else {
+				// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
+				// Build the mini LCP for this contact patch
+				//
+				// vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
+				//
+				// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
+				// b = vn0 - velocityBias
+				//
+				// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
+				// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
+				// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
+				// solution that satisfies the problem is chosen.
+				//
+				// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
+				// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
+				//
+				// Substitute:
+				//
+				// x = a + d
+				//
+				// a := old total impulse
+				// x := new total impulse
+				// d := incremental impulse
+				//
+				// For the current iteration we extend the formula for the incremental impulse
+				// to compute the new total impulse:
+				//
+				// vn = A * d + b
+				//    = A * (x - a) + b
+				//    = A * x + b - A * a
+				//    = A * x + b'
+				// b' = b - A * a;
+
+				val cp1 = vc.points[0]
+				val cp2 = vc.points[1]
+
+				val a = Vector2d(cp1.normalImpulse, cp2.normalImpulse)
+
+				check(a.x >= 0.0 && a.y >= 0.0) { a }
+
+				// Relative velocity at contact
+				var dv1 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA)
+				var dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA)
+
+				// Compute normal velocity
+				var vn1 = b2Dot(dv1, normal)
+				var vn2 = b2Dot(dv2, normal)
+
+				var b = Vector2d(
+					x = vn1 - cp1.velocityBias,
+					y = vn2 - cp2.velocityBias,
+				)
+
+				// Compute b'
+				b -= b2Mul(vc.K, a)
+
+				// for (;;)
+				run {
+					//
+					// Case 1: vn = 0
+					//
+					// 0 = A * x + b'
+					//
+					// Solve for x:
+					//
+					// x = - inv(A) * b'
+					//
+					var x = -b2Mul(vc.normalMass, b)
+
+					if (x.x >= 0.0 && x.y >= 0.0) {
+						// Get the incremental impulse
+						val d = x - a
+
+						// Apply incremental impulse
+						val P1 = normal * d.x
+						val P2 = normal * d.y
+
+						vA -= mA * (P1 + P2)
+						wA -= iA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2))
+
+						vB += mB * (P1 + P2)
+						wB += iB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2))
+
+						// Accumulate
+						cp1.normalImpulse = x.x
+						cp2.normalImpulse = x.y
+
+						// Postconditions
+						if (B2_DEBUG_SOLVER) {
+							dv1 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA)
+							dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA)
+
+							// Compute normal velocity
+							vn1 = dv1.dotProduct(normal)
+							vn2 = dv2.dotProduct(normal)
+
+							check((vn1 - cp1.velocityBias).absoluteValue < k_errorTol) { (vn1 - cp1.velocityBias).absoluteValue }
+							check((vn2 - cp2.velocityBias).absoluteValue < k_errorTol) { (vn2 - cp2.velocityBias).absoluteValue }
+						}
+
+						return@run
+					}
+
+					//
+					// Case 2: vn1 = 0 and x2 = 0
+					//
+					//   0 = a11 * x1 + a12 * 0 + b1'
+					// vn2 = a21 * x1 + a22 * 0 + b2'
+					//
+					x = Vector2d(x = -cp1.normalMass * b.x)
+
+					vn1 = 0.0
+					// vn2 = vc->K.ex.y * x.x + b.y;
+					vn2 = vc.K.m10 * x.x + b.y
+
+					if (x.x >= 0.0 && vn2 >= 0.0) {
+						// Get the incremental impulse
+						val d = x - a
+
+						// Apply incremental impulse
+						val P1 = d.x * normal
+						val P2 = d.y * normal
+
+						vA -= mA * (P1 + P2)
+						wA -= iA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2))
+
+						vB += mB * (P1 + P2)
+						wB += iB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2))
+
+						// Accumulate
+						cp1.normalImpulse = x.x
+						cp2.normalImpulse = x.y
+
+						if (B2_DEBUG_SOLVER) {
+							// Postconditions
+							dv1 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA)
+
+							// Compute normal velocity
+							vn1 = b2Dot(dv1, normal)
+
+							check((vn1 - cp1.velocityBias).absoluteValue < k_errorTol) { (vn1 - cp1.velocityBias).absoluteValue }
+						}
+
+						return@run
+					}
+
+					//
+					// Case 3: vn2 = 0 and x1 = 0
+					//
+					// vn1 = a11 * 0 + a12 * x2 + b1'
+					//   0 = a21 * 0 + a22 * x2 + b2'
+					//
+					x = Vector2d(y = -cp2.normalMass * b.y)
+					vn1 = vc.K.m01 * x.y + b.x
+					vn2 = 0.0
+
+					if (x.y >= 0.0 && vn1 >= 0.0) {
+						// Resubstitute for the incremental impulse
+						val d = x - a
+
+						// Apply incremental impulse
+						val P1 = d.x * normal
+						val P2 = d.y * normal
+
+						vA -= mA * (P1 + P2)
+						wA -= iA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2))
+
+						vB += mB * (P1 + P2)
+						wB += iB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2))
+
+						// Accumulate
+						cp1.normalImpulse = x.x
+						cp2.normalImpulse = x.y
+
+						if (B2_DEBUG_SOLVER) {
+							// Postconditions
+							dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA)
+
+							// Compute normal velocity
+							vn2 = b2Dot(dv2, normal)
+
+							check((vn2 - cp2.velocityBias).absoluteValue < k_errorTol) { (vn2 - cp2.velocityBias).absoluteValue }
+						}
+
+						return@run
+					}
+
+					//
+					// Case 4: x1 = 0 and x2 = 0
+					//
+					// vn1 = b1
+					// vn2 = b2;
+					x = Vector2d.ZERO
+					vn1 = b.x
+					vn2 = b.y
+
+					if (vn1 >= 0.0 && vn2 >= 0.0) {
+						// Resubstitute for the incremental impulse
+						val d = x - a
+
+						// Apply incremental impulse
+						val P1 = d.x * normal
+						val P2 = d.y * normal
+
+						vA -= mA * (P1 + P2)
+						wA -= iA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2))
+
+						vB += mB * (P1 + P2)
+						wB += iB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2))
+
+						// Accumulate
+						cp1.normalImpulse = x.x
+						cp2.normalImpulse = x.y
+
+						return@run
+					}
+
+					// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
+				}
+			}
+
+			velocities[indexA].v = vA
+			velocities[indexA].w = wA
+			velocities[indexB].v = vB
+			velocities[indexB].w = wB
+		}
+	}
+
+	fun storeImpulses() {
+		for (vc in velocityConstraints) {
+			val manifold = contacts[vc.contactIndex].manifold
+
+			for ((j, point) in vc.points.withIndex()) {
+				manifold.points[j].normalImpulse = point.normalImpulse
+				manifold.points[j].tangentImpulse = point.tangentImpulse
+			}
+		}
+	}
+
+	/**
+	 * Sequential solver.
+ 	 */
+	fun solvePositionConstraints(): Boolean {
+		var minSeparation = 0.0
+
+		for (pc in positionConstraints) {
+			val indexA = pc.indexA
+			val indexB = pc.indexB
+			val localCenterA = pc.localCenterA
+			val mA = pc.invMassA
+			val iA = pc.invIA
+			val localCenterB = pc.localCenterB
+			val mB = pc.invMassB
+			val iB = pc.invIB
+
+			var cA = positions[indexA].c
+			var aA = positions[indexA].a
+
+			var cB = positions[indexB].c
+			var aB = positions[indexB].a
+
+			// Solve normal constraints
+			for (j in 0 until pc.localPoints.size) {
+				val xfA = Transform()
+				val xfB = Transform()
+
+				xfA.q.set(aA)
+				xfB.q.set(aB)
+
+				xfA.p = cA - b2Mul(xfA.q, localCenterA)
+				xfB.p = cB - b2Mul(xfB.q, localCenterB)
+
+				val (normal, point, separation) = PositionSolverManifold(pc, xfA, xfB, j)
+
+				val rA = point - cA
+				val rB = point - cB
+
+				// Track max constraint error.
+				minSeparation = b2Min(minSeparation, separation)
+
+				// Prevent large corrections and allow slop.
+				val C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0)
+
+				// Compute the effective mass.
+				val rnA = b2Cross(rA, normal)
+				val rnB = b2Cross(rB, normal)
+				val K = mA + mB + iA * rnA * rnA + iB * rnB * rnB
+
+				// Compute normal impulse
+				val impulse = if (K > 0.0) -C / K else 0.0
+
+				val P = impulse * normal
+
+				cA -= mA * P
+				aA -= iA * b2Cross(rA, P)
+
+				cB += mB * P
+				aB += iB * b2Cross(rB, P)
+			}
+
+			positions[indexA].c = cA
+			positions[indexA].a = aA
+
+			positions[indexB].c = cB
+			positions[indexB].a = aB
+		}
+
+		// We can't expect minSpeparation >= -b2_linearSlop because we don't
+		// push the separation above -b2_linearSlop.
+		return minSeparation >= -3.0f * b2_linearSlop
+	}
+
+	/**
+	 * Sequential position solver for position constraints.
+	 */
+	fun solveTOIPositionConstraints(toiIndexA: Int, toiIndexB: Int): Boolean {
+		var minSeparation = 0.0
+
+		for (pc in positionConstraints) {
+			val indexA = pc.indexA
+			val indexB = pc.indexB
+			val localCenterA = pc.localCenterA
+			val localCenterB = pc.localCenterB
+
+			var mA = 0.0
+			var iA = 0.0
+
+			if (indexA == toiIndexA || indexA == toiIndexB) {
+				mA = pc.invMassA
+				iA = pc.invIA
+			}
+
+			var mB = 0.0
+			var iB = 0.0
+
+			if (indexB == toiIndexA || indexB == toiIndexB) {
+				mB = pc.invMassB
+				iB = pc.invIB
+			}
+
+			var cA = positions[indexA].c
+			var aA = positions[indexA].a
+
+			var cB = positions[indexB].c
+			var aB = positions[indexB].a
+
+			// Solve normal constraints
+			for (j in 0 until pc.localPoints.size) {
+				val xfA = Transform()
+				val xfB = Transform()
+
+				xfA.q.set(aA)
+				xfB.q.set(aB)
+
+				xfA.p = cA - b2Mul(xfA.q, localCenterA)
+				xfB.p = cB - b2Mul(xfB.q, localCenterB)
+
+				val (normal, point, separation) = PositionSolverManifold(pc, xfA, xfB, j)
+
+				val rA = point - cA
+				val rB = point - cB
+
+				// Track max constraint error.
+				minSeparation = b2Min(minSeparation, separation)
+
+				// Prevent large corrections and allow slop.
+				val C = b2Clamp(b2_toiBaumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0)
+
+				// Compute the effective mass.
+				val rnA = b2Cross(rA, normal)
+				val rnB = b2Cross(rB, normal)
+				val K = mA + mB + iA * rnA * rnA + iB * rnB * rnB
+
+				// Compute normal impulse
+				val impulse = if (K > 0.0) -C / K else 0.0
+
+				val P = impulse * normal
+
+				cA -= mA * P
+				aA -= iA * b2Cross(rA, P)
+
+				cB += mB * P
+				aB += iB * b2Cross(rB, P)
+			}
+
+			positions[indexA].c = cA
+			positions[indexA].a = aA
+
+			positions[indexB].c = cB
+			positions[indexB].a = aB
+		}
+
+		// We can't expect minSpeparation >= -b2_linearSlop because we don't
+		// push the separation above -b2_linearSlop.
+		return minSeparation >= -1.5f * b2_linearSlop
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/Island.kt

@@ -0,0 +1,480 @@
+package ru.dbotthepony.kbox2d.dynamics.internal
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.collision.DistanceProxy
+import ru.dbotthepony.kbox2d.collision.b2Distance
+import ru.dbotthepony.kbox2d.dynamics.contact.AbstractContact
+import ru.dbotthepony.kbox2d.dynamics.joint.AbstractJoint
+import ru.dbotthepony.kbox2d.dynamics.Body
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+import java.util.*
+import kotlin.collections.ArrayList
+import kotlin.math.absoluteValue
+
+private const val linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance
+private const val angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance
+
+private const val checkPositions = false
+
+/*
+Position Correction Notes
+=========================
+I tried the several algorithms for position correction of the 2D revolute joint.
+I looked at these systems:
+- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
+- suspension bridge with 30 1m long planks of length 1m.
+- multi-link chain with 30 1m long links.
+
+Here are the algorithms:
+
+Baumgarte - A fraction of the position error is added to the velocity error. There is no
+separate position solver.
+
+Pseudo Velocities - After the velocity solver and position integration,
+the position error, Jacobian, and effective mass are recomputed. Then
+the velocity constraints are solved with pseudo velocities and a fraction
+of the position error is added to the pseudo velocity error. The pseudo
+velocities are initialized to zero and there is no warm-starting. After
+the position solver, the pseudo velocities are added to the positions.
+This is also called the First Order World method or the Position LCP method.
+
+Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
+position error is re-computed for each constraint and the positions are updated
+after the constraint is solved. The radius vectors (aka Jacobians) are
+re-computed too (otherwise the algorithm has horrible instability). The pseudo
+velocity states are not needed because they are effectively zero at the beginning
+of each iteration. Since we have the current position error, we allow the
+iterations to terminate early if the error becomes smaller than b2_linearSlop.
+
+Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
+each time a constraint is solved.
+
+Here are the results:
+Baumgarte - this is the cheapest algorithm but it has some stability problems,
+especially with the bridge. The chain links separate easily close to the root
+and they jitter as they struggle to pull together. This is one of the most common
+methods in the field. The big drawback is that the position correction artificially
+affects the momentum, thus leading to instabilities and false bounce. I used a
+bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
+factor makes joints and contacts more spongy.
+
+Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
+stable. However, joints still separate with large angular velocities. Drag the
+simple pendulum in a circle quickly and the joint will separate. The chain separates
+easily and does not recover. I used a bias factor of 0.2. A larger value lead to
+the bridge collapsing when a heavy cube drops on it.
+
+Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
+Velocities, but in other ways it is worse. The bridge and chain are much more
+stable, but the simple pendulum goes unstable at high angular velocities.
+
+Full NGS - stable in all tests. The joints display good stiffness. The bridge
+still sags, but this is better than infinite forces.
+
+Recommendations
+Pseudo Velocities are not really worthwhile because the bridge and chain cannot
+recover from joint separation. In other cases the benefit over Baumgarte is small.
+
+Modified NGS is not a robust method for the revolute joint due to the violent
+instability seen in the simple pendulum. Perhaps it is viable with other constraint
+types, especially scalar constraints where the effective mass is a scalar.
+
+This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
+and is very fast. I don't think we can escape Baumgarte, especially in highly
+demanding cases where high constraint fidelity is not needed.
+
+Full NGS is robust and easy on the eyes. I recommend this as an option for
+higher fidelity simulation and certainly for suspension bridges and long chains.
+Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
+joint separation can be problematic. The number of NGS iterations can be reduced
+for better performance without harming robustness much.
+
+Each joint in a can be handled differently in the position solver. So I recommend
+a system where the user can select the algorithm on a per joint basis. I would
+probably default to the slower Full NGS and let the user select the faster
+Baumgarte method in performance critical scenarios.
+*/
+
+/*
+Cache Performance
+
+The Box2D solvers are dominated by cache misses. Data structures are designed
+to increase the number of cache hits. Much of misses are due to random access
+to body data. The constraint structures are iterated over linearly, which leads
+to few cache misses.
+
+The bodies are not accessed during iteration. Instead read only data, such as
+the mass values are stored with the constraints. The mutable data are the constraint
+impulses and the bodies velocities/positions. The impulses are held inside the
+constraint structures. The body velocities/positions are held in compact, temporary
+arrays to increase the number of cache hits. Linear and angular velocity are
+stored in a single array since multiple arrays lead to multiple misses.
+*/
+
+/*
+2D Rotation
+
+R = [cos(theta) -sin(theta)]
+    [sin(theta) cos(theta) ]
+
+thetaDot = omega
+
+Let q1 = cos(theta), q2 = sin(theta).
+R = [q1 -q2]
+    [q2  q1]
+
+q1Dot = -thetaDot * q2
+q2Dot = thetaDot * q1
+
+q1_new = q1_old - dt * w * q2
+q2_new = q2_old + dt * w * q1
+then normalize.
+
+This might be faster than computing sin+cos.
+However, we can compute sin+cos of the same angle fast.
+*/
+
+/**
+ * This is an internal class.
+ */
+internal class Island(
+	initialBodyCapacity: Int = 0,
+	initialContactCapacity: Int = 0,
+	initialJointCapacity: Int = 0,
+	val listener: IContactListener? = null
+) {
+	private val bodies = ArrayList<Body>(initialBodyCapacity)
+	private val contacts = ArrayList<AbstractContact>(initialContactCapacity)
+	private val joints = ArrayList<AbstractJoint>(initialJointCapacity)
+
+	private val velocities = ArrayList<ru.dbotthepony.kbox2d.api.B2Velocity>(initialBodyCapacity)
+	private val positions = ArrayList<ru.dbotthepony.kbox2d.api.B2Position>(initialBodyCapacity)
+
+	val bodiesAccess: List<Body> = Collections.unmodifiableList(bodies)
+
+	fun clear() {
+		bodies.clear()
+		contacts.clear()
+		joints.clear()
+
+		velocities.clear()
+		positions.clear()
+	}
+
+	fun add(body: Body) {
+		body.islandIndex = bodies.size
+		bodies.add(body)
+		velocities.add(ru.dbotthepony.kbox2d.api.B2Velocity())
+		positions.add(ru.dbotthepony.kbox2d.api.B2Position())
+	}
+
+	fun add(contact: AbstractContact) {
+		contacts.add(contact)
+	}
+
+	fun add(joint: AbstractJoint) {
+		joints.add(joint)
+	}
+
+	fun solve(profile: ru.dbotthepony.kbox2d.api.ProfileData, step: ru.dbotthepony.kbox2d.api.B2TimeStep, gravity: Vector2d, allowSleep: Boolean) {
+		val h = step.dt
+
+		// Integrate velocities and apply damping. Initialize the body state.
+		for ((i, body) in bodies.withIndex()) {
+			val c = body.sweep.c
+			val a = body.sweep.a
+			var v = body.linearVelocity
+			var w = body.angularVelocity
+
+			// Store positions for continuous collision.
+			body.sweep.c0 = body.sweep.c
+			body.sweep.a0 = body.sweep.a
+
+			if (body.type == ru.dbotthepony.kbox2d.api.BodyType.DYNAMIC) {
+				// Integrate velocities.
+				v += (gravity * body.gravityScale * body.mass + body.force) * body.invMass * h
+				w += h * body.rotInertiaInv * body.torque
+
+				// Apply damping.
+				// ODE: dv/dt + c * v = 0
+				// Solution: v(t) = v0 * exp(-c * t)
+				// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
+				// v2 = exp(-c * dt) * v1
+				// Pade approximation:
+				// v2 = v1 * 1 / (1 + c * dt)
+				v *= 1.0 / (1.0 + h * body.linearDamping)
+				w *= 1.0 / (1.0 + h * body.angularDamping)
+			}
+
+			positions[i].c = c
+			positions[i].a = a
+			velocities[i].v = v
+			velocities[i].w = w
+		}
+
+		var timer = System.nanoTime()
+
+		// Solver data
+		val solverData = ru.dbotthepony.kbox2d.api.B2SolverData(
+			step = step,
+			positions = positions,
+			velocities = velocities
+		)
+
+		// Initialize velocity constraints.
+		val contactSolver = ContactSolver(
+			step = step,
+			contacts = contacts,
+			positions = positions,
+			velocities = velocities,
+		)
+
+		contactSolver.initializeVelocityConstraints()
+
+		if (step.warmStarting) {
+			contactSolver.warmStart()
+		}
+
+		for (joint in joints) {
+			joint.initVelocityConstraints(solverData)
+		}
+
+		profile.solveInit = System.nanoTime() - timer
+		timer = System.nanoTime()
+
+		// Solve velocity constraints
+		for (i in 0 until step.velocityIterations) {
+			for (joint in joints) {
+				joint.solveVelocityConstraints(solverData)
+			}
+
+			contactSolver.solveVelocityConstraints()
+		}
+
+		// Store impulses for warm starting
+		contactSolver.storeImpulses()
+		profile.solveVelocity = System.nanoTime() - timer
+		timer = System.nanoTime()
+
+		// Integrate positions
+		for (i in bodies.indices) {
+			var c = positions[i].c
+			var a = positions[i].a
+			var v = velocities[i].v
+			var w = velocities[i].w
+
+			// Check for large velocities
+			val translation = h * v
+			if (translation.dotProduct(translation) > b2_maxTranslationSquared) {
+				v *= b2_maxTranslation / translation.length
+			}
+
+			val rotation = h * w
+			if (rotation * rotation > b2_maxRotationSquared) {
+				w *= b2_maxRotation / rotation.absoluteValue
+			}
+
+			// Integrate
+			c += h * v
+			a += h * w
+
+			positions[i].c = c
+			positions[i].a = a
+			velocities[i].v = v
+			velocities[i].w = w
+		}
+
+		profile.integratePositions = System.nanoTime() - timer
+		timer = System.nanoTime()
+
+		// Solve position constraints
+		var positionSolved = false
+
+		for (i in 0 until step.positionIterations) {
+			val contactsOkay = contactSolver.solvePositionConstraints()
+			var jointsOkay = true
+
+			for (joint in joints) {
+				val jointOkay = joint.solvePositionConstraints(solverData)
+				jointsOkay = jointsOkay && jointOkay
+			}
+
+			if (contactsOkay && jointsOkay) {
+				// Exit early if the position errors are small.
+				positionSolved = true
+				break
+			}
+		}
+
+		// Copy state buffers back to the bodies
+		for ((i, body) in bodies.withIndex()) {
+			body.sweep.c = positions[i].c
+			body.sweep.a = positions[i].a
+			body.linearVelocity = velocities[i].v
+			body.angularVelocity = velocities[i].w
+			body.synchronizeTransform()
+		}
+
+		profile.solvePosition = System.nanoTime() - timer
+
+		report(contactSolver.velocityConstraints)
+
+		if (allowSleep) {
+			var minSleepTime = Double.MAX_VALUE
+
+			for (body in bodies) {
+				if (body.type == ru.dbotthepony.kbox2d.api.BodyType.STATIC)
+					continue
+
+				if (
+					!body.allowAutoSleep ||
+					body.angularVelocity * body.angularVelocity > angTolSqr ||
+					body.linearVelocity.dotProduct(body.linearVelocity) > linTolSqr
+				) {
+					body.sleepTime = 0.0
+					minSleepTime = 0.0
+				} else {
+					body.sleepTime += h
+					minSleepTime = minSleepTime.coerceAtMost(body.sleepTime)
+				}
+			}
+
+			if (minSleepTime >= b2_timeToSleep && positionSolved) {
+				for (body in bodies) {
+					body.isAwake = false
+				}
+			}
+		}
+	}
+
+	fun solveTOI(subStep: ru.dbotthepony.kbox2d.api.B2TimeStep, toiIndexA: Int, toiIndexB: Int) {
+		check(toiIndexA < bodies.size) { "$toiIndexA >= ${bodies.size}" }
+		check(toiIndexB < bodies.size) { "$toiIndexB >= ${bodies.size}" }
+
+		// Initialize the body state.
+		for ((i, body) in bodies.withIndex()) {
+			positions[i].c = body.sweep.c
+			positions[i].a = body.sweep.a
+			velocities[i].v = body.linearVelocity
+			velocities[i].w = body.angularVelocity
+		}
+
+		val contactSolver = ContactSolver(
+			contacts = contacts,
+			step = subStep,
+			positions = positions,
+			velocities = velocities,
+		)
+
+		// Solve position constraints.
+		for (i in 0 until subStep.positionIterations) {
+			val contactsOkay = contactSolver.solveTOIPositionConstraints(toiIndexA, toiIndexB)
+
+			if (contactsOkay) {
+				break
+			}
+		}
+
+		// Is the new position really safe?
+		if (checkPositions) {
+			for (c in contacts) {
+				val fA = c.fixtureA
+				val fB = c.fixtureB
+
+				val bA = fA.body!!
+				val bB = fB.body!!
+
+				val indexA = c.childIndexA
+				val indexB = c.childIndexB
+
+				val cache = SimplexCache()
+
+				val output = b2Distance(
+					cache = cache,
+					proxyA = DistanceProxy(fA.shape, indexA),
+					proxyB = DistanceProxy(fB.shape, indexB),
+					transformA = bA.transform,
+					transformB = bB.transform,
+				)
+
+				if (output.distance == 0.0 || output.newCache.count == 3) {
+					// cache.count += 0;
+					// doesn't make much sense
+				}
+			}
+		}
+
+		// Leap of faith to new safe state.
+		bodies[toiIndexA].sweep.c0 = positions[toiIndexA].c
+		bodies[toiIndexA].sweep.a0 = positions[toiIndexA].a
+		bodies[toiIndexB].sweep.c0 = positions[toiIndexB].c
+		bodies[toiIndexB].sweep.a0 = positions[toiIndexB].a
+
+		// No warm starting is needed for TOI events because warm
+		// starting impulses were applied in the discrete solver.
+		contactSolver.initializeVelocityConstraints()
+
+		// Solve velocity constraints.
+		for (i in 0 until subStep.velocityIterations) {
+			contactSolver.solveVelocityConstraints()
+		}
+
+		// Don't store the TOI contact forces for warm starting
+		// because they can be quite large.
+
+		val h = subStep.dt
+
+		// Integrate positions
+		for ((i, body) in bodies.withIndex()) {
+			var c = positions[i].c
+			var a = positions[i].a
+			var v = velocities[i].v
+			var w = velocities[i].w
+
+			// Check for large velocities
+			val translation = h * v
+			if (translation.dotProduct(translation) > b2_maxTranslationSquared) {
+				v *= b2_maxTranslation / translation.length
+			}
+
+			val rotation = h * w
+			if (rotation * rotation > b2_maxRotationSquared) {
+				w *= b2_maxRotation / rotation.absoluteValue
+			}
+
+			// Integrate
+			c += h * v
+			a += h * w
+
+			positions[i].c = c
+			positions[i].a = a
+			velocities[i].v = v
+			velocities[i].w = w
+
+			// Sync bodies
+			body.sweep.c = c
+			body.sweep.a = a
+			body.linearVelocity = v
+			body.angularVelocity = w
+			body.synchronizeTransform()
+		}
+
+		report(contactSolver.velocityConstraints)
+	}
+
+	fun report(constraints: List<ContactVelocityConstraint>) {
+		val listener = listener ?: return
+
+		for ((i, contact) in contacts.withIndex()) {
+			val vc = constraints[i]
+
+			val impulse = ContactImpulse(
+				normalImpulses = DoubleArray(vc.points.size) { vc.points[it].normalImpulse },
+				tangentImpulses = DoubleArray(vc.points.size) { vc.points[it].tangentImpulse },
+			)
+
+			listener.postSolve(contact, impulse)
+		}
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/AbstractJoint.kt

@@ -0,0 +1,144 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import it.unimi.dsi.fastutil.objects.Object2ObjectArrayMap
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.dynamics.Body
+import ru.dbotthepony.kstarbound.math.Vector2d
+import kotlin.math.PI
+
+fun interface JointFactory {
+	fun factorize(jointDef: IJointDef): AbstractJoint
+}
+
+sealed class AbstractJoint(def: IJointDef) : IJoint {
+	init {
+		require(def.bodyA != def.bodyB) { "Tried to create join on same body" }
+	}
+
+	internal var isOnIsland = false
+
+	protected var index: Int = 0
+
+	override val collideConnected: Boolean = def.collideConnected
+	override val type: JointType = def.type
+	override var userData: Any? = def.userData
+
+	// KBox2D: In original code, nothing expects bodies to be null
+	// but certain joints (notably, mouse joint) have no meaningful
+	// value for second body involved. So, KBox2D CAN handle case where
+	// joint have only one body, and to avoid null assertions in all places
+	// possible, bodyA and bodyB getters assert for null by themselves
+	// However, there is nullable getter for bodies: nullableBodyA and nullableBodyB
+	protected var _bodyA: Body? = def.bodyA as Body?
+	protected var _bodyB: Body? = def.bodyB as Body?
+
+	final override val bodyA: Body get() = checkNotNull(_bodyA) { "Body A is not present" }
+	final override val bodyB: Body get() = checkNotNull(_bodyB) { "Body B is not present" }
+
+	val hasBodyA: Boolean get() = _bodyA != null
+	val hasBodyB: Boolean get() = _bodyB != null
+	val hasTwoBodies: Boolean get() = hasBodyA && hasBodyB
+
+	val nullableBodyA get() = _bodyA
+	val nullableBodyB get() = _bodyB
+
+	private var _edgeA: JointEdge? = JointEdge(other = _bodyB, joint = this, next = _bodyA?.jointList)
+	private var _edgeB: JointEdge? = JointEdge(other = _bodyA, joint = this, next = _bodyB?.jointList)
+
+	internal val edgeA: JointEdge get() = checkNotNull(_edgeA) { "Edge A is not present" }
+	internal val edgeB: JointEdge get() = checkNotNull(_edgeB) { "Edge B is not present" }
+
+	var isValid: Boolean = true
+		private set
+
+	init {
+		// Connect to the bodies' doubly linked lists.
+		_bodyA?.jointList?.prev = edgeA
+		_bodyA?.jointList = edgeA
+		_bodyB?.jointList?.prev = edgeB
+		_bodyB?.jointList = edgeB
+	}
+
+	final override var next: IJoint? = null
+		internal set
+	final override var prev: IJoint? = null
+		internal set
+
+	/**
+	 * Signals that this joint was destroyed, invalidate stuff
+	 * to fail-fast this object
+	 */
+	internal open fun unlink() {
+		_edgeA = null
+		_edgeB = null
+		_bodyA = null
+		_bodyB = null
+		isValid = false
+	}
+
+	internal abstract fun initVelocityConstraints(data: B2SolverData)
+	internal abstract fun solveVelocityConstraints(data: B2SolverData)
+	internal abstract fun solvePositionConstraints(data: B2SolverData): Boolean
+
+	override fun shiftOrigin(newOrigin: Vector2d) {
+		// no-op
+	}
+
+	companion object {
+		private val registry = Object2ObjectArrayMap<JointType, JointFactory>()
+
+		internal fun register(jointType: JointType, factory: JointFactory) {
+			require(registry.put(jointType, factory) == null) { "Re-registered $jointType factory" }
+		}
+
+		internal fun create(jointType: JointType, jointDef: IJointDef): AbstractJoint {
+			return requireNotNull(registry[jointType]) { "No joint factory registered for type $jointType" }.factorize(jointDef)
+		}
+
+		internal fun create(jointDef: IJointDef): AbstractJoint {
+			return create(jointDef.type, jointDef)
+		}
+
+		init {
+			register(JointType.DISTANCE) {
+				return@register DistanceJoint(it as DistanceJointDef)
+			}
+
+			register(JointType.REVOLUTE) {
+				return@register RevoluteJoint(it as RevoluteJointDef)
+			}
+
+			register(JointType.PRISMATIC) {
+				return@register PrismaticJoint(it as PrismaticJointDef)
+			}
+
+			register(JointType.PULLEY) {
+				return@register PulleyJoint(it as PulleyJointDef)
+			}
+
+			register(JointType.GEAR) {
+				return@register GearJoint(it as GearJointDef)
+			}
+
+			register(JointType.MOUSE) {
+				return@register MouseJoint(it as MouseJointDef)
+			}
+
+			register(JointType.WHEEL) {
+				return@register WheelJoint(it as WheelJointDef)
+			}
+
+			register(JointType.WELD) {
+				return@register WeldJoint(it as WeldJointDef)
+			}
+
+			register(JointType.FRICTION) {
+				return@register FrictionJoint(it as FrictionJointDef)
+			}
+
+			register(JointType.MOTOR) {
+				return@register MotorJoint(it as MotorJointDef)
+			}
+		}
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/DistanceJoint.kt

@@ -0,0 +1,336 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.B2SolverData
+import ru.dbotthepony.kbox2d.api.b2Max
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kstarbound.util.Color
+
+class DistanceJoint(def: DistanceJointDef) : AbstractJoint(def) {
+	var stiffness: Double = def.stiffness
+	var damping: Double = def.damping
+
+	var length: Double = b2Max(def.length, b2_linearSlop)
+		set(value) {
+			impulse = 0.0
+			field = b2Max(value, b2_linearSlop)
+		}
+
+	var minLength: Double = b2Max(def.minLength, b2_linearSlop)
+		set(value) {
+			lowerImpulse = 0.0
+			field = b2Clamp(value, b2_linearSlop, maxLength)
+		}
+
+	var maxLength: Double = b2Max(def.maxLength, b2_linearSlop)
+		set(value) {
+			upperImpulse = 0.0
+			field = b2Max(value, minLength)
+		}
+
+	val currentLength: Double get() {
+		val pA = bodyA.getWorldPoint(localAnchorA)
+		val pB = bodyB.getWorldPoint(localAnchorB)
+		val d = pB - pA
+		return d.length
+	}
+
+	private var bias: Double = 0.0
+
+	// Solver shared
+	private val localAnchorA: Vector2d = def.localAnchorA
+	private val localAnchorB: Vector2d = def.localAnchorB
+	private var gamma: Double = 0.0
+	private var impulse: Double = 0.0
+	private var lowerImpulse: Double = 0.0
+	private var upperImpulse: Double = 0.0
+	private var _currentLength: Double = 0.0
+
+	// Solver temp
+	private var indexA: Int = 0
+	private var indexB: Int = 0
+	private var u: Vector2d = Vector2d.ZERO
+	private var rA: Vector2d = Vector2d.ZERO
+	private var rB: Vector2d = Vector2d.ZERO
+	private var localCenterA: Vector2d = Vector2d.ZERO
+	private var localCenterB: Vector2d = Vector2d.ZERO
+	private var invMassA: Double = 0.0
+	private var invMassB: Double = 0.0
+	private var invIA: Double = 0.0
+	private var invIB: Double = 0.0
+	private var softMass: Double = 0.0
+	private var mass: Double = 0.0
+
+	override fun initVelocityConstraints(data: B2SolverData) {
+		indexA = bodyA.islandIndex
+		indexB = bodyB.islandIndex
+		invMassA = bodyA.invMass
+		invMassB = bodyB.invMass
+		invIA = bodyA.rotInertiaInv
+		invIB = bodyB.rotInertiaInv
+
+		localCenterA = bodyA.sweep.localCenter
+		localCenterB = bodyB.sweep.localCenter
+
+		val cA = data.positions[indexA].c
+		val aA = data.positions[indexA].a
+		var vA = data.velocities[indexA].v
+		var wA = data.velocities[indexA].w
+		val cB = data.positions[indexB].c
+		val aB = data.positions[indexB].a
+		var vB = data.velocities[indexB].v
+		var wB = data.velocities[indexB].w
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		rA = b2Mul(qA, localAnchorA - localCenterA)
+		rB = b2Mul(qB, localAnchorB - localCenterB)
+		u = cB + rB - cA - rA
+
+		// Handle singularity.
+		_currentLength = u.length
+
+		if (_currentLength > b2_linearSlop) {
+			u *= 1.0f / _currentLength
+		} else {
+			u = Vector2d.ZERO
+			mass = 0.0
+			impulse = 0.0
+			lowerImpulse = 0.0
+			upperImpulse = 0.0
+		}
+
+		val crAu = b2Cross(rA, u)
+		val crBu = b2Cross(rB, u)
+		var invMass = invMassA + invIA * crAu * crAu + invMassB + invIB * crBu * crBu
+		mass = if (invMass != 0.0) 1.0 / invMass else 0.0
+
+		if (stiffness > 0.0 && minLength < maxLength) {
+			// soft
+			val C = _currentLength - length
+
+			val d = damping
+			val k = stiffness
+
+			// magic formulas
+			val h = data.step.dt
+
+			// gamma = 1 / (h * (d + h * k))
+			// the extra factor of h in the denominator is since the lambda is an impulse, not a force
+			gamma = h * (d + h * k)
+			gamma = if (gamma != 0.0) 1.0 / gamma else 0.0
+			bias = C * h * k * gamma
+
+			invMass += gamma
+			softMass = if (invMass != 0.0) 1.0 / invMass else 0.0
+		} else {
+			// rigid
+			gamma = 0.0
+			bias = 0.0
+			softMass = mass
+		}
+
+		if (data.step.warmStarting) {
+			// Scale the impulse to support a variable time step.
+			impulse *= data.step.dtRatio
+			lowerImpulse *= data.step.dtRatio
+			upperImpulse *= data.step.dtRatio
+
+			val P = (impulse + lowerImpulse - upperImpulse) * u
+			vA -= invMassA * P
+			wA -= invIA * b2Cross(rA, P)
+			vB += invMassB * P
+			wB += invIB * b2Cross(rB, P)
+		} else {
+			impulse = 0.0
+		}
+
+		data.velocities[indexA].v = vA
+		data.velocities[indexA].w = wA
+		data.velocities[indexB].v = vB
+		data.velocities[indexB].w = wB
+	}
+
+	override fun solveVelocityConstraints(data: B2SolverData) {
+		var vA = data.velocities[indexA].v
+		var wA = data.velocities[indexA].w
+		var vB = data.velocities[indexB].v
+		var wB = data.velocities[indexB].w
+
+		if (minLength < maxLength) {
+			if (stiffness > 0.0) {
+				// Cdot = dot(u, v + cross(w, r))
+				val vpA = vA + b2Cross(wA, rA)
+				val vpB = vB + b2Cross(wB, rB)
+				val Cdot = b2Dot(u, vpB - vpA)
+
+				var impulse = -softMass * (Cdot + bias + gamma * impulse)
+				impulse += impulse
+
+				val P = impulse * u
+				vA -= invMassA * P
+				wA -= invIA * b2Cross(rA, P)
+				vB += invMassB * P
+				wB += invIB * b2Cross(rB, P)
+			}
+
+			// lower
+			run {
+				val C = _currentLength - minLength
+				val bias = b2Max(0.0, C) * data.step.inv_dt
+
+				val vpA = vA + b2Cross(wA, rA)
+				val vpB = vB + b2Cross(wB, rB)
+				val Cdot = b2Dot(u, vpB - vpA)
+
+				var impulse = -mass * (Cdot + bias)
+				val oldImpulse = lowerImpulse
+				lowerImpulse = b2Max(0.0, lowerImpulse + impulse)
+				impulse = lowerImpulse - oldImpulse
+				val P = impulse * u
+
+				vA -= invMassA * P
+				wA -= invIA * b2Cross(rA, P)
+				vB += invMassB * P
+				wB += invIB * b2Cross(rB, P)
+			}
+
+			// upper
+			run {
+				val C = maxLength - _currentLength
+				val bias = b2Max(0.0, C) * data.step.inv_dt
+
+				val vpA = vA + b2Cross(wA, rA)
+				val vpB = vB + b2Cross(wB, rB)
+				val Cdot = b2Dot(u, vpA - vpB)
+
+				var impulse = -mass * (Cdot + bias)
+				val oldImpulse = upperImpulse
+				upperImpulse = b2Max(0.0, upperImpulse + impulse)
+				impulse = upperImpulse - oldImpulse
+				val P = -impulse * u
+
+				vA -= invMassA * P
+				wA -= invIA * b2Cross(rA, P)
+				vB += invMassB * P
+				wB += invIB * b2Cross(rB, P)
+			}
+		} else {
+			// Equal limits
+
+			// Cdot = dot(u, v + cross(w, r))
+			val vpA = vA + b2Cross(wA, rA)
+			val vpB = vB + b2Cross(wB, rB)
+			val Cdot = b2Dot(u, vpB - vpA)
+
+			var impulse = -mass * Cdot
+			impulse += impulse
+
+			val P = impulse * u
+			vA -= invMassA * P
+			wA -= invIA * b2Cross(rA, P)
+			vB += invMassB * P
+			wB += invIB * b2Cross(rB, P)
+		}
+
+		data.velocities[indexA].v = vA
+		data.velocities[indexA].w = wA
+		data.velocities[indexB].v = vB
+		data.velocities[indexB].w = wB
+	}
+
+	override fun solvePositionConstraints(data: B2SolverData): Boolean {
+		var cA = data.positions[indexA].c
+		var aA = data.positions[indexA].a
+		var cB = data.positions[indexB].c
+		var aB = data.positions[indexB].a
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		val rA = b2Mul(qA, localAnchorA - localCenterA)
+		val rB = b2Mul(qB, localAnchorB - localCenterB)
+		var u = cB + rB - cA - rA
+
+		u.isFiniteOrThrow {
+			"u is invalid, $cB, $rB, $cA, $rA"
+		}
+
+		val length = u.length
+		u = u.normalized
+		val C: Double
+
+		if (minLength == maxLength) {
+			C = length - minLength
+		} else if (length < minLength) {
+			C = length - minLength
+		} else if (maxLength < length) {
+			C = length - maxLength
+		} else {
+			return true
+		}
+
+		val impulse = -mass * C
+		val P = impulse * u
+
+		P.isFiniteOrThrow {
+			"P is not finite, impulse: $impulse, u: $u, mass: $mass, C: $C"
+		}
+
+		cA -= invMassA * P
+		aA -= invIA * b2Cross(rA, P)
+		cB += invMassB * P
+		aB += invIB * b2Cross(rB, P)
+
+		data.positions[indexA].c = cA
+		data.positions[indexA].a = aA
+		data.positions[indexB].c = cB
+		data.positions[indexB].a = aB
+
+		return b2Abs(C) < b2_linearSlop
+	}
+
+	override fun getReactionForce(inv_dt: Double): Vector2d {
+		return inv_dt * (impulse + lowerImpulse - upperImpulse) * u
+	}
+
+	override fun getReactionTorque(inv_dt: Double): Double {
+		return 0.0
+	}
+
+	override val anchorA: Vector2d get() = bodyA.getWorldPoint(localAnchorA)
+	override val anchorB: Vector2d get() = bodyB.getWorldPoint(localAnchorB)
+
+	override fun draw(draw: IDebugDraw) {
+		val pA = b2Mul(bodyA.transform, localAnchorA)
+		val pB = b2Mul(bodyB.transform, localAnchorB)
+
+		val axis = (pB - pA).normalized
+
+		draw.drawSegment(pA, pB, c4)
+
+		val pRest = pA + length * axis
+		draw.drawPoint(pRest, 8.0, c1)
+
+		if (minLength != maxLength) {
+			if (minLength > b2_linearSlop) {
+				val pMin = pA + minLength * axis
+				draw.drawPoint(pMin, 4.0, c2)
+			}
+
+			if (maxLength < Double.MAX_VALUE) {
+				val pMax = pA + maxLength * axis
+				draw.drawPoint(pMax, 4.0, c3)
+			}
+		}
+	}
+
+	companion object {
+		private val c1 = Color(0.7f, 0.7f, 0.7f)
+		private val c2 = Color(0.3f, 0.9f, 0.3f)
+		private val c3 = Color(0.9f, 0.3f, 0.3f)
+		private val c4 = Color(0.4f, 0.4f, 0.4f)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/FrictionJoint.kt

@@ -0,0 +1,210 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.B2SolverData
+import ru.dbotthepony.kbox2d.api.b2Mul
+import ru.dbotthepony.kstarbound.math.MutableMatrix2d
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+
+// Point-to-point constraint
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Angle constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+/**
+ * Friction joint. This is used for top-down friction.
+ * It provides 2D translational friction and angular friction.
+ */
+class FrictionJoint(def: FrictionJointDef) : AbstractJoint(def) {
+	val localAnchorA: Vector2d = def.localAnchorA
+	val localAnchorB: Vector2d = def.localAnchorB
+
+	// Solver shared
+	private var linearImpulse: Vector2d = Vector2d.ZERO
+	private var angularImpulse: Double = 0.0
+	var maxForce: Double = def.maxForce
+		set(value) {
+			require(!value.isNaN()) { "Tried to set NaN force" }
+			require(value.isFinite()) { "Tried to set infinite force" }
+			require(value >= 0.0) { "Tried to set negative force: $value" }
+			field = value
+		}
+
+	var maxTorque: Double = def.maxTorque
+		set(value) {
+			require(!value.isNaN()) { "Tried to set NaN torque" }
+			require(value.isFinite()) { "Tried to set infinite torque" }
+			require(value >= 0.0) { "Tried to set negative torque: $value" }
+			field = value
+		}
+
+
+	// Solver temp
+	private var indexA: Int = 0
+	private var indexB: Int = 0
+	private var rA: Vector2d = Vector2d.ZERO
+	private var rB: Vector2d = Vector2d.ZERO
+	private var localCenterA: Vector2d = Vector2d.ZERO
+	private var localCenterB: Vector2d = Vector2d.ZERO
+	private var invMassA: Double = 0.0
+	private var invMassB: Double = 0.0
+	private var invIA: Double = 0.0
+	private var invIB: Double = 0.0
+	private var linearMass: MutableMatrix2d = MutableMatrix2d()
+	private var angularMass: Double = 0.0
+
+	override fun initVelocityConstraints(data: B2SolverData) {
+		this.indexA = this.bodyA.islandIndex
+		this.indexB = this.bodyB.islandIndex
+		this.localCenterA = this.bodyA.sweep.localCenter
+		this.localCenterB = this.bodyB.sweep.localCenter
+		this.invMassA = this.bodyA.invMass
+		this.invMassB = this.bodyB.invMass
+		this.invIA = this.bodyA.invI
+		this.invIB = this.bodyB.invI
+
+		val aA = data.positions[this.indexA].a
+		var vA = data.velocities[this.indexA].v
+		var wA = data.velocities[this.indexA].w
+
+		val aB = data.positions[this.indexB].a
+		var vB = data.velocities[this.indexB].v
+		var wB = data.velocities[this.indexB].w
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		// Compute the effective mass matrix.
+		this.rA = b2Mul(qA, this.localAnchorA - this.localCenterA)
+		this.rB = b2Mul(qB, this.localAnchorB - this.localCenterB)
+
+		// J = [-I -r1_skew I r2_skew]
+		//     [ 0       -1 0       1]
+		// r_skew = [-ry; rx]
+
+		// Matlab
+		// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
+		//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
+		//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
+
+		val mA = this.invMassA
+		val mB = this.invMassB
+		val iA = this.invIA
+		val iB = this.invIB
+
+		val K = MutableMatrix2d()
+		K.m00 = mA + mB + iA * this.rA.y * this.rA.y + iB * this.rB.y * this.rB.y
+		K.m10 = -iA * this.rA.x * this.rA.y - iB * this.rB.x * this.rB.y
+		K.m01 = K.m10
+		K.m11 = mA + mB + iA * this.rA.x * this.rA.x + iB * this.rB.x * this.rB.x
+
+		this.linearMass = K.getInverse().asMutableMatrix()
+
+		this.angularMass = iA + iB
+		if (this.angularMass > 0.0) {
+			this.angularMass = 1.0 / this.angularMass
+		}
+
+		if (data.step.warmStarting) {
+			// Scale impulses to support a variable time step.
+			this.linearImpulse *= data.step.dtRatio
+			this.angularImpulse *= data.step.dtRatio
+
+			val P = Vector2d(this.linearImpulse.x, this.linearImpulse.y)
+			vA -= mA * P
+			wA -= iA * (b2Cross(this.rA, P) + this.angularImpulse)
+			vB += mB * P
+			wB += iB * (b2Cross(this.rB, P) + this.angularImpulse)
+		} else {
+			this.linearImpulse = Vector2d.ZERO
+			this.angularImpulse = 0.0
+		}
+
+		data.velocities[this.indexA].v = vA
+		data.velocities[this.indexA].w = wA
+		data.velocities[this.indexB].v = vB
+		data.velocities[this.indexB].w = wB
+	}
+
+	override fun solveVelocityConstraints(data: B2SolverData) {
+		var vA = data.velocities[this.indexA].v
+		var wA = data.velocities[this.indexA].w
+		var vB = data.velocities[this.indexB].v
+		var wB = data.velocities[this.indexB].w
+
+		val mA = this.invMassA
+		val mB = this.invMassB
+		val iA = this.invIA
+		val iB = this.invIB
+
+		val h = data.step.dt
+
+		// Solve angular friction
+		run {
+			val Cdot = wB - wA
+			var impulse = -this.angularMass * Cdot
+
+			val oldImpulse = this.angularImpulse
+			val maxImpulse = h * this.maxTorque
+			this.angularImpulse = b2Clamp(this.angularImpulse + impulse, -maxImpulse, maxImpulse)
+			impulse = this.angularImpulse - oldImpulse
+
+			wA -= iA * impulse
+			wB += iB * impulse
+		}
+
+		// Solve linear friction
+		run {
+			val Cdot = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA)
+
+			var impulse = -b2Mul(this.linearMass, Cdot)
+			val oldImpulse = this.linearImpulse
+			this.linearImpulse += impulse
+
+			val maxImpulse = h * this.maxForce
+
+			if (this.linearImpulse.lengthSquared > maxImpulse * maxImpulse) {
+				this.linearImpulse = this.linearImpulse.normalized
+				this.linearImpulse *= maxImpulse
+			}
+
+			impulse = this.linearImpulse - oldImpulse
+
+			vA -= mA * impulse
+			wA -= iA * b2Cross(this.rA, impulse)
+
+			vB += mB * impulse
+			wB += iB * b2Cross(this.rB, impulse)
+		}
+
+		data.velocities[this.indexA].v = vA
+		data.velocities[this.indexA].w = wA
+		data.velocities[this.indexB].v = vB
+		data.velocities[this.indexB].w = wB
+	}
+
+	override fun solvePositionConstraints(data: B2SolverData): Boolean {
+		return true
+	}
+
+	override val anchorA: Vector2d
+		get() = bodyA.getWorldPoint(localAnchorA)
+	override val anchorB: Vector2d
+		get() = bodyB.getWorldPoint(localAnchorB)
+
+	override fun getReactionForce(inv_dt: Double): Vector2d {
+		return inv_dt * linearImpulse
+	}
+
+	override fun getReactionTorque(inv_dt: Double): Double {
+		return inv_dt * angularImpulse
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/GearJoint.kt

@@ -0,0 +1,468 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.dynamics.Body
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+
+// Gear Joint:
+// C0 = (coordinate1 + ratio * coordinate2)_initial
+// C = (coordinate1 + ratio * coordinate2) - C0 = 0
+// J = [J1 ratio * J2]
+// K = J * invM * JT
+//   = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
+//
+// Revolute:
+// coordinate = rotation
+// Cdot = angularVelocity
+// J = [0 0 1]
+// K = J * invM * JT = invI
+//
+// Prismatic:
+// coordinate = dot(p - pg, ug)
+// Cdot = dot(v + cross(w, r), ug)
+// J = [ug cross(r, ug)]
+// K = J * invM * JT = invMass + invI * cross(r, ug)^2
+
+/**
+ * A gear joint is used to connect two joints together. Either joint
+ * can be a revolute or prismatic joint. You specify a gear ratio
+ * to bind the motions together:
+ * coordinate1 + ratio * coordinate2 = constant
+ * The ratio can be negative or positive. If one joint is a revolute joint
+ * and the other joint is a prismatic joint, then the ratio will have units
+ * of length or units of 1/length.
+ * @warning You have to manually destroy the gear joint if joint1 or joint2
+ * is destroyed.
+ */
+class GearJoint(def: GearJointDef) : AbstractJoint(def) {
+	// KBox2D: Due to multiple local variables names clashing with class fields
+	// this class contain hard reference to this in methods to clear confusion
+	init {
+		require(def.joint1 != def.joint2) { "Definition specify the same joint ${def.joint1}" }
+	}
+
+	/**
+	 * Get the first joint.
+	 */
+	val joint1: AbstractJoint = def.joint1
+
+	/**
+	 * Get the second joint.
+	 */
+	val joint2: AbstractJoint = def.joint2
+
+	val typeA: JointType = def.joint1.type
+	val typeB: JointType = def.joint2.type
+
+	init {
+		check(typeA == JointType.REVOLUTE || typeA == JointType.PRISMATIC) { "Invalid joint A: $typeA" }
+		check(typeB == JointType.REVOLUTE || typeB == JointType.PRISMATIC) { "Invalid joint B: $typeB" }
+	}
+
+	// Solver shared
+	private var localAnchorA: Vector2d
+	private var localAnchorB: Vector2d
+	private var localAnchorC: Vector2d
+	private var localAnchorD: Vector2d
+
+	private var localAxisC: Vector2d
+	private var localAxisD: Vector2d
+
+	private var referenceAngleA: Double
+	private var referenceAngleB: Double
+
+	private var constant: Double
+
+	/**
+	 * Set/Get the gear ratio.
+	 */
+	var ratio: Double = def.ratio
+		set(value) {
+			if (!value.isFinite()) {
+				throw IllegalArgumentException("Tried to set infinite ratio")
+			}
+
+			if (value.isNaN()) {
+				throw IllegalArgumentException("Tried to set NaN ratio")
+			}
+
+			field = value
+		}
+
+	private var tolerance: Double
+
+	private var impulse: Double = 0.0
+
+	// Solver temp
+	private var indexA: Int = 0
+	private var indexB: Int = 0
+	private var indexC: Int = 0
+	private var indexD: Int = 0
+
+	private var lcA: Vector2d = Vector2d.ZERO
+	private var lcB: Vector2d = Vector2d.ZERO
+	private var lcC: Vector2d = Vector2d.ZERO
+	private var lcD: Vector2d = Vector2d.ZERO
+
+	private var mA: Double = 0.0
+	private var mB: Double = 0.0
+	private var mC: Double = 0.0
+	private var mD: Double = 0.0
+	private var iA: Double = 0.0
+	private var iB: Double = 0.0
+	private var iC: Double = 0.0
+	private var iD: Double = 0.0
+
+	private var JvAC: Vector2d = Vector2d.ZERO
+	private var JvBD: Vector2d = Vector2d.ZERO
+
+	private var JwA: Double = 0.0
+	private var JwB: Double = 0.0
+	private var JwC: Double = 0.0
+	private var JwD: Double = 0.0
+
+	private var mass: Double = 0.0
+
+	// Body A is connected to body C
+	// Body B is connected to body D
+	private var _bodyC: Body? = joint1.bodyA
+	private var _bodyD: Body?
+
+	override fun unlink() {
+		super.unlink()
+		_bodyC = null
+		_bodyD = null
+	}
+
+	val bodyC: Body get() = checkNotNull(_bodyC) { "Body C is not present" }
+	val bodyD: Body get() = checkNotNull(_bodyD) { "Body D is not present" }
+
+	init {
+		_bodyA = joint1.bodyB
+
+		val coordinateA: Double
+		val coordinateB: Double
+
+		// Body B on joint1 must be dynamic
+		check(bodyA.type == BodyType.DYNAMIC) { "Body A is expected to be DYNAMIC, hot ${bodyA.type}" }
+
+		// Get geometry of joint1
+		val xfA = bodyA.transform
+		val aA = bodyA.sweep.a
+		val xfC = bodyC.transform
+		val aC = bodyC.sweep.a
+
+		if (typeA == JointType.REVOLUTE) {
+			val revolute = def.joint1 as RevoluteJoint
+
+			localAnchorC = revolute.localAnchorA
+			localAnchorA = revolute.localAnchorB
+			referenceAngleA = revolute.referenceAngle
+			localAxisC = Vector2d.ZERO
+
+			coordinateA = aA - aC - referenceAngleA
+
+			// position error is measured in radians
+			tolerance = b2_angularSlop
+		} else {
+			val prismatic = def.joint1 as PrismaticJoint
+			localAnchorC = prismatic.localAnchorA
+			localAnchorA = prismatic.localAnchorB
+			referenceAngleA = prismatic.referenceAngle
+			localAxisC = prismatic.localXAxisA
+
+			val pC = localAnchorC
+			val pA = b2MulT(xfC.q, b2Mul(xfA.q, localAnchorA) + (xfA.p - xfC.p))
+			coordinateA = b2Dot(pA - pC, localAxisC)
+
+			// position error is measured in meters
+			tolerance = b2_linearSlop
+		}
+
+		_bodyD = joint2.bodyA
+		_bodyB = joint2.bodyB
+
+		// Body B on joint2 must be dynamic
+		check(bodyB.type == BodyType.DYNAMIC) { "Body is expected to be DYNAMIC, got ${bodyB.type}"}
+
+		// Get geometry of joint2
+		val xfB = bodyB.transform
+		val aB = bodyB.sweep.a
+		val xfD = bodyD.transform
+		val aD = bodyD.sweep.a
+
+		if (typeB == JointType.REVOLUTE) {
+			val revolute = def.joint2 as RevoluteJoint
+			localAnchorD = revolute.localAnchorA
+			localAnchorB = revolute.localAnchorB
+			referenceAngleB = revolute.referenceAngle
+			localAxisD = Vector2d.ZERO
+
+			coordinateB = aB - aD - referenceAngleB
+		} else {
+			val prismatic = def.joint2 as PrismaticJoint
+			localAnchorD = prismatic.localAnchorA
+			localAnchorB = prismatic.localAnchorB
+			referenceAngleB = prismatic.referenceAngle
+			localAxisD = prismatic.localXAxisA
+
+			val pD = localAnchorD
+			val pB = b2MulT(xfD.q, b2Mul(xfB.q, localAnchorB) + (xfB.p - xfD.p))
+			coordinateB = b2Dot(pB - pD, localAxisD)
+		}
+
+		constant = coordinateA + ratio * coordinateB
+	}
+
+	override fun initVelocityConstraints(data: B2SolverData) {
+		if (!joint1.isValid || !joint2.isValid) {
+			throw IllegalStateException("$this is orphaned!")
+		}
+
+		this.indexA = this.bodyA.islandIndex
+		this.indexB = this.bodyB.islandIndex
+		this.indexC = this.bodyC.islandIndex
+		this.indexD = this.bodyD.islandIndex
+		this.lcA = this.bodyA.sweep.localCenter
+		this.lcB = this.bodyB.sweep.localCenter
+		this.lcC = this.bodyC.sweep.localCenter
+		this.lcD = this.bodyD.sweep.localCenter
+		this.mA = this.bodyA.invMass
+		this.mB = this.bodyB.invMass
+		this.mC = this.bodyC.invMass
+		this.mD = this.bodyD.invMass
+		this.iA = this.bodyA.rotInertiaInv
+		this.iB = this.bodyB.rotInertiaInv
+		this.iC = this.bodyC.rotInertiaInv
+		this.iD = this.bodyD.rotInertiaInv
+
+		val aA = data.positions[this.indexA].a
+		var vA = data.velocities[this.indexA].v
+		var wA = data.velocities[this.indexA].w
+
+		val aB = data.positions[this.indexB].a
+		var vB = data.velocities[this.indexB].v
+		var wB = data.velocities[this.indexB].w
+
+		val aC = data.positions[this.indexC].a
+		var vC = data.velocities[this.indexC].v
+		var wC = data.velocities[this.indexC].w
+
+		val aD = data.positions[this.indexD].a
+		var vD = data.velocities[this.indexD].v
+		var wD = data.velocities[this.indexD].w
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+		val qC = Rotation(aC)
+		val qD = Rotation(aD)
+
+		this.mass = 0.0
+
+		if (this.typeA == JointType.REVOLUTE) {
+			this.JvAC = Vector2d.ZERO
+			this.JwA = 1.0
+			this.JwC = 1.0
+			this.mass += this.iA + this.iC
+		} else {
+			val u = b2Mul(qC, this.localAxisC)
+			val rC = b2Mul(qC, this.localAnchorC - this.lcC)
+			val rA = b2Mul(qA, this.localAnchorA - this.lcA)
+			this.JvAC = u
+			this.JwC = b2Cross(rC, u)
+			this.JwA = b2Cross(rA, u)
+			this.mass += this.mC + this.mA + this.iC * this.JwC * this.JwC + this.iA * this.JwA * this.JwA
+		}
+
+		if (this.typeB == JointType.REVOLUTE) {
+			this.JvBD = Vector2d.ZERO
+			this.JwB = this.ratio
+			this.JwD = this.ratio
+			this.mass += this.ratio * this.ratio * (this.iB + this.iD)
+		} else {
+			val u = b2Mul(qD, this.localAxisD)
+			val rD = b2Mul(qD, this.localAnchorD - this.lcD)
+			val rB = b2Mul(qB, this.localAnchorB - this.lcB)
+			this.JvBD = this.ratio * u
+			this.JwD = this.ratio * b2Cross(rD, u)
+			this.JwB = this.ratio * b2Cross(rB, u)
+			this.mass += this.ratio * this.ratio * (this.mD + this.mB) + this.iD * this.JwD * this.JwD + this.iB * this.JwB * this.JwB
+		}
+
+		// Compute effective mass.
+		this.mass = if (this.mass > 0.0) 1.0 / this.mass else 0.0
+
+		if (data.step.warmStarting) {
+			vA += (this.mA * this.impulse) * this.JvAC
+			wA += this.iA * this.impulse * this.JwA
+			vB += (this.mB * this.impulse) * this.JvBD
+			wB += this.iB * this.impulse * this.JwB
+			vC -= (this.mC * this.impulse) * this.JvAC
+			wC -= this.iC * this.impulse * this.JwC
+			vD -= (this.mD * this.impulse) * this.JvBD
+			wD -= this.iD * this.impulse * this.JwD
+		} else {
+			this.impulse = 0.0
+		}
+
+		data.velocities[this.indexA].v = vA
+		data.velocities[this.indexA].w = wA
+		data.velocities[this.indexB].v = vB
+		data.velocities[this.indexB].w = wB
+		data.velocities[this.indexC].v = vC
+		data.velocities[this.indexC].w = wC
+		data.velocities[this.indexD].v = vD
+		data.velocities[this.indexD].w = wD
+	}
+
+	override fun solveVelocityConstraints(data: B2SolverData) {
+		var vA = data.velocities[this.indexA].v
+		var wA = data.velocities[this.indexA].w
+		var vB = data.velocities[this.indexB].v
+		var wB = data.velocities[this.indexB].w
+		var vC = data.velocities[this.indexC].v
+		var wC = data.velocities[this.indexC].w
+		var vD = data.velocities[this.indexD].v
+		var wD = data.velocities[this.indexD].w
+
+		var Cdot = b2Dot(this.JvAC, vA - vC) + b2Dot(this.JvBD, vB - vD)
+		Cdot += (this.JwA * wA - this.JwC * wC) + (this.JwB * wB - this.JwD * wD)
+
+		val impulse = -this.mass * Cdot
+		this.impulse += impulse
+
+		vA += (this.mA * impulse) * this.JvAC
+		wA += this.iA * impulse * this.JwA
+		vB += (this.mB * impulse) * this.JvBD
+		wB += this.iB * impulse * this.JwB
+		vC -= (this.mC * impulse) * this.JvAC
+		wC -= this.iC * impulse * this.JwC
+		vD -= (this.mD * impulse) * this.JvBD
+		wD -= this.iD * impulse * this.JwD
+
+		data.velocities[this.indexA].v = vA
+		data.velocities[this.indexA].w = wA
+		data.velocities[this.indexB].v = vB
+		data.velocities[this.indexB].w = wB
+		data.velocities[this.indexC].v = vC
+		data.velocities[this.indexC].w = wC
+		data.velocities[this.indexD].v = vD
+		data.velocities[this.indexD].w = wD
+	}
+
+	override fun solvePositionConstraints(data: B2SolverData): Boolean {
+		var cA = data.positions[this.indexA].c
+		var aA = data.positions[this.indexA].a
+		var cB = data.positions[this.indexB].c
+		var aB = data.positions[this.indexB].a
+		var cC = data.positions[this.indexC].c
+		var aC = data.positions[this.indexC].a
+		var cD = data.positions[this.indexD].c
+		var aD = data.positions[this.indexD].a
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+		val qC = Rotation(aC)
+		val qD = Rotation(aD)
+
+		val coordinateA: Double
+		val coordinateB: Double
+
+		val JvAC: Vector2d
+		val JvBD: Vector2d
+		val JwA: Double
+		val JwB: Double
+		val JwC: Double
+		val JwD: Double
+
+		var mass = 0.0
+
+		if (this.typeA == JointType.REVOLUTE) {
+			JvAC = Vector2d.ZERO
+			JwA = 1.0
+			JwC = 1.0
+			mass += this.iA + this.iC
+
+			coordinateA = aA - aC - this.referenceAngleA
+		} else {
+			val u = b2Mul(qC, this.localAxisC)
+			val rC = b2Mul(qC, this.localAnchorC - this.lcC)
+			val rA = b2Mul(qA, this.localAnchorA - this.lcA)
+			JvAC = u
+			JwC = b2Cross(rC, u)
+			JwA = b2Cross(rA, u)
+			mass += this.mC + this.mA + this.iC * JwC * JwC + this.iA * JwA * JwA
+
+			val pC = this.localAnchorC - this.lcC
+			val pA = b2MulT(qC, rA + (cA - cC))
+			coordinateA = b2Dot(pA - pC, this.localAxisC)
+		}
+
+		if (this.typeB == JointType.REVOLUTE) {
+			JvBD = Vector2d.ZERO
+			JwB = this.ratio
+			JwD = this.ratio
+			mass += this.ratio * this.ratio * (this.iB + this.iD)
+
+			coordinateB = aB - aD - this.referenceAngleB
+		} else {
+			val u = b2Mul(qD, this.localAxisD)
+			val rD = b2Mul(qD, this.localAnchorD - this.lcD)
+			val rB = b2Mul(qB, this.localAnchorB - this.lcB)
+			JvBD = this.ratio * u
+			JwD = this.ratio * b2Cross(rD, u)
+			JwB = this.ratio * b2Cross(rB, u)
+			mass += this.ratio * this.ratio * (this.mD + this.mB) + this.iD * JwD * JwD + this.iB * JwB * JwB
+
+			val pD = this.localAnchorD - this.lcD
+			val pB = b2MulT(qD, rB + (cB - cD))
+			coordinateB = b2Dot(pB - pD, this.localAxisD)
+		}
+
+		val C = (coordinateA + this.ratio * coordinateB) - this.constant
+
+		var impulse = 0.0
+
+		if (mass > 0.0f) {
+			impulse = -C / mass
+		}
+
+		cA += this.mA * impulse * JvAC
+		aA += this.iA * impulse * JwA
+		cB += this.mB * impulse * JvBD
+		aB += this.iB * impulse * JwB
+		cC -= this.mC * impulse * JvAC
+		aC -= this.iC * impulse * JwC
+		cD -= this.mD * impulse * JvBD
+		aD -= this.iD * impulse * JwD
+
+		data.positions[this.indexA].c = cA
+		data.positions[this.indexA].a = aA
+		data.positions[this.indexB].c = cB
+		data.positions[this.indexB].a = aB
+		data.positions[this.indexC].c = cC
+		data.positions[this.indexC].a = aC
+		data.positions[this.indexD].c = cD
+		data.positions[this.indexD].a = aD
+
+		if (b2Abs(C) < this.tolerance) {
+			return true
+		}
+
+		return false
+	}
+
+	override val anchorA: Vector2d
+		get() = bodyA.getWorldPoint(localAnchorA)
+	override val anchorB: Vector2d
+		get() = bodyB.getWorldPoint(localAnchorB)
+
+	override fun getReactionForce(inv_dt: Double): Vector2d {
+		return inv_dt * (impulse * JvAC)
+	}
+
+	override fun getReactionTorque(inv_dt: Double): Double {
+		return inv_dt * (impulse * JwA)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MotorJoint.kt

@@ -0,0 +1,225 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.B2SolverData
+import ru.dbotthepony.kbox2d.api.b2Mul
+import ru.dbotthepony.kstarbound.math.MutableMatrix2d
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+
+class MotorJoint(def: MotorJointDef) : AbstractJoint(def) {
+	// Solver shared
+	var linearOffset: Vector2d = def.linearOffset
+		set(value) {
+			if (value != field) {
+				field = value
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+			}
+		}
+
+	var angularOffset: Double = def.angularOffset
+		set(value) {
+			if (value != field) {
+				field = value
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+			}
+		}
+
+	private var linearImpulse: Vector2d = Vector2d.ZERO
+	private var angularImpulse: Double = 0.0
+
+	var maxForce: Double = def.maxForce
+		set(value) {
+			require(!value.isNaN()) { "Tried to set NaN force" }
+			require(value.isFinite()) { "Tried to set infinite force" }
+			require(value >= 0.0) { "Tried to set negative force: $value" }
+			field = value
+		}
+
+	var maxTorque: Double = def.maxTorque
+		set(value) {
+			require(!value.isNaN()) { "Tried to set NaN torque" }
+			require(value.isFinite()) { "Tried to set infinite torque" }
+			require(value >= 0.0) { "Tried to set negative torque: $value" }
+			field = value
+		}
+
+	var correctionFactor: Double = def.correctionFactor
+		set(value) {
+			require(!value.isNaN()) { "Tried to set NaN correction factor" }
+			require(value.isFinite()) { "Tried to set infinite correction factor" }
+			require(value in 0.0 .. 1.0) { "Tried to set correction factor out of bounds: $value" }
+			field = value
+		}
+
+	// Solver temp
+	private var indexA: Int = 0
+	private var indexB: Int = 0
+	private var rA: Vector2d = Vector2d.ZERO
+	private var rB: Vector2d = Vector2d.ZERO
+	private var localCenterA: Vector2d = Vector2d.ZERO
+	private var localCenterB: Vector2d = Vector2d.ZERO
+	private var linearError: Vector2d = Vector2d.ZERO
+	private var angularError: Double = 0.0
+	private var invMassA: Double = 0.0
+	private var invMassB: Double = 0.0
+	private var invIA: Double = 0.0
+	private var invIB: Double = 0.0
+	private var linearMass: MutableMatrix2d = MutableMatrix2d()
+	private var angularMass: Double = 0.0
+
+	override fun initVelocityConstraints(data: B2SolverData) {
+		this.indexA = this.bodyA.islandIndex
+		this.indexB = this.bodyB.islandIndex
+		this.localCenterA = this.bodyA.sweep.localCenter
+		this.localCenterB = this.bodyB.sweep.localCenter
+		this.invMassA = this.bodyA.invMass
+		this.invMassB = this.bodyB.invMass
+		this.invIA = this.bodyA.invI
+		this.invIB = this.bodyB.invI
+
+		val cA = data.positions[this.indexA].c
+		val aA = data.positions[this.indexA].a
+		var vA = data.velocities[this.indexA].v
+		var wA = data.velocities[this.indexA].w
+
+		val cB = data.positions[this.indexB].c
+		val aB = data.positions[this.indexB].a
+		var vB = data.velocities[this.indexB].v
+		var wB = data.velocities[this.indexB].w
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		// Compute the effective mass matrix.
+		this.rA = b2Mul(qA, this.linearOffset - this.localCenterA)
+		this.rB = b2Mul(qB, -this.localCenterB)
+
+		// J = [-I -r1_skew I r2_skew]
+		// r_skew = [-ry; rx]
+
+		// Matlab
+		// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
+		//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
+		//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
+
+		val mA = this.invMassA
+		val mB = this.invMassB
+		val iA = this.invIA
+		val iB = this.invIB
+
+		// Upper 2 by 2 of K for point to point
+		val K = MutableMatrix2d()
+		K.m00 = mA + mB + iA * this.rA.y * this.rA.y + iB * this.rB.y * this.rB.y
+		K.m10 = -iA * this.rA.x * this.rA.y - iB * this.rB.x * this.rB.y
+		K.m01 = K.m10
+		K.m11 = mA + mB + iA * this.rA.x * this.rA.x + iB * this.rB.x * this.rB.x
+
+		this.linearMass = K.getInverse().asMutableMatrix()
+
+		this.angularMass = iA + iB
+		if (this.angularMass > 0.0) {
+			this.angularMass = 1.0 / this.angularMass
+		}
+
+		this.linearError = cB + this.rB - cA - this.rA
+		this.angularError = aB - aA - this.angularOffset
+
+		if (data.step.warmStarting) {
+			// Scale impulses to support a variable time step.
+			this.linearImpulse *= data.step.dtRatio
+			this.angularImpulse *= data.step.dtRatio
+
+			val P = Vector2d(this.linearImpulse.x, this.linearImpulse.y)
+			vA -= mA * P
+			wA -= iA * (b2Cross(this.rA, P) + this.angularImpulse)
+			vB += mB * P
+			wB += iB * (b2Cross(this.rB, P) + this.angularImpulse)
+		} else {
+			this.linearImpulse = Vector2d.ZERO
+			this.angularImpulse = 0.0
+		}
+
+		data.velocities[this.indexA].v = vA
+		data.velocities[this.indexA].w = wA
+		data.velocities[this.indexB].v = vB
+		data.velocities[this.indexB].w = wB
+	}
+
+	override fun solveVelocityConstraints(data: B2SolverData) {
+		var vA = data.velocities[this.indexA].v
+		var wA = data.velocities[this.indexA].w
+		var vB = data.velocities[this.indexB].v
+		var wB = data.velocities[this.indexB].w
+
+		val mA = this.invMassA
+		val mB = this.invMassB
+		val iA = this.invIA
+		val iB = this.invIB
+
+		val h = data.step.dt
+		val inv_h = data.step.inv_dt
+
+		// Solve angular friction
+		run {
+			val Cdot = wB - wA + inv_h * this.correctionFactor * this.angularError
+			var impulse = -this.angularMass * Cdot
+
+			val oldImpulse = this.angularImpulse
+			val maxImpulse = h * this.maxTorque
+			this.angularImpulse = b2Clamp(this.angularImpulse + impulse, -maxImpulse, maxImpulse)
+			impulse = this.angularImpulse - oldImpulse
+
+			wA -= iA * impulse
+			wB += iB * impulse
+		}
+
+		// Solve linear friction
+		run {
+			val Cdot = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA) + inv_h * this.correctionFactor * this.linearError
+
+			var impulse = -b2Mul(this.linearMass, Cdot)
+			val oldImpulse = this.linearImpulse
+			this.linearImpulse += impulse
+
+			val maxImpulse = h * this.maxForce
+
+			if (this.linearImpulse.lengthSquared > maxImpulse * maxImpulse) {
+				this.linearImpulse = this.linearImpulse.normalized
+				this.linearImpulse *= maxImpulse
+			}
+
+			impulse = this.linearImpulse - oldImpulse
+
+			vA -= mA * impulse
+			wA -= iA * b2Cross(this.rA, impulse)
+
+			vB += mB * impulse
+			wB += iB * b2Cross(this.rB, impulse)
+		}
+
+		data.velocities[this.indexA].v = vA
+		data.velocities[this.indexA].w = wA
+		data.velocities[this.indexB].v = vB
+		data.velocities[this.indexB].w = wB
+	}
+
+	override fun solvePositionConstraints(data: B2SolverData): Boolean {
+		return true
+	}
+
+	override val anchorA: Vector2d
+		get() = bodyA.position
+	override val anchorB: Vector2d
+		get() = bodyB.position
+
+	override fun getReactionForce(inv_dt: Double): Vector2d {
+		return inv_dt * linearImpulse
+	}
+
+	override fun getReactionTorque(inv_dt: Double): Double {
+		return inv_dt * angularImpulse
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MouseJoint.kt

@@ -0,0 +1,150 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.B2SolverData
+import ru.dbotthepony.kbox2d.api.b2MulT
+import ru.dbotthepony.kstarbound.math.MutableMatrix2d
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+
+// p = attached point, m = mouse point
+// C = p - m
+// Cdot = v
+//      = v + cross(w, r)
+// J = [I r_skew]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+class MouseJoint(def: MouseJointDef) : AbstractJoint(def) {
+	private val localAnchorB: Vector2d = b2MulT(bodyB.transform, def.target)
+	var targetA: Vector2d = def.target
+		set(value) {
+			value.isFiniteOrThrow { "Tried to set illegal target $value" }
+			field = value
+			bodyB.isAwake = true
+		}
+
+	var maxForce = def.maxForce
+	var stiffness = def.stiffness
+	var damping = def.damping
+
+	// Solver shared
+	private var impulse: Vector2d = Vector2d.ZERO
+	private var gamma: Double = 0.0
+	private var beta: Double = 0.0
+
+	// Solver temp
+	private var indexA: Int = 0
+	private var indexB: Int = 0
+	private var rB: Vector2d = Vector2d.ZERO
+	private var localCenterB: Vector2d = Vector2d.ZERO
+	private var invMassB: Double = 0.0
+	private var invIB: Double = 0.0
+	private var mass: MutableMatrix2d = MutableMatrix2d().also { it.zero() }
+	private var C: Vector2d = Vector2d.ZERO
+
+	override fun initVelocityConstraints(data: B2SolverData) {
+		indexB = bodyB.islandIndex
+		localCenterB = bodyB.sweep.localCenter
+		invMassB = bodyB.invMass
+		invIB = bodyB.invI
+
+		val cB = data.positions[indexB].c
+		val aB = data.positions[indexB].a
+		var vB = data.velocities[indexB].v
+		var wB = data.velocities[indexB].w
+
+		val qB = Rotation(aB)
+
+		val d = damping
+		val k = stiffness
+
+		// magic formulas
+		// gamma has units of inverse mass.
+		// beta has units of inverse time.
+		val h = data.step.dt
+		gamma = h * (d + h * k)
+
+		if (gamma != 0.0) {
+			gamma = 1.0f / gamma
+		}
+
+		beta = h * k * gamma
+
+		// Compute the effective mass matrix.
+		rB = b2Mul(qB, localAnchorB - localCenterB)
+
+		// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+		//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+		//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
+		val K = MutableMatrix2d()
+		K.m00 = invMassB + invIB * rB.y * rB.y + gamma
+		K.m10 = -invIB * rB.x * rB.y
+		K.m01 = K.m10
+		K.m11 = invMassB + invIB * rB.x * rB.x + gamma
+
+		mass = K.getInverse().asMutableMatrix()
+
+		C = cB + rB - targetA
+		C *= beta
+
+		// Cheat with some damping
+		wB *= 0.98f
+
+		if (data.step.warmStarting) {
+			impulse *= data.step.dtRatio
+			vB += invMassB * impulse
+			wB += invIB * b2Cross(rB, impulse)
+		} else {
+			impulse = Vector2d.ZERO
+		}
+
+		data.velocities[indexB].v = vB
+		data.velocities[indexB].w = wB
+	}
+
+	override fun solveVelocityConstraints(data: B2SolverData) {
+		var vB = data.velocities[indexB].v;
+		var wB = data.velocities[indexB].w;
+
+		// Cdot = v + cross(w, r)
+		val Cdot = vB + b2Cross(wB, rB);
+		var impulse = b2Mul(mass, -(Cdot + C + gamma * impulse));
+
+		val oldImpulse = this.impulse;
+		this.impulse += impulse;
+		val maxImpulse = data.step.dt * maxForce;
+		if (this.impulse.lengthSquared > maxImpulse * maxImpulse) {
+			this.impulse *= maxImpulse / this.impulse.length
+		}
+
+		impulse = this.impulse - oldImpulse;
+
+		vB += invMassB * impulse;
+		wB += invIB * b2Cross(rB, impulse);
+
+		data.velocities[indexB].v = vB;
+		data.velocities[indexB].w = wB;
+	}
+
+	override fun solvePositionConstraints(data: B2SolverData): Boolean {
+		return true
+	}
+
+	override val anchorA: Vector2d
+		get() = targetA
+	override val anchorB: Vector2d
+		get() = bodyB.getWorldPoint(localAnchorB)
+
+	override fun getReactionForce(inv_dt: Double): Vector2d {
+		return inv_dt * impulse
+	}
+
+	override fun getReactionTorque(inv_dt: Double): Double {
+		return 0.0
+	}
+
+	override fun shiftOrigin(newOrigin: Vector2d) {
+		targetA -= newOrigin
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PrismaticJoint.kt

@@ -0,0 +1,605 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.B2SolverData
+import ru.dbotthepony.kbox2d.api.b2Cross
+import ru.dbotthepony.kbox2d.api.b2Mul
+import ru.dbotthepony.kstarbound.math.*
+import ru.dbotthepony.kstarbound.util.Color
+
+// Linear constraint (point-to-line)
+// d = p2 - p1 = x2 + r2 - x1 - r1
+// C = dot(perp, d)
+// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+//      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
+// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
+//
+// Angular constraint
+// C = a2 - a1 + a_initial
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+//
+// K = J * invM * JT
+//
+// J = [-a -s1 a s2]
+//     [0  -1  0  1]
+// a = perp
+// s1 = cross(d + r1, a) = cross(p2 - x1, a)
+// s2 = cross(r2, a) = cross(p2 - x2, a)
+
+// Motor/Limit linear constraint
+// C = dot(ax1, d)
+// Cdot = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+
+// Predictive limit is applied even when the limit is not active.
+// Prevents a constraint speed that can lead to a constraint error in one time step.
+// Want C2 = C1 + h * Cdot >= 0
+// Or:
+// Cdot + C1/h >= 0
+// I do not apply a negative constraint error because that is handled in position correction.
+// So:
+// Cdot + max(C1, 0)/h >= 0
+
+// Block Solver
+// We develop a block solver that includes the angular and linear constraints. This makes the limit stiffer.
+//
+// The Jacobian has 2 rows:
+// J = [-uT -s1 uT s2] // linear
+//     [0   -1   0  1] // angular
+//
+// u = perp
+// s1 = cross(d + r1, u), s2 = cross(r2, u)
+// a1 = cross(d + r1, v), a2 = cross(r2, v)
+
+class PrismaticJoint(def: PrismaticJointDef) : AbstractJoint(def) {
+	internal val localAnchorA: Vector2d = def.localAnchorA
+	internal val localAnchorB: Vector2d = def.localAnchorB
+	internal val localXAxisA: Vector2d = def.localAxisA.normalized
+	internal val localYAxisA: Vector2d = b2Cross(1.0, localXAxisA)
+	internal val referenceAngle: Double = def.referenceAngle
+
+	private var impulse: Vector2d = Vector2d.ZERO
+	private var motorImpulse: Double = 0.0
+	private var lowerImpulse: Double = 0.0
+	private var upperImpulse: Double = 0.0
+
+	// Solver temp
+	private var indexA: Int = 0
+	private var indexB: Int = 0
+	private var localCenterA: Vector2d = Vector2d.ZERO
+	private var localCenterB: Vector2d = Vector2d.ZERO
+	private var invMassA: Double = 0.0
+	private var invMassB: Double = 0.0
+	private var invIA: Double = 0.0
+	private var invIB: Double = 0.0
+	private var axis: Vector2d = Vector2d.ZERO
+	private var perp: Vector2d = Vector2d.ZERO
+	private var s1: Double = 0.0
+	private var s2: Double = 0.0
+	private var a1: Double = 0.0
+	private var a2: Double = 0.0
+	private var K: MutableMatrix2d = MutableMatrix2d()
+	private var translation: Double = 0.0
+	private var axialMass: Double = 0.0
+
+	override fun initVelocityConstraints(data: B2SolverData) {
+		indexA = bodyA.islandIndex
+		indexB = bodyB.islandIndex
+		localCenterA = bodyA.sweep.localCenter
+		localCenterB = bodyB.sweep.localCenter
+		invMassA = bodyA.invMass
+		invMassB = bodyB.invMass
+		invIA = bodyA.rotInertiaInv
+		invIB = bodyB.rotInertiaInv
+
+		val cA = data.positions[indexA].c
+		val aA = data.positions[indexA].a
+		var vA = data.velocities[indexA].v
+		var wA = data.velocities[indexA].w
+
+		val cB = data.positions[indexB].c
+		val aB = data.positions[indexB].a
+		var vB = data.velocities[indexB].v
+		var wB = data.velocities[indexB].w
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		// Compute the effective masses.
+		val rA = b2Mul(qA, localAnchorA - localCenterA)
+		val rB = b2Mul(qB, localAnchorB - localCenterB)
+		val d = (cB - cA) + rB - rA
+
+		val mA = invMassA
+		val mB = invMassB
+		val iA = invIA
+		val iB = invIB
+
+		// Compute motor Jacobian and effective mass.
+		run {
+			this.axis = b2Mul(qA, this.localXAxisA)
+			this.a1 = b2Cross(d + rA, this.axis)
+			this.a2 = b2Cross(rB, this.axis)
+
+			this.axialMass = mA + mB + iA * this.a1 * this.a1 + iB * this.a2 * this.a2
+			if (this.axialMass > 0.0f)
+			{
+				this.axialMass = 1.0f / this.axialMass
+			}
+		}
+
+		// Prismatic constraint.
+		run {
+			this.perp = b2Mul(qA, this.localYAxisA)
+
+			this.s1 = b2Cross(d + rA, this.perp)
+			this.s2 = b2Cross(rB, this.perp)
+
+			val k11 = mA + mB + iA * this.s1 * this.s1 + iB * this.s2 * this.s2
+			val k12 = iA * this.s1 + iB * this.s2
+			var k22 = iA + iB
+
+			if (k22 == 0.0) {
+				// For bodies with fixed rotation.
+				k22 = 1.0
+			}
+
+			this.K.m00 = k11
+			this.K.m10 = k12
+
+			this.K.m01 = k12
+			this.K.m11 = k22
+		}
+
+		if (enableLimit) {
+			translation = b2Dot(axis, d)
+		} else {
+			lowerImpulse = 0.0
+			upperImpulse = 0.0
+		}
+
+		if (!enableMotor) {
+			motorImpulse = 0.0
+		}
+
+		if (data.step.warmStarting) {
+			// Account for variable time step.
+			impulse *= data.step.dtRatio
+			motorImpulse *= data.step.dtRatio
+			lowerImpulse *= data.step.dtRatio
+			upperImpulse *= data.step.dtRatio
+
+			val axialImpulse = motorImpulse + lowerImpulse - upperImpulse
+			val P = impulse.x * perp + axialImpulse * axis
+			val LA = impulse.x * s1 + impulse.y + axialImpulse * a1
+			val LB = impulse.x * s2 + impulse.y + axialImpulse * a2
+
+			vA -= mA * P
+			wA -= iA * LA
+
+			vB += mB * P
+			wB += iB * LB
+		} else {
+			impulse = Vector2d.ZERO
+			motorImpulse = 0.0
+			lowerImpulse = 0.0
+			upperImpulse = 0.0
+		}
+
+		data.velocities[indexA].v = vA
+		data.velocities[indexA].w = wA
+		data.velocities[indexB].v = vB
+		data.velocities[indexB].w = wB
+	}
+
+	override fun solveVelocityConstraints(data: B2SolverData) {
+		var vA = data.velocities[indexA].v
+		var wA = data.velocities[indexA].w
+		var vB = data.velocities[indexB].v
+		var wB = data.velocities[indexB].w
+
+		val mA = invMassA
+		val mB = invMassB
+		val iA = invIA
+		val iB = invIB
+
+		// Solve linear motor constraint
+		if (enableMotor) {
+			val Cdot = b2Dot(axis, vB - vA) + a2 * wB - a1 * wA
+			var impulse = axialMass * (motorSpeed - Cdot)
+			val oldImpulse = motorImpulse
+			val maxImpulse = data.step.dt * maxMotorForce
+			motorImpulse = b2Clamp(motorImpulse + impulse, -maxImpulse, maxImpulse)
+			impulse = motorImpulse - oldImpulse
+
+			val P = impulse * axis
+			val LA = impulse * a1
+			val LB = impulse * a2
+
+			vA -= mA * P
+			wA -= iA * LA
+			vB += mB * P
+			wB += iB * LB
+		}
+
+		if (enableLimit) {
+			// Lower limit
+			run {
+				val C = this.translation - this.lowerTranslation
+				val Cdot = b2Dot(this.axis, vB - vA) + this.a2 * wB - this.a1 * wA
+				var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt)
+				val oldImpulse = this.lowerImpulse
+				this.lowerImpulse = b2Max(this.lowerImpulse + impulse, 0.0)
+				impulse = this.lowerImpulse - oldImpulse
+
+				val P = impulse * this.axis
+				val LA = impulse * this.a1
+				val LB = impulse * this.a2
+
+				vA -= mA * P
+				wA -= iA * LA
+				vB += mB * P
+				wB += iB * LB
+			}
+
+			// Upper limit
+			// Note: signs are flipped to keep C positive when the constraint is satisfied.
+			// This also keeps the impulse positive when the limit is active.
+			run {
+				val C = this.upperTranslation - this.translation
+				val Cdot = b2Dot(this.axis, vA - vB) + this.a1 * wA - this.a2 * wB
+				var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt)
+				val oldImpulse = this.upperImpulse
+				this.upperImpulse = b2Max(this.upperImpulse + impulse, 0.0)
+				impulse = this.upperImpulse - oldImpulse
+
+				val P = impulse * this.axis
+				val LA = impulse * this.a1
+				val LB = impulse * this.a2
+
+				vA += mA * P
+				wA += iA * LA
+				vB -= mB * P
+				wB -= iB * LB
+			}
+		}
+
+		// Solve the prismatic constraint in block form.
+		run {
+			val Cdot = Vector2d(
+				x = b2Dot(this.perp, vB - vA) + this.s2 * wB - this.s1 * wA,
+				y = wB - wA
+			)
+
+			val df = this.K.solve(-Cdot)
+			this.impulse += df
+
+			val P = df.x * this.perp
+			val LA = df.x * this.s1 + df.y
+			val LB = df.x * this.s2 + df.y
+
+			vA -= mA * P
+			wA -= iA * LA
+
+			vB += mB * P
+			wB += iB * LB
+		}
+
+		data.velocities[indexA].v = vA
+		data.velocities[indexA].w = wA
+		data.velocities[indexB].v = vB
+		data.velocities[indexB].w = wB
+	}
+
+	/**
+	 * A velocity based solver computes reaction forces(impulses) using the velocity constraint solver.Under this context,
+	 * the position solver is not there to resolve forces.It is only there to cope with integration error.
+	 *
+	 * Therefore, the pseudo impulses in the position solver do not have any physical meaning.Thus it is okay if they suck.
+	 *
+	 * We could take the active state from the velocity solver.However, the joint might push past the limit when the velocity
+	 * solver indicates the limit is inactive.
+	 */
+	override fun solvePositionConstraints(data: B2SolverData): Boolean {
+		var cA = data.positions[indexA].c
+		var aA = data.positions[indexA].a
+		var cB = data.positions[indexB].c
+		var aB = data.positions[indexB].a
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		val mA = invMassA
+		val mB = invMassB
+		val iA = invIA
+		val iB = invIB
+
+		// Compute fresh Jacobians
+		val rA = b2Mul(qA, localAnchorA - localCenterA)
+		val rB = b2Mul(qB, localAnchorB - localCenterB)
+		val d = cB + rB - cA - rA
+
+		val axis = b2Mul(qA, localXAxisA)
+		val a1 = b2Cross(d + rA, axis)
+		val a2 = b2Cross(rB, axis)
+		val perp = b2Mul(qA, localYAxisA)
+
+		val s1 = b2Cross(d + rA, perp)
+		val s2 = b2Cross(rB, perp)
+
+		val impulse: Vector3d
+
+		val C1 = Vector2d(
+			x = b2Dot(perp, d),
+			y = aB - aA - referenceAngle
+		)
+
+		var linearError = b2Abs(C1.x)
+		val angularError = b2Abs(C1.y)
+
+		var active = false
+		var C2 = 0.0
+
+		if (enableLimit) {
+			val translation = b2Dot(axis, d)
+			if (b2Abs(upperTranslation - lowerTranslation) < 2.0 * b2_linearSlop) {
+				C2 = translation
+				linearError = b2Max(linearError, b2Abs(translation))
+				active = true
+			} else if (translation <= lowerTranslation) {
+				C2 = b2Min(translation - lowerTranslation, 0.0)
+				linearError = b2Max(linearError, lowerTranslation - translation)
+				active = true
+			} else if (translation >= upperTranslation) {
+				C2 = b2Max(translation - upperTranslation, 0.0)
+				linearError = b2Max(linearError, translation - upperTranslation)
+				active = true
+			}
+		}
+
+		if (active) {
+			val k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2
+			val k12 = iA * s1 + iB * s2
+			val k13 = iA * s1 * a1 + iB * s2 * a2
+			var k22 = iA + iB
+
+			if (k22 == 0.0) {
+				// For fixed rotation
+				k22 = 1.0
+			}
+
+			val k23 = iA * a1 + iB * a2
+			val k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2
+
+			val K = MutableMatrix3d()
+
+			K.m00 = k11
+			K.m10 = k12
+			K.m20 = k13
+
+			K.m01 = k12
+			K.m11 = k22
+			K.m21 = k23
+
+			K.m02 = k13
+			K.m12 = k23
+			K.m22 = k33
+
+			val C = Vector3d(
+				x = C1.x,
+				y = C1.y,
+				z = C2,
+			)
+
+			impulse = K.solve(-C)
+		} else {
+			val k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2
+			val k12 = iA * s1 + iB * s2
+			var k22 = iA + iB
+
+			if (k22 == 0.0) {
+				k22 = 1.0
+			}
+
+			val K = MutableMatrix2d()
+
+			K.m00 = k11
+			K.m10 = k12
+
+			K.m01 = k12
+			K.m11 = k22
+
+			val impulse1 = K.solve(-C1)
+			impulse = Vector3d(impulse1.x, impulse1.y)
+		}
+
+		val P = impulse.x * perp + impulse.z * axis
+		val LA = impulse.x * s1 + impulse.y + impulse.z * a1
+		val LB = impulse.x * s2 + impulse.y + impulse.z * a2
+
+		cA -= mA * P
+		aA -= iA * LA
+		cB += mB * P
+		aB += iB * LB
+
+		data.positions[indexA].c = cA
+		data.positions[indexA].a = aA
+		data.positions[indexB].c = cB
+		data.positions[indexB].a = aB
+
+		return linearError <= b2_linearSlop && angularError <= b2_angularSlop
+	}
+
+	override val anchorA: Vector2d
+		get() = bodyA.getWorldPoint(localAnchorA)
+	override val anchorB: Vector2d
+		get() = bodyB.getWorldPoint(localAnchorB)
+
+	override fun getReactionForce(inv_dt: Double): Vector2d {
+		return inv_dt * (impulse.x * perp + (motorImpulse + lowerImpulse - upperImpulse) * axis)
+	}
+
+	override fun getReactionTorque(inv_dt: Double): Double {
+		return inv_dt * impulse.y
+	}
+
+	/**
+	 * Get the current joint translation, usually in meters.
+	 */
+	val jointTranslation: Double get() {
+		val pA = bodyA.getWorldPoint(localAnchorA)
+		val pB = bodyB.getWorldPoint(localAnchorB)
+		val d = pB - pA
+		val axis = bodyA.getWorldVector(localXAxisA)
+
+		return b2Dot(d, axis)
+	}
+
+	/**
+	 * Get the current joint translation speed, usually in meters per second.
+	 */
+	val jointSpeed: Double get() {
+		val bA = bodyA
+		val bB = bodyB
+
+		val rA = b2Mul(bA.transform.q, localAnchorA - bA.sweep.localCenter)
+		val rB = b2Mul(bB.transform.q, localAnchorB - bB.sweep.localCenter)
+		val p1 = bA.sweep.c + rA
+		val p2 = bB.sweep.c + rB
+		val d = p2 - p1
+		val axis = b2Mul(bA.transform.q, localXAxisA)
+
+		val vA = bA.linearVelocity
+		val vB = bB.linearVelocity
+		val wA = bA.angularVelocity
+		val wB = bB.angularVelocity
+
+		return b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA))
+	}
+
+	/**
+	 * Get the lower joint limit, usually in meters.
+	 */
+	var lowerTranslation: Double = def.lowerTranslation
+		private set
+
+	/**
+	 * Get the upper joint limit, usually in meters.
+	 */
+	var upperTranslation: Double = def.upperTranslation
+		private set
+
+	/**
+	 * Set the joint limits, usually in meters.
+	 */
+	fun setLimits(lower: Double, upper: Double) {
+		require(lower <= upper) { "$lower !<= $upper" }
+
+		if (lower != lowerTranslation || upper != upperTranslation) {
+			bodyA.isAwake = true
+			bodyB.isAwake = true
+			lowerImpulse = 0.0
+			upperImpulse = 0.0
+			lowerTranslation = lower
+			upperTranslation = upper
+		}
+	}
+
+	/**
+	 * Set the maximum motor force, usually in N.
+	 */
+	var maxMotorForce: Double = def.maxMotorForce
+		set(value) {
+			if (value != field) {
+				field = value
+
+				if (enableMotor) {
+					bodyA.isAwake = true
+					bodyB.isAwake = true
+				}
+			}
+		}
+
+	/**
+	 * Set the motor speed, usually in meters per second.
+	 */
+	var motorSpeed: Double = def.motorSpeed
+		set(value) {
+			if (value != field) {
+				field = value
+
+				if (enableMotor) {
+					bodyA.isAwake = true
+					bodyB.isAwake = true
+				}
+			}
+		}
+
+	/**
+	 * Enable/disable the joint limit.
+	 */
+	var enableLimit: Boolean = def.enableLimit
+		set(value) {
+			if (value != field) {
+				field = value
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+				lowerImpulse = 0.0
+				upperImpulse = 0.0
+			}
+		}
+
+	/**
+	 * Enable/disable the joint motor.
+	 */
+	var enableMotor: Boolean = def.enableMotor
+		set(value) {
+			if (value != field) {
+				field = value
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+			}
+		}
+
+	init {
+		require(lowerTranslation <= upperTranslation) { "$lowerTranslation !<= $upperTranslation" }
+	}
+
+	fun getMotorForce(inv_dt: Double): Double {
+		return inv_dt * motorImpulse
+	}
+
+	override fun draw(draw: IDebugDraw) {
+		val xfA = bodyA.transform
+		val xfB = bodyB.transform
+		val pA = b2Mul(xfA, localAnchorA)
+		val pB = b2Mul(xfB, localAnchorB)
+
+		val axis = b2Mul(xfA.q, localXAxisA)
+
+		draw.drawSegment(pA, pB, c5)
+
+		if (enableLimit) {
+			val lower = pA + lowerTranslation * axis
+			val upper = pA + upperTranslation * axis
+			val perp = b2Mul(xfA.q, localYAxisA)
+			draw.drawSegment(lower, upper, c1)
+			draw.drawSegment(lower - 0.5 * perp, lower + 0.5 * perp, c2)
+			draw.drawSegment(upper - 0.5 * perp, upper + 0.5 * perp, c3)
+		} else {
+			draw.drawSegment(pA - 1.0 * axis, pA + 1.0 * axis, c1)
+		}
+
+		draw.drawPoint(pA, 5.0, c1)
+		draw.drawPoint(pB, 5.0, c4)
+	}
+
+	companion object {
+		private val c1 = Color(0.7f, 0.7f, 0.7f)
+		private val c2 = Color(0.3f, 0.9f, 0.3f)
+		private val c3 = Color(0.9f, 0.3f, 0.3f)
+		private val c4 = Color(0.3f, 0.3f, 0.9f)
+		private val c5 = Color(0.4f, 0.4f, 0.4f)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PulleyJoint.kt

@@ -0,0 +1,281 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.B2SolverData
+import ru.dbotthepony.kbox2d.api.b2Mul
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+
+/**
+ * The pulley joint is connected to two bodies and two fixed ground points.
+ * The pulley supports a ratio such that:
+ * length1 + ratio * length2 <= constant
+ * Yes, the force transmitted is scaled by the ratio.
+ * Warning: the pulley joint can get a bit squirrelly by itself. They often
+ * work better when combined with prismatic joints. You should also cover the
+ * the anchor points with static shapes to prevent one side from going to
+ * zero length.
+ */
+class PulleyJoint(def: PulleyJointDef) : AbstractJoint(def) {
+	/**
+	 * Get the first ground anchor.
+	 */
+	var groundAnchorA = def.groundAnchorA
+		private set
+
+	/**
+	 * Get the second ground anchor.
+	 */
+	var groundAnchorB = def.groundAnchorB
+		private set
+
+	/**
+	 * Get the current length of the segment attached to bodyA.
+	 */
+	val lengthA = def.lengthA
+
+	/**
+	 * Get the current length of the segment attached to bodyB.
+	 */
+	val lengthB = def.lengthB
+
+	// Solver shared
+	private val localAnchorA: Vector2d = def.localAnchorA
+	private val localAnchorB: Vector2d = def.localAnchorB
+	private val constant: Double = def.lengthA + def.ratio * def.lengthB
+
+	/**
+	 * Get the pulley ratio.
+	 */
+	val ratio: Double = def.ratio
+
+	init {
+		require(ratio != 0.0) { "Ratio is zero" }
+	}
+
+	private var impulse: Double = 0.0
+
+	// Solver temp
+	private var indexA: Int = 0
+	private var indexB: Int = 0
+	private var uA: Vector2d = Vector2d.ZERO
+	private var uB: Vector2d = Vector2d.ZERO
+	private var rA: Vector2d = Vector2d.ZERO
+	private var rB: Vector2d = Vector2d.ZERO
+	private var localCenterA: Vector2d = Vector2d.ZERO
+	private var localCenterB: Vector2d = Vector2d.ZERO
+	private var invMassA: Double = 0.0
+	private var invMassB: Double = 0.0
+	private var invIA: Double = 0.0
+	private var invIB: Double = 0.0
+	private var mass: Double = 0.0
+
+	override fun initVelocityConstraints(data: B2SolverData) {
+		indexA = bodyA.islandIndex
+		indexB = bodyB.islandIndex
+		localCenterA = bodyA.sweep.localCenter
+		localCenterB = bodyB.sweep.localCenter
+		invMassA = bodyA.invMass
+		invMassB = bodyB.invMass
+		invIA = bodyA.rotInertiaInv
+		invIB = bodyB.rotInertiaInv
+
+		val cA = data.positions[indexA].c
+		val aA = data.positions[indexA].a
+		var vA = data.velocities[indexA].v
+		var wA = data.velocities[indexA].w
+
+		val cB = data.positions[indexB].c
+		val aB = data.positions[indexB].a
+		var vB = data.velocities[indexB].v
+		var wB = data.velocities[indexB].w
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		rA = b2Mul(qA, localAnchorA - localCenterA)
+		rB = b2Mul(qB, localAnchorB - localCenterB)
+
+		// Get the pulley axes.
+		uA = cA + rA - groundAnchorA
+		uB = cB + rB - groundAnchorB
+
+		val lengthA = uA.length
+		val lengthB = uB.length
+
+		if (lengthA > 10.0 * b2_linearSlop) {
+			uA *= 1.0 / lengthA
+		} else {
+			uA = Vector2d.ZERO
+		}
+
+		if (lengthB > 10.0 * b2_linearSlop) {
+			uB *= 1.0 / lengthB
+		} else {
+			uB = Vector2d.ZERO
+		}
+
+		// Compute effective mass.
+		val ruA = b2Cross(rA, uA)
+		val ruB = b2Cross(rB, uB)
+
+		val mA = invMassA + invIA * ruA * ruA
+		val mB = invMassB + invIB * ruB * ruB
+
+		mass = mA + ratio * ratio * mB
+
+		if (mass > 0.0) {
+			mass = 1.0 / mass
+		}
+
+		if (data.step.warmStarting) {
+			// Scale impulses to support variable time steps.
+			impulse *= data.step.dtRatio
+
+			// Warm starting.
+			val PA = -(impulse) * uA
+			val PB = (-ratio * impulse) * uB
+
+			vA += invMassA * PA
+			wA += invIA * b2Cross(rA, PA)
+			vB += invMassB * PB
+			wB += invIB * b2Cross(rB, PB)
+		} else {
+			impulse = 0.0
+		}
+
+		data.velocities[indexA].v = vA
+		data.velocities[indexA].w = wA
+		data.velocities[indexB].v = vB
+		data.velocities[indexB].w = wB
+	}
+
+	override fun solveVelocityConstraints(data: B2SolverData) {
+		var vA = data.velocities[indexA].v
+		var wA = data.velocities[indexA].w
+		var vB = data.velocities[indexB].v
+		var wB = data.velocities[indexB].w
+
+		val vpA = vA + b2Cross(wA, rA)
+		val vpB = vB + b2Cross(wB, rB)
+
+		val Cdot = -b2Dot(uA, vpA) - ratio * b2Dot(uB, vpB)
+		val impulse = -mass * Cdot
+		this.impulse += impulse
+
+		val PA = -impulse * uA
+		val PB = -ratio * impulse * uB
+		vA += invMassA * PA
+		wA += invIA * b2Cross(rA, PA)
+		vB += invMassB * PB
+		wB += invIB * b2Cross(rB, PB)
+
+		data.velocities[indexA].v = vA
+		data.velocities[indexA].w = wA
+		data.velocities[indexB].v = vB
+		data.velocities[indexB].w = wB
+	}
+
+	override fun solvePositionConstraints(data: B2SolverData): Boolean {
+		var cA = data.positions[indexA].c
+		var aA = data.positions[indexA].a
+		var cB = data.positions[indexB].c
+		var aB = data.positions[indexB].a
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		val rA = b2Mul(qA, localAnchorA - localCenterA)
+		val rB = b2Mul(qB, localAnchorB - localCenterB)
+
+		// Get the pulley axes.
+		var uA = cA + rA - groundAnchorA
+		var uB = cB + rB - groundAnchorB
+
+		val lengthA = uA.length
+		val lengthB = uB.length
+
+		if (lengthA > 10.0 * b2_linearSlop) {
+			uA *= 1.0 / lengthA
+		} else {
+			uA = Vector2d.ZERO
+		}
+
+		if (lengthB > 10.0 * b2_linearSlop) {
+			uB *= 1.0 / lengthB
+		} else {
+			uB = Vector2d.ZERO
+		}
+
+		// Compute effective mass.
+		val ruA = b2Cross(rA, uA)
+		val ruB = b2Cross(rB, uB)
+
+		val mA = invMassA + invIA * ruA * ruA
+		val mB = invMassB + invIB * ruB * ruB
+
+		var mass = mA + ratio * ratio * mB
+
+		if (mass > 0.0f) {
+			mass = 1.0f / mass
+		}
+
+		val C = constant - lengthA - ratio * lengthB
+		val linearError = b2Abs(C)
+
+		val impulse = -mass * C
+
+		val PA = -impulse * uA
+		val PB = -ratio * impulse * uB
+
+		cA += invMassA * PA
+		aA += invIA * b2Cross(rA, PA)
+		cB += invMassB * PB
+		aB += invIB * b2Cross(rB, PB)
+
+		data.positions[indexA].c = cA
+		data.positions[indexA].a = aA
+		data.positions[indexB].c = cB
+		data.positions[indexB].a = aB
+
+		return linearError < b2_linearSlop
+	}
+
+	override val anchorA: Vector2d
+		get() = bodyA.getWorldPoint(localAnchorA)
+	override val anchorB: Vector2d
+		get() = bodyB.getWorldPoint(localAnchorB)
+
+	override fun getReactionForce(inv_dt: Double): Vector2d {
+		return inv_dt * (impulse * uB)
+	}
+
+	override fun getReactionTorque(inv_dt: Double): Double {
+		return 0.0
+	}
+
+	/**
+	 * Get the current length of the segment attached to bodyA.
+	 */
+	val currentLengthA: Double get() {
+		val p = bodyA.getWorldPoint(localAnchorA)
+		val s = groundAnchorA
+		val d = p - s
+		return d.length
+	}
+
+	/**
+	 * Get the current length of the segment attached to bodyB.
+	 */
+	val currentLengthB: Double get() {
+		val p = bodyB.getWorldPoint(localAnchorB)
+		val s = groundAnchorB
+		val d = p - s
+		return d.length
+	}
+
+	override fun shiftOrigin(newOrigin: Vector2d) {
+		groundAnchorA -= newOrigin
+		groundAnchorB -= newOrigin
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/RevoluteJoint.kt

@@ -0,0 +1,463 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.B2SolverData
+import ru.dbotthepony.kbox2d.api.b2Mul
+import ru.dbotthepony.kstarbound.math.MutableMatrix2d
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kstarbound.util.Color
+import kotlin.math.cos
+import kotlin.math.sin
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Motor constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+class RevoluteJoint(def: RevoluteJointDef) : AbstractJoint(def) {
+	// Solver shared
+	val localAnchorA: Vector2d = def.localAnchorA
+	val localAnchorB: Vector2d = def.localAnchorB
+	private var impulse: Vector2d = Vector2d.ZERO
+	private var motorImpulse: Double = 0.0
+	private var lowerImpulse: Double = 0.0
+	private var upperImpulse: Double = 0.0
+	val referenceAngle: Double = def.referenceAngle
+
+	// Solver temp
+	private var indexA: Int = 0
+	private var indexB: Int = 0
+	private var rA: Vector2d = Vector2d.ZERO
+	private var rB: Vector2d = Vector2d.ZERO
+	private var localCenterA: Vector2d = Vector2d.ZERO
+	private var localCenterB: Vector2d = Vector2d.ZERO
+	private var invMassA: Double = 0.0
+	private var invMassB: Double = 0.0
+	private var invIA: Double = 0.0
+	private var invIB: Double = 0.0
+	private var K: MutableMatrix2d = MutableMatrix2d().also { it.zero() }
+	private var angle: Double = 0.0
+	private var axialMass: Double = 0.0
+
+	override fun initVelocityConstraints(data: B2SolverData) {
+		indexA = bodyA.islandIndex
+		indexB = bodyB.islandIndex
+		localCenterA = bodyA.sweep.localCenter
+		localCenterB = bodyB.sweep.localCenter
+		invMassA = bodyA.invMass
+		invMassB = bodyB.invMass
+		invIA = bodyA.rotInertiaInv
+		invIB = bodyB.rotInertiaInv
+
+		val aA = data.positions[indexA].a
+		var vA = data.velocities[indexA].v
+		var wA = data.velocities[indexA].w
+
+		val aB = data.positions[indexB].a
+		var vB = data.velocities[indexB].v
+		var wB = data.velocities[indexB].w
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		rA = b2Mul(qA, localAnchorA - localCenterA)
+		rB = b2Mul(qB, localAnchorB - localCenterB)
+
+		// J = [-I -r1_skew I r2_skew]
+		// r_skew = [-ry; rx]
+
+		// Matlab
+		// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x]
+		//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB]
+
+		val mA = invMassA
+		val mB = invMassB
+		val iA = invIA
+		val iB = invIB
+
+		K.m00 = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB
+		K.m01 = -rA.y * rA.x * iA - rB.y * rB.x * iB
+		K.m10 = K.m01
+		K.m11 = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB
+
+		axialMass = iA + iB
+		val fixedRotation: Boolean
+
+		if (axialMass > 0.0f) {
+			axialMass = 1.0f / axialMass
+			fixedRotation = false
+		} else {
+			fixedRotation = true
+		}
+
+		angle = aB - aA - referenceAngle
+
+		if (!enableLimit || fixedRotation) {
+			lowerImpulse = 0.0
+			upperImpulse = 0.0
+		}
+
+		if (!enableMotor || fixedRotation) {
+			motorImpulse = 0.0
+		}
+
+		if (data.step.warmStarting) {
+			// Scale impulses to support a variable time step.
+			impulse *= data.step.dtRatio
+			motorImpulse *= data.step.dtRatio
+			lowerImpulse *= data.step.dtRatio
+			upperImpulse *= data.step.dtRatio
+
+			val axialImpulse = motorImpulse + lowerImpulse - upperImpulse
+			val P = Vector2d(impulse.x, impulse.y)
+
+			vA -= mA * P
+			wA -= iA * (b2Cross(rA, P) + axialImpulse)
+
+			vB += mB * P
+			wB += iB * (b2Cross(rB, P) + axialImpulse)
+		} else {
+			impulse = Vector2d.ZERO
+			motorImpulse = 0.0
+			lowerImpulse = 0.0
+			upperImpulse = 0.0
+		}
+
+		data.velocities[indexA].v = vA
+		data.velocities[indexA].w = wA
+		data.velocities[indexB].v = vB
+		data.velocities[indexB].w = wB
+	}
+
+	override fun solveVelocityConstraints(data: B2SolverData) {
+		var vA = data.velocities[indexA].v
+		var wA = data.velocities[indexA].w
+		var vB = data.velocities[indexB].v
+		var wB = data.velocities[indexB].w
+
+		val mA = invMassA
+		val mB = invMassB
+		val iA = invIA
+		val iB = invIB
+
+		val fixedRotation = iA + iB == 0.0
+
+		// Solve motor constraint.
+		if (enableMotor && !fixedRotation) {
+			val Cdot = wB - wA - motorSpeed
+			var impulse = -axialMass * Cdot
+			val oldImpulse = motorImpulse
+			val maxImpulse = data.step.dt * maxMotorTorque
+			motorImpulse = b2Clamp(motorImpulse + impulse, -maxImpulse, maxImpulse)
+			impulse = motorImpulse - oldImpulse
+
+			wA -= iA * impulse
+			wB += iB * impulse
+		}
+
+		if (enableLimit && !fixedRotation) {
+			// Lower limit
+			run {
+				val C = this.angle - this.lowerAngle
+				val Cdot = wB - wA
+				var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt)
+				val oldImpulse = this.lowerImpulse
+				this.lowerImpulse = b2Max(this.lowerImpulse + impulse, 0.0)
+				impulse = this.lowerImpulse - oldImpulse
+
+				wA -= iA * impulse
+				wB += iB * impulse
+			}
+
+			// Upper limit
+			// Note: signs are flipped to keep C positive when the constraint is satisfied.
+			// This also keeps the impulse positive when the limit is active.
+			run {
+				val C = this.upperAngle - this.angle
+				val Cdot = wA - wB
+				var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt)
+				val oldImpulse = this.upperImpulse
+				this.upperImpulse = b2Max(this.upperImpulse + impulse, 0.0)
+				impulse = this.upperImpulse - oldImpulse
+
+				wA += iA * impulse
+				wB -= iB * impulse
+			}
+		}
+
+		// Solve point-to-point constraint
+		run {
+			val Cdot = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA)
+			val impulse = this.K.solve(-Cdot)
+
+			this.impulse += impulse
+
+			vA -= mA * impulse
+			wA -= iA * b2Cross(this.rA, impulse)
+
+			vB += mB * impulse
+			wB += iB * b2Cross(this.rB, impulse)
+		}
+
+		data.velocities[indexA].v = vA
+		data.velocities[indexA].w = wA
+		data.velocities[indexB].v = vB
+		data.velocities[indexB].w = wB
+	}
+
+	override fun solvePositionConstraints(data: B2SolverData): Boolean {
+		var cA = data.positions[indexA].c
+		var aA = data.positions[indexA].a
+		var cB = data.positions[indexB].c
+		var aB = data.positions[indexB].a
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		var angularError = 0.0
+		var positionError = 0.0
+
+		val fixedRotation = invIA + invIB == 0.0
+
+		// Solve angular limit constraint
+		if (enableLimit && !fixedRotation) {
+			val angle = aB - aA - referenceAngle
+			var C = 0.0
+
+			if (b2Abs(upperAngle - lowerAngle) < 2.0f * b2_angularSlop) {
+				// Prevent large angular corrections
+				C = b2Clamp(angle - lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection)
+			} else if (angle <= lowerAngle) {
+				// Prevent large angular corrections and allow some slop.
+				C = b2Clamp(angle - lowerAngle + b2_angularSlop, -b2_maxAngularCorrection, 0.0)
+			} else if (angle >= upperAngle) {
+				// Prevent large angular corrections and allow some slop.
+				C = b2Clamp(angle - upperAngle - b2_angularSlop, 0.0, b2_maxAngularCorrection)
+			}
+
+			val limitImpulse = -axialMass * C
+			aA -= invIA * limitImpulse
+			aB += invIB * limitImpulse
+			angularError = b2Abs(C)
+		}
+
+		// Solve point-to-point constraint.
+		run {
+			qA.set(aA)
+			qB.set(aB)
+			val rA = b2Mul(qA, this.localAnchorA - this.localCenterA)
+			val rB = b2Mul(qB, this.localAnchorB - this.localCenterB)
+
+			val C = cB + rB - cA - rA
+			positionError = C.length
+
+			val mA = this.invMassA
+			val mB = this.invMassB
+			val iA = this.invIA
+			val iB = this.invIB
+
+			val K = MutableMatrix2d()
+			K.m00 = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y
+			K.m10 = -iA * rA.x * rA.y - iB * rB.x * rB.y
+			K.m01 = K.m10
+			K.m11 = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x
+
+			val impulse = -K.solve(C)
+
+			cA -= mA * impulse
+			aA -= iA * b2Cross(rA, impulse)
+
+			cB += mB * impulse
+			aB += iB * b2Cross(rB, impulse)
+		}
+
+		data.positions[indexA].c = cA
+		data.positions[indexA].a = aA
+		data.positions[indexB].c = cB
+		data.positions[indexB].a = aB
+
+		return positionError <= b2_linearSlop && angularError <= b2_angularSlop
+	}
+
+	/**
+	 * Get the reaction force given the inverse time step.
+	 *
+	 * Unit is N.
+	 */
+	override fun getReactionForce(inv_dt: Double): Vector2d {
+		return inv_dt * Vector2d(impulse.x, impulse.y)
+	}
+
+	/**
+	 * Get the reaction torque due to the joint limit given the inverse time step.
+	 *
+	 * Unit is N*m.
+	 */
+	override fun getReactionTorque(inv_dt: Double): Double {
+		return inv_dt * (motorImpulse + lowerImpulse - upperImpulse)
+	}
+
+	override val anchorA: Vector2d
+		get() = bodyA.getWorldPoint(localAnchorA)
+	override val anchorB: Vector2d
+		get() = bodyB.getWorldPoint(localAnchorB)
+
+	/**
+	 * Get the current joint angle in radians.
+	 */
+	val jointAngle: Double get() {
+		return bodyB.sweep.a - bodyA.sweep.a - referenceAngle
+	}
+
+	/**
+	 * Get the current joint angle speed in radians per second.
+	 */
+	val jointSpeed: Double get() {
+		return bodyB.angularVelocity - bodyA.angularVelocity
+	}
+
+	var enableMotor: Boolean = def.enableMotor
+		set(value) {
+			if (value != field) {
+				field = value
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+			}
+		}
+
+	/**
+	 * Get the current motor torque given the inverse time step.
+	 *
+	 * Unit is N*m.
+	 */
+	fun getMotorTorque(inv_dt: Double): Double {
+		return inv_dt * motorImpulse
+	}
+
+	/**
+	 *  Set the motor speed in radians per second.
+	 */
+	var motorSpeed: Double = def.motorSpeed
+		set(value) {
+			if (field != value) {
+				if (enableMotor) {
+					bodyA.isAwake = true
+					bodyB.isAwake = true
+				}
+
+				field = value
+			}
+		}
+
+	/**
+	 * Set the maximum motor torque, usually in N-m.
+	 */
+	var maxMotorTorque: Double = def.maxMotorTorque
+		set(value) {
+			if (field != value) {
+				if (enableMotor) {
+					bodyA.isAwake = true
+					bodyB.isAwake = true
+				}
+
+				field = value
+			}
+		}
+
+	/**
+	 * Enable/disable the joint limit.
+	 */
+	var enableLimit: Boolean = def.enableLimit
+		set(value) {
+			if (field != value) {
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+				field = value
+				lowerImpulse = 0.0
+				upperImpulse = 0.0
+			}
+		}
+
+	/**
+	 * Get the lower joint limit in radians.
+	 */
+	var lowerAngle: Double = def.lowerAngle
+		private set
+
+	/**
+	 * Get the upper joint limit in radians.
+	 */
+	var upperAngle: Double = def.upperAngle
+		private set
+
+	init {
+		// KBox2D: Overlooked check in constructor
+		// TODO: Notify Erin
+		require(lowerAngle <= upperAngle) { "$lowerAngle !<= $upperAngle" }
+	}
+
+	/**
+	 * Set the joint limits in radians.
+	 */
+	fun setLimits(lower: Double, upper: Double) {
+		require(lower <= upper) { "$lower !<= $upper" }
+
+		if (lower != lowerAngle || upper != upperAngle) {
+			bodyA.isAwake = true
+			bodyB.isAwake = true
+			lowerImpulse = 0.0
+			upperImpulse = 0.0
+			lowerAngle = lower
+			upperAngle = upper
+		}
+	}
+
+	override fun draw(draw: IDebugDraw) {
+		val xfA = bodyA.transform
+		val xfB = bodyB.transform
+		val pA = b2Mul(xfA, localAnchorA)
+		val pB = b2Mul(xfB, localAnchorB)
+
+		draw.drawPoint(pA, 5.0, c4)
+		draw.drawPoint(pB, 5.0, c5)
+
+		val aA = bodyA.angle
+		val aB = bodyB.angle
+		val angle = aB - aA - referenceAngle
+
+		val r = L * Vector2d(cos(angle), sin(angle))
+		draw.drawSegment(pB, pB + r, c1)
+		draw.drawCircle(pB, L, c1)
+
+		if (enableLimit) {
+			val rlo = L * Vector2d(cos(lowerAngle), sin(lowerAngle))
+			val rhi = L * Vector2d(cos(upperAngle), sin(upperAngle))
+
+			draw.drawSegment(pB, pB + rlo, c2)
+			draw.drawSegment(pB, pB + rhi, c3)
+		}
+
+		draw.drawSegment(xfA.p, pA, color)
+		draw.drawSegment(pA, pB, color)
+		draw.drawSegment(xfB.p, pB, color)
+	}
+
+	companion object {
+		private const val L = 0.5
+		private val color = Color(0.5f, 0.8f, 0.8f)
+		private val c1 = Color(0.7f, 0.7f, 0.7f)
+		private val c2 = Color(0.3f, 0.9f, 0.3f)
+		private val c3 = Color(0.9f, 0.3f, 0.3f)
+		private val c4 = Color(0.3f, 0.3f, 0.9f)
+		private val c5 = Color(0.4f, 0.4f, 0.4f)
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WeldJoint.kt

@@ -0,0 +1,281 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.B2SolverData
+import ru.dbotthepony.kbox2d.api.b2Mul
+import ru.dbotthepony.kstarbound.math.*
+
+class WeldJoint(def: WeldJointDef) : AbstractJoint(def) {
+	var stiffness: Double = def.stiffness
+	var damping: Double = def.damping
+	private var bias: Double = 0.0
+
+	// Solver shared
+	val localAnchorA = def.localAnchorA
+	val localAnchorB = def.localAnchorB
+	val referenceAngle = def.referenceAngle
+	private var gamma: Double = 0.0
+	private var impulse: Vector3d = Vector3d()
+
+	// Solver temp
+	private var indexA: Int = 0
+	private var indexB: Int = 0
+	private var rA: Vector2d = Vector2d.ZERO
+	private var rB: Vector2d = Vector2d.ZERO
+	private var localCenterA: Vector2d = Vector2d.ZERO
+	private var localCenterB: Vector2d = Vector2d.ZERO
+	private var invMassA: Double = 0.0
+	private var invMassB: Double = 0.0
+	private var invIA: Double = 0.0
+	private var invIB: Double = 0.0
+	private var mass: MutableMatrix3d = MutableMatrix3d().also { it.zero() }
+
+	override fun initVelocityConstraints(data: B2SolverData) {
+		this.indexA = this.bodyA.islandIndex
+		this.indexB = this.bodyB.islandIndex
+		this.localCenterA = this.bodyA.sweep.localCenter
+		this.localCenterB = this.bodyB.sweep.localCenter
+		this.invMassA = this.bodyA.invMass
+		this.invMassB = this.bodyB.invMass
+		this.invIA = this.bodyA.invI
+		this.invIB = this.bodyB.invI
+
+		val aA = data.positions[this.indexA].a
+		var vA = data.velocities[this.indexA].v
+		var wA = data.velocities[this.indexA].w
+
+		val aB = data.positions[this.indexB].a
+		var vB = data.velocities[this.indexB].v
+		var wB = data.velocities[this.indexB].w
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		this.rA = b2Mul(qA, this.localAnchorA - this.localCenterA)
+		this.rB = b2Mul(qB, this.localAnchorB - this.localCenterB)
+
+		// J = [-I -r1_skew I r2_skew]
+		//     [ 0       -1 0       1]
+		// r_skew = [-ry; rx]
+
+		// Matlab
+		// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
+		//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
+		//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
+
+		val mA = this.invMassA
+		val mB = this.invMassB
+		val iA = this.invIA
+		val iB = this.invIB
+
+		val K = MutableMatrix3d()
+		K.m00 = mA + mB + this.rA.y * this.rA.y * iA + this.rB.y * this.rB.y * iB
+		K.m01 = -this.rA.y * this.rA.x * iA - this.rB.y * this.rB.x * iB
+		K.m02 = -this.rA.y * iA - this.rB.y * iB
+
+		K.m10 = K.m01
+		K.m11 = mA + mB + this.rA.x * this.rA.x * iA + this.rB.x * this.rB.x * iB
+		K.m12 = this.rA.x * iA + this.rB.x * iB
+
+		K.m20 = K.m02
+		K.m21 = K.m12
+		K.m22 = iA + iB
+
+		if (this.stiffness > 0.0) {
+			this.mass = K.getInverse2().asMutableMatrix()
+
+			var invM = iA + iB
+
+			val C = aB - aA - this.referenceAngle
+
+			// Damping coefficient
+			val d = this.damping
+
+			// Spring stiffness
+			val k = this.stiffness
+
+			// magic formulas
+			val h = data.step.dt
+			this.gamma = h * (d + h * k)
+			this.gamma = if (this.gamma != 0.0) 1.0 / this.gamma else 0.0
+			this.bias = C * h * k * this.gamma
+
+			invM += this.gamma
+			this.mass.m22 = if (invM != 0.0) 1.0 / invM else 0.0
+		} else if (K.m22 == 0.0) {
+			this.mass = K.getInverse2().asMutableMatrix()
+			this.gamma = 0.0
+			this.bias = 0.0
+		} else {
+			this.mass = K.getInverse().asMutableMatrix()
+			this.gamma = 0.0
+			this.bias = 0.0
+		}
+
+		if (data.step.warmStarting) {
+			// Scale impulses to support a variable time step.
+			this.impulse *= data.step.dtRatio
+
+			val P = Vector2d(this.impulse.x, this.impulse.y)
+
+			vA -= mA * P
+			wA -= iA * (b2Cross(this.rA, P) + this.impulse.z)
+
+			vB += mB * P
+			wB += iB * (b2Cross(this.rB, P) + this.impulse.z)
+		} else {
+			this.impulse = Vector3d()
+		}
+
+		data.velocities[this.indexA].v = vA
+		data.velocities[this.indexA].w = wA
+		data.velocities[this.indexB].v = vB
+		data.velocities[this.indexB].w = wB
+	}
+
+	override fun solveVelocityConstraints(data: B2SolverData) {
+		var vA = data.velocities[this.indexA].v
+		var wA = data.velocities[this.indexA].w
+		var vB = data.velocities[this.indexB].v
+		var wB = data.velocities[this.indexB].w
+
+		val mA = this.invMassA
+		val mB = this.invMassB
+		val iA = this.invIA
+		val iB = this.invIB
+
+		if (this.stiffness > 0.0) {
+			val Cdot2 = wB - wA
+
+			val impulse2 = -this.mass.m22 * (Cdot2 + this.bias + this.gamma * this.impulse.z)
+			this.impulse += Vector3d(z = impulse2)
+
+			wA -= iA * impulse2
+			wB += iB * impulse2
+
+			val Cdot1 = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA)
+
+			val impulse1 = -b2Mul22(this.mass, Cdot1)
+			this.impulse += Vector3d(impulse1.x, impulse1.y)
+
+			val P = impulse1
+
+			vA -= mA * P
+			wA -= iA * b2Cross(this.rA, P)
+
+			vB += mB * P
+			wB += iB * b2Cross(this.rB, P)
+		} else {
+			val Cdot1 = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA)
+			val Cdot2 = wB - wA
+			val Cdot = Vector3d(Cdot1.x, Cdot1.y, Cdot2)
+
+			val impulse = -b2Mul(this.mass, Cdot)
+			this.impulse += impulse
+
+			val P = Vector2d(impulse.x, impulse.y)
+
+			vA -= mA * P
+			wA -= iA * (b2Cross(this.rA, P) + impulse.z)
+
+			vB += mB * P
+			wB += iB * (b2Cross(this.rB, P) + impulse.z)
+		}
+
+		data.velocities[this.indexA].v = vA
+		data.velocities[this.indexA].w = wA
+		data.velocities[this.indexB].v = vB
+		data.velocities[this.indexB].w = wB
+	}
+
+	override fun solvePositionConstraints(data: B2SolverData): Boolean {
+		var cA = data.positions[this.indexA].c
+		var aA = data.positions[this.indexA].a
+		var cB = data.positions[this.indexB].c
+		var aB = data.positions[this.indexB].a
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		val mA = this.invMassA
+		val mB = this.invMassB
+		val iA = this.invIA
+		val iB = this.invIB
+
+		val rA = b2Mul(qA, this.localAnchorA - this.localCenterA)
+		val rB = b2Mul(qB, this.localAnchorB - this.localCenterB)
+
+		val positionError: Double
+		val angularError: Double
+
+		val K = MutableMatrix3d()
+		K.m00 = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB
+		K.m01 = -rA.y * rA.x * iA - rB.y * rB.x * iB
+		K.m02 = -rA.y * iA - rB.y * iB
+		K.m10 = K.m01
+		K.m11 = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB
+		K.m12 = rA.x * iA + rB.x * iB
+		K.m20 = K.m02
+		K.m21 = K.m12
+		K.m22 = iA + iB
+
+		if (this.stiffness > 0.0f) {
+			val C1 = cB + rB - cA - rA
+
+			positionError = C1.length
+			angularError = 0.0
+
+			val P = -K.solve(C1)
+
+			cA -= mA * P
+			aA -= iA * b2Cross(rA, P)
+
+			cB += mB * P
+			aB += iB * b2Cross(rB, P)
+		} else {
+			val C1 = cB + rB - cA - rA
+			val C2 = aB - aA - this.referenceAngle
+
+			positionError = C1.length
+			angularError = b2Abs(C2)
+
+			val C = Vector3d(C1.x, C1.y, C2)
+
+			val impulse: Vector3d
+			if (K.m22 > 0.0) {
+				impulse = -K.solve(C)
+			} else {
+				val impulse2 = -K.solve(C1)
+				impulse = Vector3d(impulse2.x, impulse2.y)
+			}
+
+			val P = Vector2d(impulse.x, impulse.y)
+
+			cA -= mA * P
+			aA -= iA * (b2Cross(rA, P) + impulse.z)
+
+			cB += mB * P
+			aB += iB * (b2Cross(rB, P) + impulse.z)
+		}
+
+		data.positions[this.indexA].c = cA
+		data.positions[this.indexA].a = aA
+		data.positions[this.indexB].c = cB
+		data.positions[this.indexB].a = aB
+
+		return positionError <= b2_linearSlop && angularError <= b2_angularSlop
+	}
+
+	override val anchorA: Vector2d
+		get() = bodyA.getWorldPoint(localAnchorA)
+	override val anchorB: Vector2d
+		get() = bodyB.getWorldPoint(localAnchorB)
+
+	override fun getReactionForce(inv_dt: Double): Vector2d {
+		return Vector2d(impulse.x * inv_dt, impulse.y * inv_dt)
+	}
+
+	override fun getReactionTorque(inv_dt: Double): Double {
+		return inv_dt * impulse.z
+	}
+}

src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WheelJoint.kt

@@ -0,0 +1,534 @@
+package ru.dbotthepony.kbox2d.dynamics.joint
+
+import ru.dbotthepony.kbox2d.api.*
+import ru.dbotthepony.kbox2d.api.B2SolverData
+import ru.dbotthepony.kbox2d.api.b2Cross
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kstarbound.util.Color
+
+class WheelJoint(def: WheelJointDef) : AbstractJoint(def) {
+	val localAnchorA: Vector2d = def.localAnchorA
+	val localAnchorB: Vector2d = def.localAnchorB
+	val localXAxisA: Vector2d = def.localAxisA
+	val localYAxisA: Vector2d = b2Cross(1.0, localXAxisA)
+
+	private var impulse: Double = 0.0
+	private var motorImpulse: Double = 0.0
+	private var springImpulse: Double = 0.0
+
+	private var lowerImpulse: Double = 0.0
+	private var upperImpulse: Double = 0.0
+	private var translation: Double = 0.0
+
+	// Solver temp
+	private var indexA: Int = 0
+	private var indexB: Int = 0
+	private var localCenterA: Vector2d = Vector2d.ZERO
+	private var localCenterB: Vector2d = Vector2d.ZERO
+	private var invMassA: Double = 0.0
+	private var invMassB: Double = 0.0
+	private var invIA: Double = 0.0
+	private var invIB: Double = 0.0
+
+	private var ax: Vector2d = Vector2d.ZERO
+	private var ay: Vector2d = Vector2d.ZERO
+
+	private var sAx: Double = 0.0
+	private var sBx: Double = 0.0
+	private var sAy: Double = 0.0
+	private var sBy: Double = 0.0
+
+	private var mass: Double = 0.0
+	private var motorMass: Double = 0.0
+	private var axialMass: Double = 0.0
+	private var springMass: Double = 0.0
+
+	private var bias: Double = 0.0
+	private var gamma: Double = 0.0
+
+	override fun initVelocityConstraints(data: B2SolverData) {
+		this.indexA = this.bodyA.islandIndex
+		this.indexB = this.bodyB.islandIndex
+		this.localCenterA = this.bodyA.sweep.localCenter
+		this.localCenterB = this.bodyB.sweep.localCenter
+		this.invMassA = this.bodyA.invMass
+		this.invMassB = this.bodyB.invMass
+		this.invIA = this.bodyA.invI
+		this.invIB = this.bodyB.invI
+
+		val mA = this.invMassA
+		val mB = this.invMassB
+		val iA = this.invIA
+		val iB = this.invIB
+
+		val cA = data.positions[this.indexA].c
+		val aA = data.positions[this.indexA].a
+		var vA = data.velocities[this.indexA].v
+		var wA = data.velocities[this.indexA].w
+
+		val cB = data.positions[this.indexB].c
+		val aB = data.positions[this.indexB].a
+		var vB = data.velocities[this.indexB].v
+		var wB = data.velocities[this.indexB].w
+
+		val qA = Rotation(aA)
+		val qB = Rotation(aB)
+
+		// Compute the effective masses.
+		val rA = b2Mul(qA, this.localAnchorA - this.localCenterA)
+		val rB = b2Mul(qB, this.localAnchorB - this.localCenterB)
+		val d = cB + rB - cA - rA
+
+		// Point to line constraint
+		run {
+			this.ay = b2Mul(qA, this.localYAxisA)
+			this.sAy = b2Cross(d + rA, this.ay)
+			this.sBy = b2Cross(rB, this.ay)
+
+			this.mass = mA + mB + iA * this.sAy * this.sAy + iB * this.sBy * this.sBy
+
+			if (this.mass > 0.0) {
+				this.mass = 1.0 / this.mass
+			}
+		}
+
+		// Spring constraint
+		this.ax = b2Mul(qA, this.localXAxisA)
+		this.sAx = b2Cross(d + rA, this.ax)
+		this.sBx = b2Cross(rB, this.ax)
+
+		val invMass = mA + mB + iA * this.sAx * this.sAx + iB * this.sBx * this.sBx
+
+		if (invMass > 0.0) {
+			this.axialMass = 1.0 / invMass
+		} else {
+			this.axialMass = 0.0
+		}
+
+		this.springMass = 0.0
+		this.bias = 0.0
+		this.gamma = 0.0
+
+		if (this.stiffness > 0.0f && invMass > 0.0f) {
+			this.springMass = 1.0f / invMass
+
+			val C = b2Dot(d, this.ax)
+
+			// magic formulas
+			val h = data.step.dt
+			this.gamma = h * (this.damping + h * this.stiffness)
+			if (this.gamma > 0.0f) {
+				this.gamma = 1.0f / this.gamma
+			}
+
+			this.bias = C * h * this.stiffness * this.gamma
+
+			this.springMass = invMass + this.gamma
+			if (this.springMass > 0.0) {
+				this.springMass = 1.0 / this.springMass
+			}
+		} else {
+			this.springImpulse = 0.0
+		}
+
+		if (this.enableLimit) {
+			this.translation = b2Dot(this.ax, d)
+		} else {
+			this.lowerImpulse = 0.0
+			this.upperImpulse = 0.0
+		}
+
+		if (this.enableMotor) {
+			this.motorMass = iA + iB
+
+			if (this.motorMass > 0.0f) {
+				this.motorMass = 1.0f / this.motorMass
+			}
+		} else {
+			this.motorMass = 0.0
+			this.motorImpulse = 0.0
+		}
+
+		if (data.step.warmStarting) {
+			// Account for variable time step.
+			this.impulse *= data.step.dtRatio
+			this.springImpulse *= data.step.dtRatio
+			this.motorImpulse *= data.step.dtRatio
+
+			val axialImpulse = this.springImpulse + this.lowerImpulse - this.upperImpulse
+			val P = this.impulse * this.ay + axialImpulse * this.ax
+			val LA = this.impulse * this.sAy + axialImpulse * this.sAx + this.motorImpulse
+			val LB = this.impulse * this.sBy + axialImpulse * this.sBx + this.motorImpulse
+
+			vA -= this.invMassA * P
+			wA -= this.invIA * LA
+
+			vB += this.invMassB * P
+			wB += this.invIB * LB
+		} else {
+			this.impulse = 0.0
+			this.springImpulse = 0.0
+			this.motorImpulse = 0.0
+			this.lowerImpulse = 0.0
+			this.upperImpulse = 0.0
+		}
+
+		data.velocities[this.indexA].v = vA
+		data.velocities[this.indexA].w = wA
+		data.velocities[this.indexB].v = vB
+		data.velocities[this.indexB].w = wB
+	}
+
+	override fun solveVelocityConstraints(data: B2SolverData) {
+		val mA = this.invMassA
+		val mB = this.invMassB
+		val iA = this.invIA
+		val iB = this.invIB
+
+		var vA = data.velocities[this.indexA].v
+		var wA = data.velocities[this.indexA].w
+		var vB = data.velocities[this.indexB].v
+		var wB = data.velocities[this.indexB].w
+
+		// Solve spring constraint
+		run {
+			val Cdot = b2Dot(this.ax, vB - vA) + this.sBx * wB - this.sAx * wA
+			val impulse = -this.springMass * (Cdot + this.bias + this.gamma * this.springImpulse)
+			this.springImpulse += impulse
+
+			val P = impulse * this.ax
+			val LA = impulse * this.sAx
+			val LB = impulse * this.sBx
+
+			vA -= mA * P
+			wA -= iA * LA
+
+			vB += mB * P
+			wB += iB * LB
+		}
+
+		// Solve rotational motor constraint
+		run {
+			val Cdot = wB - wA - this.motorSpeed
+			var impulse = -this.motorMass * Cdot
+
+			val oldImpulse = this.motorImpulse
+			val maxImpulse = data.step.dt * this.maxMotorTorque
+			this.motorImpulse = b2Clamp(this.motorImpulse + impulse, -maxImpulse, maxImpulse)
+			impulse = this.motorImpulse - oldImpulse
+
+			wA -= iA * impulse
+			wB += iB * impulse
+		}
+
+		if (this.enableLimit) {
+			// Lower limit
+			run {
+				val C = this.translation - this.lowerTranslation
+				val Cdot = b2Dot(this.ax, vB - vA) + this.sBx * wB - this.sAx * wA
+				var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt)
+				val oldImpulse = this.lowerImpulse
+				this.lowerImpulse = b2Max(this.lowerImpulse + impulse, 0.0)
+				impulse = this.lowerImpulse - oldImpulse
+
+				val P = impulse * this.ax
+				val LA = impulse * this.sAx
+				val LB = impulse * this.sBx
+
+				vA -= mA * P
+				wA -= iA * LA
+				vB += mB * P
+				wB += iB * LB
+			}
+
+			// Upper limit
+			// Note: signs are flipped to keep C positive when the constraint is satisfied.
+			// This also keeps the impulse positive when the limit is active.
+			run {
+				val C = this.upperTranslation - this.translation
+				val Cdot = b2Dot(this.ax, vA - vB) + this.sAx * wA - this.sBx * wB
+				var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt)
+				val oldImpulse = this.upperImpulse
+				this.upperImpulse = b2Max(this.upperImpulse + impulse, 0.0)
+				impulse = this.upperImpulse - oldImpulse
+
+				val P = impulse * this.ax
+				val LA = impulse * this.sAx
+				val LB = impulse * this.sBx
+
+				vA += mA * P
+				wA += iA * LA
+				vB -= mB * P
+				wB -= iB * LB
+			}
+		}
+
+		// Solve point to line constraint
+		run {
+			val Cdot = b2Dot(this.ay, vB - vA) + this.sBy * wB - this.sAy * wA
+			val impulse = -this.mass * Cdot
+			this.impulse += impulse
+
+			val P = impulse * this.ay
+			val LA = impulse * this.sAy
+			val LB = impulse * this.sBy
+
+			vA -= mA * P
+			wA -= iA * LA
+
+			vB += mB * P
+			wB += iB * LB
+		}
+
+		data.velocities[this.indexA].v = vA
+		data.velocities[this.indexA].w = wA
+		data.velocities[this.indexB].v = vB
+		data.velocities[this.indexB].w = wB
+	}
+
+	override fun solvePositionConstraints(data: B2SolverData): Boolean {
+		var cA = data.positions[this.indexA].c
+		var aA = data.positions[this.indexA].a
+		var cB = data.positions[this.indexB].c
+		var aB = data.positions[this.indexB].a
+
+		var linearError = 0.0
+
+		if (this.enableLimit) {
+			val qA = Rotation(aA)
+			val qB = Rotation(aB)
+
+			val rA = b2Mul(qA, this.localAnchorA - this.localCenterA)
+			val rB = b2Mul(qB, this.localAnchorB - this.localCenterB)
+			val d = (cB - cA) + rB - rA
+
+			val ax = b2Mul(qA, this.localXAxisA)
+			val sAx = b2Cross(d + rA, this.ax)
+			val sBx = b2Cross(rB, this.ax)
+
+			var C = 0.0
+			val translation = b2Dot(ax, d)
+			if (b2Abs(this.upperTranslation - this.lowerTranslation) < 2.0 * b2_linearSlop) {
+				C = translation
+			} else if (translation <= this.lowerTranslation) {
+				C = b2Min(translation - this.lowerTranslation, 0.0)
+			} else if (translation >= this.upperTranslation) {
+				C = b2Max(translation - this.upperTranslation, 0.0)
+			}
+
+			if (C != 0.0) {
+				val invMass = this.invMassA + this.invMassB + this.invIA * sAx * sAx + this.invIB * sBx * sBx
+				var impulse = 0.0
+				if (invMass != 0.0) {
+					impulse = -C / invMass
+				}
+
+				val P = impulse * ax
+				val LA = impulse * sAx
+				val LB = impulse * sBx
+
+				cA -= this.invMassA * P
+				aA -= this.invIA * LA
+				cB += this.invMassB * P
+				aB += this.invIB * LB
+
+				linearError = b2Abs(C)
+			}
+		}
+
+		// Solve perpendicular constraint
+		run {
+			val qA = Rotation(aA)
+			val qB = Rotation(aB)
+
+			val rA = b2Mul(qA, this.localAnchorA - this.localCenterA)
+			val rB = b2Mul(qB, this.localAnchorB - this.localCenterB)
+			val d = (cB - cA) + rB - rA
+
+			val ay = b2Mul(qA, this.localYAxisA)
+
+			val sAy = b2Cross(d + rA, ay)
+			val sBy = b2Cross(rB, ay)
+
+			val C = b2Dot(d, ay)
+
+			val invMass = this.invMassA + this.invMassB + this.invIA * this.sAy * this.sAy + this.invIB * this.sBy * this.sBy
+
+			var impulse = 0.0
+			if (invMass != 0.0) {
+				impulse = - C / invMass
+			}
+
+			val P = impulse * ay
+			val LA = impulse * sAy
+			val LB = impulse * sBy
+
+			cA -= this.invMassA * P
+			aA -= this.invIA * LA
+			cB += this.invMassB * P
+			aB += this.invIB * LB
+
+			linearError = b2Max(linearError, b2Abs(C))
+		}
+
+		data.positions[this.indexA].c = cA
+		data.positions[this.indexA].a = aA
+		data.positions[this.indexB].c = cB
+		data.positions[this.indexB].a = aB
+
+		return linearError <= b2_linearSlop
+	}
+
+	override val anchorA: Vector2d
+		get() = bodyA.getWorldPoint(localAnchorA)
+	override val anchorB: Vector2d
+		get() = bodyB.getWorldPoint(localAnchorB)
+
+	override fun getReactionForce(inv_dt: Double): Vector2d {
+		return inv_dt * (impulse * ay + (springImpulse + lowerImpulse - upperImpulse) * ax)
+	}
+
+	override fun getReactionTorque(inv_dt: Double): Double {
+		return inv_dt * motorImpulse
+	}
+
+	val jointTranslation: Double get() {
+		val bA = bodyA
+		val bB = bodyB
+
+		val pA = bA.getWorldPoint(localAnchorA)
+		val pB = bB.getWorldPoint(localAnchorB)
+		val d = pB - pA
+		val axis = bA.getWorldVector(localXAxisA)
+
+		return b2Dot(d, axis)
+	}
+
+	val jointLinearSpeed: Double get() {
+		val bA = bodyA
+		val bB = bodyB
+
+		val rA = b2Mul(bA.xf.q, localAnchorA - bA.sweep.localCenter)
+		val rB = b2Mul(bB.xf.q, localAnchorB - bB.sweep.localCenter)
+		val p1 = bA.sweep.c + rA
+		val p2 = bB.sweep.c + rB
+		val d = p2 - p1
+		val axis = b2Mul(bA.xf.q, localXAxisA)
+
+		val vA = bA.linearVelocity
+		val vB = bB.linearVelocity
+		val wA = bA.angularVelocity
+		val wB = bB.angularVelocity
+
+		return b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA))
+	}
+
+	val jointAngle: Double get() {
+		return bodyB.sweep.a - bodyA.sweep.a
+	}
+
+	val jointAngularSpeed: Double get() {
+		return bodyB.angularVelocity - bodyA.angularVelocity
+	}
+
+	var lowerTranslation: Double = def.lowerTranslation
+		private set
+	var upperTranslation: Double = def.upperTranslation
+		private set
+
+	init {
+		require(lowerTranslation <= upperTranslation) { "$lowerTranslation !<= $upperTranslation" }
+	}
+
+	fun setLimits(lower: Double, upper: Double) {
+		require(lower <= upper) { "$lower !<= $upper" }
+
+		if (lower != lowerTranslation || upper != upperTranslation) {
+			bodyA.isAwake = true
+			bodyB.isAwake = true
+			lowerTranslation = lower
+			upperTranslation = upper
+			lowerImpulse = 0.0
+			upperImpulse = 0.0
+		}
+	}
+
+	var maxMotorTorque: Double = 0.0
+		set(value) {
+			if (field != value) {
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+				field = value
+			}
+		}
+
+	fun getMotorTorque(inv_dt: Double): Double {
+		return inv_dt * motorImpulse
+	}
+
+	var motorSpeed: Double = 0.0
+		set(value) {
+			if (field != value) {
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+				field = value
+			}
+		}
+
+	var enableLimit: Boolean = def.enableLimit
+		set(value) {
+			if (field != value) {
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+				field = value
+				lowerImpulse = 0.0
+				upperImpulse = 0.0
+			}
+		}
+
+	var enableMotor: Boolean = def.enableMotor
+		set(value) {
+			if (field != value) {
+				bodyA.isAwake = true
+				bodyB.isAwake = true
+				field = value
+			}
+		}
+
+	var stiffness: Double = def.stiffness
+	var damping: Double = def.damping
+
+	override fun draw(draw: IDebugDraw) {
+		val xfA = bodyA.transform
+		val xfB = bodyB.transform
+		val pA = b2Mul(xfA, localAnchorA)
+		val pB = b2Mul(xfB, localAnchorB)
+
+		val axis = b2Mul(xfA.q, localXAxisA)
+
+		draw.drawSegment(pA, pB, c5)
+
+		if (enableLimit) {
+			val lower = pA + lowerTranslation * axis
+			val upper = pA + upperTranslation * axis
+			val perp = b2Mul(xfA.q, localYAxisA)
+			draw.drawSegment(lower, upper, c1)
+			draw.drawSegment(lower - 0.5 * perp, lower + 0.5 * perp, c2)
+			draw.drawSegment(upper - 0.5 * perp, upper + 0.5 * perp, c3)
+		} else {
+			draw.drawSegment(pA - 1.0 * axis, pA + 1.0 * axis, c1)
+		}
+
+		draw.drawPoint(pA, 5.0, c1)
+		draw.drawPoint(pB, 5.0, c4)
+	}
+
+	companion object {
+		private val c1 = Color(0.7f, 0.7f, 0.7f)
+		private val c2 = Color(0.3f, 0.9f, 0.3f)
+		private val c3 = Color(0.9f, 0.3f, 0.3f)
+		private val c4 = Color(0.3f, 0.3f, 0.9f)
+		private val c5 = Color(0.4f, 0.4f, 0.4f)
+	}
+}

src/main/kotlin/ru/dbotthepony/kstarbound/Main.kt

@@ -3,16 +3,20 @@ package ru.dbotthepony.kstarbound
 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.shapes.ChainShape
+import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
+import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
+import ru.dbotthepony.kbox2d.dynamics.B2World
+import ru.dbotthepony.kbox2d.dynamics.joint.MouseJoint
 import ru.dbotthepony.kstarbound.client.StarboundClient
-import ru.dbotthepony.kstarbound.defs.TileDefinition
 import ru.dbotthepony.kstarbound.math.Vector2d
-import ru.dbotthepony.kstarbound.world.CHUNK_SIZE_FF
 import ru.dbotthepony.kstarbound.world.Chunk
-import ru.dbotthepony.kstarbound.world.ChunkPos
 import ru.dbotthepony.kstarbound.world.entities.Move
 import ru.dbotthepony.kstarbound.world.entities.PlayerEntity
-import ru.dbotthepony.kstarbound.world.entities.Projectile
 import java.io.File
+import java.util.*
+import kotlin.collections.ArrayList
 
 private val LOGGER = LogManager.getLogger()
 
@@ -27,14 +31,380 @@ fun main() {
 		//return
 	}
 
+	val world = B2World(Vector2d(y = -10.0))
+
+	val groundDef = BodyDef(
+		position = Vector2d(y = 0.0)
+	)
+
+	val ground = world.createBody(groundDef)
+
+	//val groundPoly = PolygonShape()
+	//groundPoly.setAsBox(50.0, 10.0)
+
+	val groundPoly = ChainShape()
+	groundPoly.createLoop(listOf(
+		Vector2d(-30.0, 10.0),
+		Vector2d(-25.0, 0.0),
+		Vector2d(25.0, 0.0),
+		Vector2d(30.0, 10.0),
+		Vector2d(30.0, -2.0),
+		Vector2d(-30.0, -2.0)).asReversed())
+
+	ground.createFixture(groundPoly, 0.0)
+
+	val boxes = ArrayList<ru.dbotthepony.kbox2d.api.IBody>()
+
+	/*run {
+		val movingDef = BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(y = 4.0),
+			angle = PI / 4.0
+		)
+		val movingBody = world.createBody(movingDef)
+
+		val dynamicBox = PolygonShape()
+		dynamicBox.setAsBox(1.0, 1.0)
+
+		val fixtureDef = FixtureDef(
+			shape = dynamicBox,
+			density = 1.0,
+			friction = 0.3
+		)
+
+		movingBody.createFixture(fixtureDef)
+		boxes.add(movingBody)
+	}*/
+
+	run {
+		if (true)
+			return@run
+		val movingDef = BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = -1.0, y = 6.0),
+		)
+
+		val movingBody = world.createBody(movingDef)
+
+		val dynamicBox = PolygonShape()
+		dynamicBox.setAsBox(1.0, 1.0)
+
+		val fixtureDef = FixtureDef(
+			shape = dynamicBox,
+			density = 1.0,
+			friction = 0.3
+		)
+
+		movingBody.createFixture(fixtureDef)
+		boxes.add(movingBody)
+	}
+
+	val rand = Random()
+
+	val mouseJoints = ArrayList<MouseJoint>()
+
+	run {
+		val stripes = 0
+
+		for (stripe in 0 until stripes) {
+			for (x in 0 .. (stripes - stripe)) {
+				val movingBody = world.createBody(BodyDef(
+					type = BodyType.DYNAMIC,
+					position = Vector2d(x = (-stripes + stripe) * 1.0 + x * 2.1, y = 8.0 + stripe * 2.1),
+					gravityScale = 1.1
+				))
+
+				val dynamicBox: IShape<*>
+
+				if (false) {
+					dynamicBox = PolygonShape()
+					dynamicBox.setAsBox(1.0, 1.0)
+				} else {
+					dynamicBox = CircleShape(1.0)
+				}
+
+				movingBody.createFixture(FixtureDef(
+					shape = dynamicBox,
+					density = 1.0,
+					friction = 0.3
+				))
+
+				/*for (otherBody in boxes) {
+					val def = DistanceJointDef(movingBody, otherBody, movingBody.position, otherBody.position)
+					world.createJoint(def)
+				}*/
+
+				/*val mouse = world.createJoint(MouseJointDef(
+					Vector2d(y = 10.0, x = 10.0),
+					bodyB = movingBody,
+					maxForce = 1000.0
+				).linearStiffness(50.0, 0.7, bodyB = movingBody, bodyA = null))
+
+				mouseJoints.add(mouse as MouseJoint)*/
+
+				boxes.add(movingBody)
+			}
+		}
+	}
+
+	run {
+		if (true)
+			return@run
+		val movingDef1 = BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = -4.0, y = 4.0),
+			gravityScale = 1.1
+		)
+
+		val movingBody1 = world.createBody(movingDef1)
+
+		val dynamicBox = PolygonShape()
+		dynamicBox.setAsBox(1.0, 1.0)
+
+		val fixtureDef1 = FixtureDef(
+			shape = dynamicBox,
+			density = 1.0,
+			friction = 0.3
+		)
+
+		movingBody1.createFixture(fixtureDef1)
+
+		boxes.add(movingBody1)
+
+		val movingDef2 = BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = 4.0, y = 6.0),
+			gravityScale = 1.1
+		)
+
+		val movingBody2 = world.createBody(movingDef2)
+
+		val fixtureDef2 = FixtureDef(
+			shape = dynamicBox,
+			density = 1.0,
+			friction = 0.3
+		)
+
+		movingBody2.createFixture(fixtureDef2)
+		boxes.add(movingBody2)
+
+		val groundAnchor1 = movingBody1.position + Vector2d(y = 10.0)
+		val groundAnchor2 = movingBody2.position + Vector2d(y = 12.0)
+		val def = PulleyJointDef(movingBody1, movingBody2, groundAnchor1, groundAnchor2, movingBody1.position, movingBody2.position, 1.0)
+
+		world.createJoint(def)
+	}
+
+	run {
+		val base = world.createBody(BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = 0.0, y = 16.0),
+		))
+
+		val wheel1 = world.createBody(BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = -2.0, y = 15.0),
+		))
+
+		val wheel2 = world.createBody(BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = 2.0, y = 15.0),
+		))
+
+		base.createFixture(FixtureDef(
+			shape = PolygonShape().also { it.setAsBox(2.0, 0.5) },
+			density = 1.0
+		))
+
+		wheel1.createFixture(FixtureDef(
+			shape = CircleShape(0.5),
+			density = 1.0,
+			friction = 1.5
+		))
+
+		wheel2.createFixture(FixtureDef(
+			shape = CircleShape(0.5),
+			density = 1.0,
+			friction = 1.5
+		))
+
+		world.createJoint(WheelJointDef(
+			bodyA = base,
+			bodyB = wheel1,
+			anchor = Vector2d(x = -2.0, y = 15.0),
+			axis = Vector2d.UP,
+			enableLimit = true,
+			upperTranslation = 0.25,
+			lowerTranslation = -0.25,
+		))
+
+		world.createJoint(WheelJointDef(
+			bodyA = base,
+			bodyB = wheel2,
+			anchor = Vector2d(x = 2.0, y = 15.0),
+			axis = Vector2d.UP,
+			enableLimit = true,
+			upperTranslation = 0.25,
+			lowerTranslation = -0.25,
+		))
+
+		base.setTransform(base.position, 0.56)
+	}
+
+	run {
+		val circleNail = world.createBody(BodyDef(
+			type = BodyType.STATIC,
+			position = Vector2d(x = -12.0, y = 4.0),
+		))
+
+		val circleBody = world.createBody(BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = -12.0, y = 4.0),
+		))
+
+		circleBody.createFixture(FixtureDef(
+			shape = CircleShape(2.0),
+			density = 1.0,
+			friction = 0.3
+		))
+
+		val circleNailJoint = world.createJoint(RevoluteJointDef(
+			circleNail,
+			circleBody,
+			Vector2d(x = -12.0, y = 4.0)
+		))
+
+		val boxNail = world.createBody(BodyDef(
+			type = BodyType.STATIC,
+			position = Vector2d(x = 2.0, y = 4.0),
+		))
+
+		val boxBody = world.createBody(BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = 2.0, y = 4.0),
+		))
+
+		boxBody.createFixture(FixtureDef(
+			shape = PolygonShape().also { it.setAsBox(3.5, 0.5) },
+			density = 1.0,
+			friction = 0.3
+		))
+
+		val boxNailJoint = world.createJoint(RevoluteJointDef(
+			boxNail,
+			boxBody,
+			Vector2d(x = 2.0, y = 4.0)
+		))
+
+		val gearJoint = world.createJoint(GearJointDef(
+			circleBody,
+			boxBody,
+			circleNailJoint,
+			boxNailJoint,
+			1.0
+		))
+	}
+
+	run {
+		val boxA = world.createBody(BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = -2.0, y = 6.0),
+		))
+
+		val boxB = world.createBody(BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = 0.0, y = 6.0),
+		))
+
+		val boxC = world.createBody(BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = 2.0, y = 6.0),
+		))
+
+		val dynamicBox = PolygonShape()
+		dynamicBox.setAsBox(1.0, 1.0)
+
+		val fixtureDef = FixtureDef(
+			shape = dynamicBox,
+			density = 1.0,
+			friction = 0.3
+		)
+
+		boxA.createFixture(fixtureDef)
+		boxB.createFixture(fixtureDef)
+		boxC.createFixture(fixtureDef)
+
+		world.createJoint(WeldJointDef(
+			bodyA = boxA, bodyB = boxB, (boxA.position + boxB.position) * 0.5
+		).linearStiffness(5.0, 0.5, boxA, boxB))
+
+		world.createJoint(WeldJointDef(
+			bodyA = boxB, bodyB = boxC, (boxB.position + boxC.position) * 0.5
+		).linearStiffness(5.0, 0.5, boxB, boxC))
+	}
+
+	run {
+		val boxA = world.createBody(BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = -2.0, y = 8.0),
+		))
+
+		val dynamicBox = PolygonShape()
+		dynamicBox.setAsBox(1.0, 1.0)
+
+		val fixtureDef = FixtureDef(
+			shape = dynamicBox,
+			density = 1.0,
+			friction = 0.3
+		)
+
+		boxA.createFixture(fixtureDef)
+
+		world.createJoint(FrictionJointDef(
+			bodyA = boxA,
+			bodyB = ground,
+			Vector2d(x = -2.0, y = 8.0),
+			maxForce = 1.0,
+			maxTorque = 10.0,
+			collideConnected = true
+		))
+	}
+
+	run {
+		val boxA = world.createBody(BodyDef(
+			type = BodyType.DYNAMIC,
+			position = Vector2d(x = -2.0, y = 8.0),
+		))
+
+		val dynamicBox = PolygonShape()
+		dynamicBox.setAsBox(1.0, 1.0)
+
+		val fixtureDef = FixtureDef(
+			shape = dynamicBox,
+			density = 1.0,
+			friction = 0.3
+		)
+
+		boxA.createFixture(fixtureDef)
+
+		world.createJoint(MotorJointDef(
+			bodyA = boxA,
+			bodyB = ground,
+			maxForce = 1000.0,
+			collideConnected = true,
+		))
+	}
+
+	val timeStep = 1.0 / 144.0
+
 	val client = StarboundClient()
 
 	//Starbound.addFilePath(File("./unpacked_assets/"))
 	Starbound.addPakPath(File("J:\\SteamLibrary\\steamapps\\common\\Starbound\\assets\\packed.pak"))
 
-	Starbound.initializeGame { finished, replaceStatus, status ->
+	/*Starbound.initializeGame { finished, replaceStatus, status ->
 		client.putDebugLog(status, replaceStatus)
-	}
+	}*/
 
 	client.onTermination {
 		Starbound.terminateLoading = true
@@ -45,7 +415,7 @@ fun main() {
 	val ent = PlayerEntity(client.world!!)
 
 	Starbound.onInitialize {
-		chunkA = client.world!!.computeIfAbsent(ChunkPos(0, 0)).chunk
+		/*chunkA = client.world!!.computeIfAbsent(ChunkPos(0, 0)).chunk
 		val chunkB = client.world!!.computeIfAbsent(ChunkPos(-1, 0)).chunk
 		val chunkC = client.world!!.computeIfAbsent(ChunkPos(-2, 0)).chunk
 
@@ -121,11 +491,9 @@ fun main() {
 			val projEnt = Projectile(client.world!!, proj)
 			projEnt.pos = Vector2d(i * 2.0, 10.0)
 			projEnt.spawn()
-		}
+		}*/
 	}
 
-	//val rand = Random()
-
 	ent.pos += Vector2d(y = 36.0, x = -10.0)
 
 	client.onDrawGUI {
@@ -134,8 +502,71 @@ fun main() {
 	}
 
 	client.onPreDrawWorld {
-		client.camera.pos.x = ent.pos.x.toFloat()
-		client.camera.pos.y = ent.pos.y.toFloat()
+		//client.camera.pos.x = ent.pos.x.toFloat()
+		//client.camera.pos.y = ent.pos.y.toFloat()
+	}
+
+	client.camera.pos.y = 10f
+
+	world.debugDraw = client.gl.box2dRenderer
+
+	client.gl.box2dRenderer.drawShapes = true
+	client.gl.box2dRenderer.drawPairs = false
+	client.gl.box2dRenderer.drawAABB = false
+	client.gl.box2dRenderer.drawJoints = false
+
+	client.onPostDrawWorld {
+		world.debugDraw()
+
+		client.gl.quadWireframe {
+			var pos: Vector2d
+
+			/*for (box in boxes) {
+				pos = box.position
+
+				it.quadRotated(
+					-1f,
+					-1f,
+					1f,
+					1f,
+					pos.x.toFloat(),
+					pos.y.toFloat(),
+					box.angle,
+				)
+			}*/
+
+			/*for (box in boxes) {
+				val broad = world.contactManager.broadPhase
+				val f = box.fixtureList!!
+				val proxy = f.proxies[0].proxyId
+				val aabb = broad.getFatAABB(proxy)
+
+				it.quad(
+					aabb,
+				)
+			}*/
+
+			/*pos = ground.position
+
+			it.quad(
+				(pos.x - 50f).toFloat(),
+				(pos.y - 10f).toFloat(),
+				(pos.x + 50f).toFloat(),
+				(pos.y + 10f).toFloat(),
+			)*/
+		}
+
+		for (joint in mouseJoints) {
+			joint.targetA = Vector2d(rand.nextDouble() * 20.0, rand.nextDouble() * 20.0)
+
+			if (rand.nextDouble() < 0.01) {
+				world.destroyJoint(joint)
+				mouseJoints.remove(joint)
+				break
+			}
+		}
+
+		world.step(timeStep, 6, 4)
 	}
 
 	ent.spawn()
@@ -144,8 +575,8 @@ fun main() {
 		Starbound.pollCallbacks()
 
 		//ent.think(client.frameRenderTime)
-		client.camera.pos.x = ent.pos.x.toFloat()
-		client.camera.pos.y = ent.pos.y.toFloat()
+		//client.camera.pos.x = ent.pos.x.toFloat()
+		//client.camera.pos.y = ent.pos.y.toFloat()
 
 		//println(client.camera.velocity.toDoubleVector() * client.frameRenderTime * 0.1)
 

src/main/kotlin/ru/dbotthepony/kstarbound/client/gl/GLStateTracker.kt

@@ -6,6 +6,7 @@ import org.lwjgl.opengl.GL46.*
 import ru.dbotthepony.kstarbound.Starbound
 import ru.dbotthepony.kstarbound.api.IStruct4f
 import ru.dbotthepony.kstarbound.client.freetype.FreeType
+import ru.dbotthepony.kstarbound.client.render.Box2DRenderer
 import ru.dbotthepony.kstarbound.math.Matrix4f
 import ru.dbotthepony.kstarbound.math.Matrix4fStack
 import ru.dbotthepony.kstarbound.client.render.Font
@@ -368,6 +369,26 @@ class GLStateTracker {
 		fragment.unlink()
 	}
 
+	val flat2DLines = object : GLStreamBuilderList {
+		override val small by lazy {
+			return@lazy StreamVertexBuilder(GLFlatAttributeList.VEC2F, VertexType.LINES, 1024)
+		}
+
+		override val statefulSmall by lazy {
+			return@lazy StatefulStreamVertexBuilder(this@GLStateTracker, small)
+		}
+	}
+
+	val flat2DTriangles = object : GLStreamBuilderList {
+		override val small by lazy {
+			return@lazy StreamVertexBuilder(GLFlatAttributeList.VEC2F, VertexType.TRIANGLES, 1024)
+		}
+
+		override val statefulSmall by lazy {
+			return@lazy StatefulStreamVertexBuilder(this@GLStateTracker, small)
+		}
+	}
+
 	val flat2DQuads = object : GLStreamBuilderList {
 		override val small by lazy {
 			return@lazy StreamVertexBuilder(GLFlatAttributeList.VEC2F, VertexType.QUADS, 1024)
@@ -437,6 +458,8 @@ class GLStateTracker {
 		}
 	}
 
+	val box2dRenderer = Box2DRenderer(this)
+
 	companion object {
 		private val LOGGER = LogManager.getLogger(GLStateTracker::class.java)
 	}

src/main/kotlin/ru/dbotthepony/kstarbound/client/gl/VertexBuilder.kt

@@ -6,6 +6,8 @@ import java.io.Closeable
 import java.nio.ByteBuffer
 import java.nio.ByteOrder
 import kotlin.collections.ArrayList
+import kotlin.math.cos
+import kotlin.math.sin
 
 enum class VertexType(val elements: Int, val indicies: IntArray) {
 	LINES(2, intArrayOf(0, 1)),
@@ -41,6 +43,29 @@ interface IVertexBuilder<This : IVertexBuilder<This, VertexType>, VertexType : I
 		return this as This
 	}
 
+	fun quadRotated(
+		x0: Float,
+		y0: Float,
+		x1: Float,
+		y1: Float,
+		x: Float,
+		y: Float,
+		angle: Double,
+		lambda: VertexTransformer = emptyTransform
+	): This {
+		check(type.elements == 4) { "Currently building $type" }
+
+		val s = sin(angle).toFloat()
+		val c = cos(angle).toFloat()
+
+		lambda(vertex().pushVec2f(x + x0 * c - s * y0, y + s * x0 + c * y0), 0).end()
+		lambda(vertex().pushVec2f(x + x1 * c - s * y0, y + s * x1 + c * y0), 1).end()
+		lambda(vertex().pushVec2f(x + x0 * c - s * y1, y + s * x0 + c * y1), 2).end()
+		lambda(vertex().pushVec2f(x + x1 * c - s * y1, y + s * x1 + c * y1), 3).end()
+
+		return this as This
+	}
+
 	fun quad(aabb: AABB, lambda: VertexTransformer = emptyTransform): This {
 		return quad(
 			aabb.mins.x.toFloat(),

src/main/kotlin/ru/dbotthepony/kstarbound/client/render/Box2DRenderer.kt

@@ -0,0 +1,128 @@
+package ru.dbotthepony.kstarbound.client.render
+
+import org.lwjgl.opengl.GL46.*
+import ru.dbotthepony.kbox2d.api.IDebugDraw
+import ru.dbotthepony.kbox2d.api.Transform
+import ru.dbotthepony.kstarbound.PIXELS_IN_STARBOUND_UNIT
+import ru.dbotthepony.kstarbound.client.gl.GLStateTracker
+import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kstarbound.util.Color
+import kotlin.math.cos
+import kotlin.math.sin
+
+class Box2DRenderer(val state: GLStateTracker) : IDebugDraw {
+	override var drawShapes: Boolean = false
+	override var drawJoints: Boolean = false
+	override var drawAABB: Boolean = false
+	override var drawPairs: Boolean = false
+	override var drawCenterOfMess: Boolean = false
+
+	override fun drawPolygon(vertices: List<Vector2d>, color: Color) {
+		require(vertices.size > 1) { "Vertex list had only ${vertices.size} namings in it" }
+
+		val stateful = state.flat2DLines.statefulSmall
+		val builder = stateful.builder
+
+		builder.begin()
+
+		for (i in vertices.indices) {
+			val current = vertices[i]
+			val next = vertices[(i + 1) % vertices.size]
+			builder.Vertex().pushVec2f(current.x.toFloat(), current.y.toFloat())
+			builder.Vertex().pushVec2f(next.x.toFloat(), next.y.toFloat())
+		}
+
+		stateful.upload()
+
+		state.flatProgram.use()
+		state.flatProgram.color.set(color)
+		state.flatProgram.transform.set(state.matrixStack.last)
+
+		stateful.draw(GL_LINES)
+	}
+
+	private fun drawSolid(vertices: List<Vector2d>, color: Color) {
+		require(vertices.size >= 3) { "Vertex list had only ${vertices.size} namings in it" }
+
+		val stateful = state.flat2DTriangles.statefulSmall
+		val builder = stateful.builder
+
+		builder.begin()
+
+		val zero = vertices[0]
+
+		for (i in 1 until vertices.size) {
+			val current = vertices[i]
+			val next = vertices[(i + 1) % vertices.size]
+			builder.Vertex().pushVec2f(zero.x.toFloat(), zero.y.toFloat())
+			builder.Vertex().pushVec2f(current.x.toFloat(), current.y.toFloat())
+			builder.Vertex().pushVec2f(next.x.toFloat(), next.y.toFloat())
+		}
+
+		stateful.upload()
+
+		state.flatProgram.use()
+		state.flatProgram.color.set(color)
+		state.flatProgram.transform.set(state.matrixStack.last)
+
+		stateful.draw(GL_TRIANGLES)
+	}
+
+	override fun drawSolidPolygon(vertices: List<Vector2d>, color: Color) {
+		drawSolid(vertices, color.copy(alpha = 0.5f))
+		drawPolygon(vertices, color)
+	}
+
+	override fun drawCircle(center: Vector2d, radius: Double, color: Color) {
+		val vertexList = ArrayList<Vector2d>()
+
+		for (i in 0 until 360 step 15) {
+			val rad = Math.toRadians(i.toDouble())
+			val c = cos(rad)
+			val s = sin(rad)
+
+			vertexList.add(Vector2d(
+				center.x + c * radius,
+				center.y + s * radius
+			))
+		}
+
+		drawPolygon(vertexList, color)
+	}
+
+	override fun drawSolidCircle(center: Vector2d, radius: Double, axis: Vector2d, color: Color) {
+		val vertexList = ArrayList<Vector2d>()
+
+		for (i in 0 until 360 step 15) {
+			val rad = Math.toRadians(i.toDouble())
+			val c = cos(rad)
+			val s = sin(rad)
+
+			vertexList.add(Vector2d(
+				center.x + c * radius,
+				center.y + s * radius
+			))
+		}
+
+		drawSolidPolygon(vertexList, color.copy(alpha = 0.5f))
+		drawPolygon(vertexList, color)
+		drawPolygon(listOf(center, center + axis * radius), color)
+	}
+
+	override fun drawSegment(p1: Vector2d, p2: Vector2d, color: Color) {
+		drawPolygon(listOf(p1, p2), color)
+	}
+
+	override fun drawTransform(xf: Transform) {
+		TODO("Not yet implemented")
+	}
+
+	override fun drawPoint(p: Vector2d, size: Double, color: Color) {
+		drawSolid(listOf(
+			Vector2d(x = p.x - size / (PIXELS_IN_STARBOUND_UNIT * 2.0), y = p.y - size / (PIXELS_IN_STARBOUND_UNIT * 2.0)),
+			Vector2d(x = p.x + size / (PIXELS_IN_STARBOUND_UNIT * 2.0), y = p.y - size / (PIXELS_IN_STARBOUND_UNIT * 2.0)),
+			Vector2d(x = p.x + size / (PIXELS_IN_STARBOUND_UNIT * 2.0), y = p.y + size / (PIXELS_IN_STARBOUND_UNIT * 2.0)),
+			Vector2d(x = p.x - size / (PIXELS_IN_STARBOUND_UNIT * 2.0), y = p.y + size / (PIXELS_IN_STARBOUND_UNIT * 2.0)),
+		), color)
+	}
+}

src/main/kotlin/ru/dbotthepony/kstarbound/client/render/EntityRenderer.kt

@@ -67,10 +67,10 @@ open class ProjectileRenderer(state: GLStateTracker, entity: Projectile, chunk:
 
 		builder.begin()
 
-		val (u0, v0) = texture.pixelToUV(def.image.frames[animator.frame].texturePosition)
-		val (u1, v1) = texture.pixelToUV(def.image.frames[animator.frame].textureEndPosition)
+		val (u0, v0) = texture.pixelToUV(animator.frameObj.texturePosition)
+		val (u1, v1) = texture.pixelToUV(animator.frameObj.textureEndPosition)
 
-		builder.quadZ(0f, 0f, 1f, def.image.frames[animator.frame].aspectRatioHW, 5f, VertexTransformers.uv(u0, v0, u1, v1))
+		builder.quadZ(0f, 0f, 1f, animator.frameObj.aspectRatioHW, 5f, VertexTransformers.uv(u0, v0, u1, v1))
 
 		stateful.upload()
 		stateful.draw()

src/main/kotlin/ru/dbotthepony/kstarbound/client/render/FrameSetAnimator.kt

@@ -32,6 +32,8 @@ class FrameSetAnimator(
 	var frame = 0
 		private set
 
+	val frameObj get() = set.frames[frame + firstFrame]
+
 	/**
 	 * Возвращает разницу между последним и первым кадром анимации
 	 */

src/main/kotlin/ru/dbotthepony/kstarbound/math/AABB.kt

@@ -57,7 +57,7 @@ data class AABB(val mins: Vector2d, val maxs: Vector2d) {
 
 	val xSpan get() = maxs.x - mins.x
 	val ySpan get() = maxs.y - mins.y
-	val centre get() = mins + maxs * 0.5
+	val centre get() = (mins + maxs) * 0.5
 
 	val A get() = mins
 	val B get() = Vector2d(mins.x, maxs.y)
@@ -69,12 +69,16 @@ data class AABB(val mins: Vector2d, val maxs: Vector2d) {
 	val topRight get() = C
 	val bottomRight get() = D
 
-	val width get() = (maxs.x - mins.x) / 2.0
-	val height get() = (maxs.y - mins.y) / 2.0
+	val width get() = maxs.x - mins.x
+	val height get() = maxs.y - mins.y
+
+	val extents get() = Vector2d(width * 0.5, height * 0.5)
 
 	val diameter get() = mins.distance(maxs)
 	val radius get() = diameter / 2.0
 
+	val perimeter get() = (xSpan + ySpan) * 2.0
+
 	fun isInside(point: Vector2d): Boolean {
 		return point.x in mins.x .. maxs.x && point.y in mins.y .. maxs.y
 	}
@@ -105,6 +109,19 @@ data class AABB(val mins: Vector2d, val maxs: Vector2d) {
 		return intersectY
 	}
 
+	/**
+	 * @return Находится ли [other] внутри этого AABB
+	 */
+	fun contains(other: AABB): Boolean {
+		if (xSpan < other.xSpan || ySpan < other.ySpan)
+			return false
+
+		return  other.mins.x in mins.x .. maxs.x &&
+				other.maxs.x in mins.x .. maxs.x &&
+				other.mins.y in mins.y .. maxs.y &&
+				other.maxs.y in mins.y .. maxs.y
+	}
+
 	/**
 	 * Есть ли пересечение между этим AABB и [other]
 	 *
@@ -436,6 +453,8 @@ data class AABBi(val mins: Vector2i, val maxs: Vector2i) {
 	val diameter get() = mins.distance(maxs)
 	val radius get() = diameter / 2.0
 
+	val perimeter get() = (xSpan + ySpan) * 2
+
 	fun isInside(point: Vector2i): Boolean {
 		return point.x in mins.x .. maxs.x && point.y in mins.y .. maxs.y
 	}

src/main/kotlin/ru/dbotthepony/kstarbound/math/FastMath.kt

@@ -0,0 +1,24 @@
+package ru.dbotthepony.kstarbound.math
+
+/**
+ * Выполняет скалярное умножение между a и векторным произведением b, c на стеке
+ */
+fun scalarDotWithCross(
+	ax: Double,
+	ay: Double,
+	az: Double,
+
+	bx: Double,
+	by: Double,
+	bz: Double,
+
+	cx: Double,
+	cy: Double,
+	cz: Double,
+): Double {
+	val crossX = by * cz - bz * cy
+	val crossY = bz * cx - bx * cz
+	val crossZ = bx * cy - by * cx
+
+	return ax * crossX + ay * crossY + az * crossZ
+}

src/main/kotlin/ru/dbotthepony/kstarbound/math/Vector.kt

@@ -5,10 +5,7 @@ import com.google.gson.TypeAdapter
 import com.google.gson.stream.JsonReader
 import com.google.gson.stream.JsonWriter
 import ru.dbotthepony.kstarbound.api.*
-import kotlin.math.cos
-import kotlin.math.pow
-import kotlin.math.sin
-import kotlin.math.sqrt
+import kotlin.math.*
 
 // Так как у нас нет шаблонов ни в Java, ни в Kotlin
 // а дженерики вызывают autoboxing
@@ -56,6 +53,7 @@ abstract class IVector2i<T : IVector2i<T>> : IMatrixLike, IMatrixLikeInt, IStruc
 	operator fun unaryMinus() = make(-x, -y)
 
 	val length get() = sqrt(x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble())
+	val lengthSquared get() = x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble()
 
 	fun dotProduct(other: IVector2i<*>): Double {
 		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble()
@@ -69,15 +67,15 @@ abstract class IVector2i<T : IVector2i<T>> : IMatrixLike, IMatrixLikeInt, IStruc
 		return other.x * x.toDouble() + other.y * y.toDouble()
 	}
 
-	fun InvDotProduct(other: IVector2i<*>): Double {
+	fun invDotProduct(other: IVector2i<*>): Double {
 		return other.x.toDouble() * y.toDouble() + other.y.toDouble() * x.toDouble()
 	}
 
-	fun InvDotProduct(other: IVector2f<*>): Double {
+	fun invDotProduct(other: IVector2f<*>): Double {
 		return other.x.toDouble() * y.toDouble() + other.y.toDouble() * x.toDouble()
 	}
 
-	fun InvDotProduct(other: IVector2d<*>): Double {
+	fun invDotProduct(other: IVector2d<*>): Double {
 		return other.x * y.toDouble() + other.y * x.toDouble()
 	}
 
@@ -98,6 +96,29 @@ abstract class IVector2i<T : IVector2i<T>> : IMatrixLike, IMatrixLikeInt, IStruc
 		return Vector2d(x / len, y / len)
 	}
 
+	val absoluteVector: Vector2i get() = Vector2i(x.absoluteValue, y.absoluteValue)
+
+	fun minimumPerComponent(other: IVector2i<*>): Vector2i {
+		return Vector2i(
+			x = x.coerceAtMost(other.x),
+			y = y.coerceAtMost(other.y),
+		)
+	}
+
+	fun maximumPerComponent(other: IVector2i<*>): Vector2i {
+		return Vector2i(
+			x = x.coerceAtLeast(other.x),
+			y = y.coerceAtLeast(other.y),
+		)
+	}
+
+	fun clampPerComponent(min: IVector2i<*>, max: IVector2i<*>): Vector2i {
+		return Vector2i(
+			x = x.coerceAtLeast(min.x).coerceAtMost(max.x),
+			y = y.coerceAtLeast(min.y).coerceAtMost(max.y),
+		)
+	}
+
 	fun left() = make(x - 1, y)
 	fun right() = make(x + 1, y)
 	fun up() = make(x, y + 1)
@@ -197,6 +218,9 @@ abstract class IVector2f<T : IVector2f<T>> : IMatrixLike, IMatrixLikeFloat, IStr
 	fun down() = make(x, y - 1)
 
 	val length get() = sqrt(x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble())
+	val lengthSquared get() = x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble()
+
+	val isFinite get() = x.isFinite() && y.isFinite() && !x.isNaN() && !y.isNaN()
 
 	fun dotProduct(other: IVector2i<*>): Double {
 		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble()
@@ -210,15 +234,15 @@ abstract class IVector2f<T : IVector2f<T>> : IMatrixLike, IMatrixLikeFloat, IStr
 		return other.x * x.toDouble() + other.y * y.toDouble()
 	}
 
-	fun InvDotProduct(other: IVector2i<*>): Double {
+	fun invDotProduct(other: IVector2i<*>): Double {
 		return other.x.toDouble() * y.toDouble() + other.y.toDouble() * x.toDouble()
 	}
 
-	fun InvDotProduct(other: IVector2f<*>): Double {
+	fun invDotProduct(other: IVector2f<*>): Double {
 		return other.x.toDouble() * y.toDouble() + other.y.toDouble() * x.toDouble()
 	}
 
-	fun InvDotProduct(other: IVector2d<*>): Double {
+	fun invDotProduct(other: IVector2d<*>): Double {
 		return other.x * y.toDouble() + other.y * x.toDouble()
 	}
 
@@ -239,6 +263,29 @@ abstract class IVector2f<T : IVector2f<T>> : IMatrixLike, IMatrixLikeFloat, IStr
 		return Vector2d(x / len, y / len)
 	}
 
+	val absoluteVector: Vector2f get() = Vector2f(x.absoluteValue, y.absoluteValue)
+
+	fun minimumPerComponent(other: IVector2f<*>): Vector2f {
+		return Vector2f(
+			x = x.coerceAtMost(other.x),
+			y = y.coerceAtMost(other.y),
+		)
+	}
+
+	fun maximumPerComponent(other: IVector2f<*>): Vector2f {
+		return Vector2f(
+			x = x.coerceAtLeast(other.x),
+			y = y.coerceAtLeast(other.y),
+		)
+	}
+
+	fun clampPerComponent(min: IVector2f<*>, max: IVector2f<*>): Vector2f {
+		return Vector2f(
+			x = x.coerceAtLeast(min.x).coerceAtMost(max.x),
+			y = y.coerceAtLeast(min.y).coerceAtMost(max.y),
+		)
+	}
+
 	override fun get(row: Int, column: Int): Float {
 		if (column != 0) {
 			throw IndexOutOfBoundsException("Column must be 0 ($column given)")
@@ -326,6 +373,15 @@ abstract class IVector2d<T : IVector2d<T>> : IMatrixLike, IMatrixLikeDouble, ISt
 	operator fun unaryMinus() = make(-x, -y)
 
 	val length get() = sqrt(x * x + y * y)
+	val lengthSquared get() = x * x + y * y
+
+	val isFinite get() = x.isFinite() && y.isFinite() && !x.isNaN() && !y.isNaN()
+
+	inline fun isFiniteOrThrow(lazy: () -> Any) {
+		if (!isFinite) {
+			throw IllegalStateException(lazy.invoke().toString())
+		}
+	}
 
 	fun dotProduct(other: IVector2i<*>): Double {
 		return other.x * x + other.y * y
@@ -351,6 +407,22 @@ abstract class IVector2d<T : IVector2d<T>> : IMatrixLike, IMatrixLikeDouble, ISt
 		return other.x * y + other.y * x
 	}
 
+	fun crossProduct(other: IVector2i<*>): Double {
+		return x * other.y - y * other.x
+	}
+
+	fun crossProduct(other: IVector2f<*>): Double {
+		return x * other.y - y * other.x
+	}
+
+	fun crossProduct(other: IVector2d<*>): Double {
+		return x * other.y - y * other.x
+	}
+
+	fun crossProduct(other: Double): Vector2d {
+		return Vector2d(y * other, x * -other)
+	}
+
 	fun distance(other: IVector2i<*>): Double {
 		return sqrt((x - other.x).toDouble().pow(2.0) + (y - other.y).toDouble().pow(2.0))
 	}
@@ -363,11 +435,46 @@ abstract class IVector2d<T : IVector2d<T>> : IMatrixLike, IMatrixLikeDouble, ISt
 		return sqrt((x - other.x).toDouble().pow(2.0) + (y - other.y).toDouble().pow(2.0))
 	}
 
+	fun distanceSquared(other: IVector2i<*>): Double {
+		return (x - other.x).toDouble().pow(2.0) + (y - other.y).toDouble().pow(2.0)
+	}
+
+	fun distanceSquared(other: IVector2f<*>): Double {
+		return (x - other.x).toDouble().pow(2.0) + (y - other.y).toDouble().pow(2.0)
+	}
+
+	fun distanceSquared(other: IVector2d<*>): Double {
+		return (x - other.x).toDouble().pow(2.0) + (y - other.y).toDouble().pow(2.0)
+	}
+
 	val normalized: Vector2d get() {
 		val len = length
 		return Vector2d(x / len, y / len)
 	}
 
+	val absoluteVector: Vector2d get() = Vector2d(x.absoluteValue, y.absoluteValue)
+
+	fun minimumPerComponent(other: IVector2d<*>): Vector2d {
+		return Vector2d(
+			x = x.coerceAtMost(other.x),
+			y = y.coerceAtMost(other.y),
+		)
+	}
+
+	fun maximumPerComponent(other: IVector2d<*>): Vector2d {
+		return Vector2d(
+			x = x.coerceAtLeast(other.x),
+			y = y.coerceAtLeast(other.y),
+		)
+	}
+
+	fun clampPerComponent(min: IVector2d<*>, max: IVector2d<*>): Vector2d {
+		return Vector2d(
+			x = x.coerceAtLeast(min.x).coerceAtMost(max.x),
+			y = y.coerceAtLeast(min.y).coerceAtMost(max.y),
+		)
+	}
+
 	fun left() = make(x - 1, y)
 	fun right() = make(x + 1, y)
 	fun up() = make(x, y + 1)
@@ -385,13 +492,38 @@ abstract class IVector2d<T : IVector2d<T>> : IMatrixLike, IMatrixLikeDouble, ISt
 		}
 	}
 
+	operator fun times(other: IMatrixLikeDouble): T {
+		if (other.rows >= 2 && other.columns >= 2) {
+			val x = this.x * other[0, 0] +
+					this.y * other[0, 1]
+
+			val y = this.x * other[1, 0] +
+					this.y * other[1, 1]
+
+			return make(x, y)
+		}
+
+		throw IllegalArgumentException("Incompatible matrix provided: ${other.rows} x ${other.columns}")
+	}
+
 	protected abstract fun make(x: Double, y: Double): T
 	fun toFloatVector(): Vector2f = Vector2f(x.toFloat(), y.toFloat())
 }
 
+fun Double.crossProduct(other: IVector2d<*>): Vector2d {
+	return Vector2d(-this * other.y, this * other.x)
+}
+
+// Только Vector2d во избежание двоякого поведения с мутирующими векторами
+operator fun Double.times(other: Vector2d) = Vector2d(this * other.x, this * other.y)
+
 data class Vector2d(override val x: Double = 0.0, override val y: Double = 0.0) : IVector2d<Vector2d>() {
 	override fun make(x: Double, y: Double) = Vector2d(x, y)
 
+	fun toMutableVector(): MutableVector2d {
+		return MutableVector2d(x, y)
+	}
+
 	companion object {
 		fun fromJson(input: JsonArray): Vector2d {
 			return Vector2d(input[0].asDouble, input[1].asDouble)
@@ -438,6 +570,21 @@ data class MutableVector2d(override var x: Double = 0.0, override var y: Double
 		return this
 	}
 
+	fun zero(): MutableVector2d {
+		this.x = 0.0
+		this.y = 0.0
+		return this
+	}
+
+	fun toVector(): Vector2d {
+		return Vector2d(x, y)
+	}
+
+	fun load(from: IStruct2d) {
+		x = from.component1()
+		y = from.component2()
+	}
+
 	companion object {
 		fun fromJson(input: JsonArray): MutableVector2d {
 			return MutableVector2d(input[0].asDouble, input[1].asDouble)
@@ -466,6 +613,32 @@ abstract class IVector3f<T : IVector3f<T>> : IMatrixLike, IMatrixLikeFloat, IStr
 	operator fun unaryMinus() = make(-x, -y, -z)
 
 	val length get() = sqrt(x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble() + z.toDouble() * z.toDouble())
+	val lengthSquared get() = x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble() + z.toDouble() * z.toDouble()
+
+	val isFinite get() = x.isFinite() && y.isFinite() && z.isFinite() && !x.isNaN() && !y.isNaN() && !z.isNaN()
+
+	val absoluteVector: Vector3f get() = Vector3f(x.absoluteValue, y.absoluteValue, z.absoluteValue)
+
+	fun minimumPerComponent(other: IVector3f<*>): Vector3f {
+		return Vector3f(
+			x = x.coerceAtMost(other.x),
+			y = y.coerceAtMost(other.y),
+		)
+	}
+
+	fun maximumPerComponent(other: IVector3f<*>): Vector3f {
+		return Vector3f(
+			x = x.coerceAtLeast(other.x),
+			y = y.coerceAtLeast(other.y),
+		)
+	}
+
+	fun clampPerComponent(min: IVector3f<*>, max: IVector3f<*>): Vector3f {
+		return Vector3f(
+			x = x.coerceAtLeast(min.x).coerceAtMost(max.x),
+			y = y.coerceAtLeast(min.y).coerceAtMost(max.y),
+		)
+	}
 
 	fun dotProduct(other: IVector3f<*>): Double {
 		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble() + other.z.toDouble() * z.toDouble()
@@ -568,6 +741,155 @@ data class MutableVector3f(override var x: Float = 0f, override var y: Float = 0
 	}
 }
 
+abstract class IVector3d<T : IVector3d<T>> : IMatrixLike, IMatrixLikeDouble, IStruct3d {
+	override val columns = 1
+	override val rows = 3
+
+	abstract val x: Double
+	abstract val y: Double
+	abstract val z: Double
+
+	operator fun plus(other: IVector3d<*>) = make(x + other.x, y + other.y, z + other.z)
+	operator fun minus(other: IVector3d<*>) = make(x - other.x, y - other.y, z - other.z)
+	operator fun times(other: IVector3d<*>) = make(x * other.x, y * other.y, z * other.z)
+	operator fun div(other: IVector3d<*>) = make(x / other.x, y / other.y, z / other.z)
+
+	operator fun plus(other: Double) = make(x + other, y + other, z + other)
+	operator fun minus(other: Double) = make(x - other, y - other, z - other)
+	operator fun times(other: Double) = make(x * other, y * other, z * other)
+	operator fun div(other: Double) = make(x / other, y / other, z / other)
+
+	operator fun unaryMinus() = make(-x, -y, -z)
+
+	val length get() = sqrt(x * x + y * y + z * z)
+	val lengthSquared get() = x * x + y * y + z * z
+
+	val isFinite get() = x.isFinite() && y.isFinite() && z.isFinite() && !x.isNaN() && !y.isNaN() && !z.isNaN()
+
+	val absoluteVector: Vector3d get() = Vector3d(x.absoluteValue, y.absoluteValue, z.absoluteValue)
+
+	fun minimumPerComponent(other: IVector3d<*>): Vector3d {
+		return Vector3d(
+			x = x.coerceAtMost(other.x),
+			y = y.coerceAtMost(other.y),
+		)
+	}
+
+	fun maximumPerComponent(other: IVector3d<*>): Vector3d {
+		return Vector3d(
+			x = x.coerceAtLeast(other.x),
+			y = y.coerceAtLeast(other.y),
+		)
+	}
+
+	fun clampPerComponent(min: IVector3d<*>, max: IVector3d<*>): Vector3d {
+		return Vector3d(
+			x = x.coerceAtLeast(min.x).coerceAtMost(max.x),
+			y = y.coerceAtLeast(min.y).coerceAtMost(max.y),
+		)
+	}
+
+	fun dotProduct(other: IVector3d<*>): Double {
+		return other.x * x + other.y * y + other.z * z
+	}
+
+	override fun get(row: Int, column: Int): Double {
+		if (column != 0) {
+			throw IndexOutOfBoundsException("Column must be 0 ($column given)")
+		}
+
+		return when (row) {
+			0 -> x
+			1 -> y
+			2 -> z
+			else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
+		}
+	}
+
+	fun rotateAroundThis(rotation: Double): Matrix4d {
+		val c = cos(rotation)
+		val s = sin(rotation)
+		val cInv = 1f - c
+
+		return Matrix4d(
+			m00 = c + x * x * cInv,			m01 = x * y * cInv - z * s,		m02 = x * z * cInv + y * s,
+			m10 = y * x * cInv + z * s,		m11 = c + y * y * cInv,			m12 = y * z * cInv - x * s,
+			m20 = z * x * cInv - y * s,		m21 = z * y * cInv + x * s,		m22 = c + z * z * cInv,
+		)
+	}
+
+	operator fun times(other: IMatrixLikeDouble): T {
+		if (other.rows >= 4 && other.columns >= 4) {
+			val x = this.x * other[0, 0] +
+					this.y * other[0, 1] +
+					this.z * other[0, 2] +
+					other[0, 3]
+
+			val y = this.x * other[1, 0] +
+					this.y * other[1, 1] +
+					this.z * other[1, 2] +
+					other[1, 3]
+
+			val z = this.x * other[2, 0] +
+					this.y * other[2, 1] +
+					this.z * other[2, 2] +
+					other[2, 3]
+
+			return make(x, y, z)
+		} else if (other.rows >= 3 && other.columns >= 3) {
+			val x = this.x * other[0, 0] +
+					this.y * other[0, 1] +
+					this.z * other[0, 2]
+
+			val y = this.x * other[1, 0] +
+					this.y * other[1, 1] +
+					this.z * other[1, 2]
+
+			val z = this.x * other[2, 0] +
+					this.y * other[2, 1] +
+					this.z * other[2, 2]
+
+			return make(x, y, z)
+		}
+
+		throw IllegalArgumentException("Incompatible matrix provided: ${other.rows} x ${other.columns}")
+	}
+
+	protected abstract fun make(x: Double, y: Double, z: Double): T
+}
+
+data class Vector3d(override val x: Double = 0.0, override val y: Double = 0.0, override val z: Double = 0.0) : IVector3d<Vector3d>() {
+	override fun make(x: Double, y: Double, z: Double): Vector3d {
+		return Vector3d(x, y, z)
+	}
+
+	fun toMutableVector(): MutableVector3d {
+		return MutableVector3d(x, y, z)
+	}
+
+	companion object {
+		val UP = Vector3d(0.0, 1.0, 0.0)
+		val DOWN = Vector3d(0.0, -1.0, 0.0)
+		val LEFT = Vector3d(-1.0, 0.0, 0.0)
+		val RIGHT = Vector3d(1.0, 0.0, 0.0)
+		val FORWARD = Vector3d(0.0, 0.0, 1.0)
+		val BACKWARD = Vector3d(0.0, 0.0, -1.0)
+	}
+}
+
+data class MutableVector3d(override var x: Double = 0.0, override var y: Double = 0.0, override var z: Double = 0.0) : IVector3d<MutableVector3d>() {
+	fun toVector(): Vector3d {
+		return Vector3d(x, y, z)
+	}
+
+	override fun make(x: Double, y: Double, z: Double): MutableVector3d {
+		this.x = x
+		this.y = y
+		this.z = z
+		return this
+	}
+}
+
 abstract class IVector4f<T : IVector4f<T>> : IMatrixLike, IMatrixLikeFloat, IStruct4f {
 	abstract val x: Float
 	abstract val y: Float
@@ -586,6 +908,31 @@ abstract class IVector4f<T : IVector4f<T>> : IMatrixLike, IMatrixLikeFloat, IStr
 
 	operator fun unaryMinus() = make(-x, -y, -z, -w)
 
+	val absoluteVector: Vector4f get() = Vector4f(x.absoluteValue, y.absoluteValue, z.absoluteValue, w.absoluteValue)
+
+	val isFinite get() = x.isFinite() && y.isFinite() && z.isFinite() && w.isFinite() && !x.isNaN() && !y.isNaN() && !z.isNaN() && !w.isNaN()
+
+	fun minimumPerComponent(other: IVector4f<*>): Vector4f {
+		return Vector4f(
+			x = x.coerceAtMost(other.x),
+			y = y.coerceAtMost(other.y),
+		)
+	}
+
+	fun maximumPerComponent(other: IVector4f<*>): Vector4f {
+		return Vector4f(
+			x = x.coerceAtLeast(other.x),
+			y = y.coerceAtLeast(other.y),
+		)
+	}
+
+	fun clampPerComponent(min: IVector4f<*>, max: IVector4f<*>): Vector4f {
+		return Vector4f(
+			x = x.coerceAtLeast(min.x).coerceAtMost(max.x),
+			y = y.coerceAtLeast(min.y).coerceAtMost(max.y),
+		)
+	}
+
 	override val columns = 1
 	override val rows = 4