Move everything to kvector, implement more stuff

build.gradle.kts

@@ -34,11 +34,27 @@ sourceSets {
 	create("kvector") {
 
 	}
+
+	create("kbox2d") {
+
+	}
+}
+
+sourceSets["kbox2d"].compileClasspath += sourceSets["kvector"].output
+sourceSets["kbox2d"].runtimeClasspath += sourceSets["kvector"].output
+
+sourceSets.main {
+	compileClasspath += sourceSets["kvector"].output
+	runtimeClasspath += sourceSets["kvector"].output
+	compileClasspath += sourceSets["kbox2d"].output
+	runtimeClasspath += sourceSets["kbox2d"].output
 }
 
 sourceSets.test {
 	compileClasspath += sourceSets["kvector"].output
 	runtimeClasspath += sourceSets["kvector"].output
+	compileClasspath += sourceSets["kbox2d"].output
+	runtimeClasspath += sourceSets["kbox2d"].output
 }
 
 dependencies {

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

@@ -1,6 +1,6 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 /**
  * Profiling data. Times are in milliseconds.

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

@@ -1,30 +1,6 @@
 package ru.dbotthepony.kbox2d.api
 
-/*
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-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

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

@@ -1,8 +1,5 @@
 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

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

@@ -1,6 +1,6 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 /// The features that intersect to form the contact point
 /// This must be 4 bytes or less. ?

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

@@ -1,6 +1,6 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 /**
  * A distance proxy is used by the GJK algorithm.

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

@@ -1,9 +1,8 @@
 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
+import ru.dbotthepony.kvector.util2d.AABB
 
 private const val DEBUG_CYCLIC_REFERENCES = true
 
@@ -30,23 +29,6 @@ data class TreeNode(
 
 	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 {

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

@@ -1,7 +1,7 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 sealed interface IFilter {
 	/**

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

@@ -1,7 +1,7 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.Color
-import ru.dbotthepony.kstarbound.util.Color
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 /**
  * Implement and register this class with a b2World to provide debug drawing of physics

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

@@ -1,6 +1,6 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 interface IMovable {
 	/**

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

@@ -1,7 +1,7 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 fun interface ProxyQueryCallback {
 	fun invoke(nodeId: Int, userData: Any?): Boolean

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

@@ -1,8 +1,7 @@
 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 ru.dbotthepony.kvector.vector.ndouble.Vector2d
 import kotlin.math.PI
 
 data class StiffnessResult(val stiffness: Double, val damping: Double)
@@ -758,7 +757,7 @@ interface IJoint : IMovable {
 	/**
 	 * Dump this joint to the log file.
 	 */
-	fun dump() { LOGGER.warn("Dump is not supported for this join type: $this") }
+	fun dump() { }
 
 	/**
 	 * Debug draw this joint
@@ -766,8 +765,4 @@ interface IJoint : IMovable {
 	fun draw(draw: IDebugDraw) { }
 
 	var userData: Any?
-
-	companion object {
-		private val LOGGER = LogManager.getLogger()
-	}
 }

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

@@ -1,6 +1,12 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.math.*
+import ru.dbotthepony.kvector.api.concrete.IMatrix2d
+import ru.dbotthepony.kvector.api.concrete.IMatrix3d
+import ru.dbotthepony.kvector.matrix.multiplyMatrix
+import ru.dbotthepony.kvector.vector.ndouble.MutableVector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector3d
+import ru.dbotthepony.kvector.vector.ndouble.cross
 import kotlin.math.*
 
 /// "Next Largest Power of 2
@@ -115,11 +121,11 @@ class Rotation(
 		)
 	}
 
-	operator fun times(v: IVector2d<*>): Vector2d {
+	operator fun times(v: Vector2d): Vector2d {
 		return Vector2d(c * v.x - s * v.y, s * v.x + c * v.y)
 	}
 
-	fun timesT(v: IVector2d<*>): Vector2d {
+	fun timesT(v: Vector2d): Vector2d {
 		return Vector2d(c * v.x + s * v.y, -s * v.x + c * v.y)
 	}
 }
@@ -157,7 +163,7 @@ class Transform(
 	}
 
 	/// Set this based on the position and angle.
-	fun set(position: IVector2d<*>, angle: Double) {
+	fun set(position: Vector2d, angle: Double) {
 		this.position = Vector2d(position.x, position.y)
 		rotation.set(angle)
 	}
@@ -176,14 +182,14 @@ class Transform(
 		)
 	}
 
-	operator fun times(v: IVector2d<*>): Vector2d {
+	operator fun times(v: Vector2d): 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 {
+	fun timesT(v: Vector2d): Vector2d {
 		val px = v.x - position.x
 		val py = v.y - position.y
 
@@ -330,7 +336,7 @@ class Sweep {
  */
 @Suppress("NOTHING_TO_INLINE")
 internal inline fun b2Dot(a: Vector2d, b: Vector2d): Double {
-	return a.dotProduct(b)
+	return a.dot(b)
 }
 
 /**
@@ -338,7 +344,7 @@ internal inline fun b2Dot(a: Vector2d, b: Vector2d): Double {
  */
 @Suppress("NOTHING_TO_INLINE")
 internal inline fun b2Cross(a: Vector2d, b: Vector2d): Double {
-	return a.crossProduct(b)
+	return a.cross(b)
 }
 
 /**
@@ -346,7 +352,7 @@ internal inline fun b2Cross(a: Vector2d, b: Vector2d): Double {
  */
 @Suppress("NOTHING_TO_INLINE")
 internal inline fun b2Cross(a: Vector2d, b: Double): Vector2d {
-	return a.crossProduct(b)
+	return a.cross(b)
 }
 
 /**
@@ -354,7 +360,7 @@ internal inline fun b2Cross(a: Vector2d, b: Double): Vector2d {
  */
 @Suppress("NOTHING_TO_INLINE")
 internal inline fun b2Cross(a: Double, b: Vector2d): Vector2d {
-	return a.crossProduct(b)
+	return a.cross(b)
 }
 
 /**
@@ -362,7 +368,7 @@ internal inline fun b2Cross(a: Double, b: Vector2d): Vector2d {
  */
 @Suppress("NOTHING_TO_INLINE")
 internal inline fun b2Abs(a: Vector2d): Vector2d {
-	return a.absoluteVector
+	return a.absoluteValue
 }
 
 /**
@@ -409,8 +415,9 @@ internal inline fun b2MulT(t: Transform, v: Vector2d): Vector2d {
  * Shortcut for faster porting of C++ code
  */
 @Suppress("NOTHING_TO_INLINE")
-internal inline fun b2Mul22(t: AbstractMatrix3d<*>, v: Vector2d): Vector2d {
+internal inline fun b2Mul22(t: IMatrix3d<*>, v: Vector2d): Vector2d {
-	return v.times(t)
+	val result = multiplyMatrix(t.toMatrix2d(), v)
+	return Vector2d(result[0, 0], result[0, 1])
 }
 
 /**
@@ -433,16 +440,18 @@ internal inline fun b2MulT(t: Transform, v: Transform): Transform {
  * Shortcut for faster porting of C++ code
  */
 @Suppress("NOTHING_TO_INLINE")
-internal inline fun b2Mul(t: AbstractMatrix2d<*>, v: Vector2d): Vector2d {
+internal inline fun b2Mul(t: IMatrix2d<*>, v: Vector2d): Vector2d {
-	return v.times(t)
+	val result = multiplyMatrix(t, v)
+	return Vector2d(result[0, 0], result[0, 1])
 }
 
 /**
  * Shortcut for faster porting of C++ code
  */
 @Suppress("NOTHING_TO_INLINE")
-internal inline fun b2Mul(t: AbstractMatrix3d<*>, v: Vector3d): Vector3d {
+internal inline fun b2Mul(t: IMatrix3d<*>, v: Vector3d): Vector3d {
-	return v.times(t)
+	val result = multiplyMatrix(t, v)
+	return Vector3d(result[0, 0], result[0, 1], result[0, 2])
 }
 
 /**
@@ -458,7 +467,7 @@ internal inline fun b2Min(a: Double, b: Double): Double {
  */
 @Suppress("NOTHING_TO_INLINE")
 internal inline fun b2Min(a: Vector2d, b: Vector2d): Vector2d {
-	return a.minimumPerComponent(b)
+	return a.coerceAtMost(b)
 }
 
 /**
@@ -482,7 +491,7 @@ internal inline fun b2Max(a: Double, b: Double): Double {
  */
 @Suppress("NOTHING_TO_INLINE")
 internal inline fun b2Max(a: Vector2d, b: Vector2d): Vector2d {
-	return a.maximumPerComponent(b)
+	return a.coerceAtLeast(b)
 }
 
 /**
@@ -508,3 +517,89 @@ internal inline fun b2DistanceSquared(a: Vector2d, b: Vector2d): Double {
 internal inline fun b2Clamp(a: Double, min: Double, max: Double): Double {
 	return b2Max(min, b2Min(a, max))
 }
+
+private 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
+}
+
+/**
+ * Solve A * x = b, where b is a column vector. This is more efficient
+ * than computing the inverse in one-shot cases.
+ */
+internal fun IMatrix3d<*>.solve(b: Vector3d): Vector3d {
+	var determinant = determinant!!
+
+	if (determinant != 0.0) {
+		determinant = 1.0 / determinant
+	}
+
+	return Vector3d(
+		x = determinant * scalarDotWithCross(
+			b.x, b.y, b.z,
+			r01, r11, r21,
+			r02, r12, r22,
+		),
+
+		y = determinant * scalarDotWithCross(
+			r00, r10, r20,
+			b.x, b.y, b.z,
+			r02, r12, r22,
+		),
+
+		z = determinant * scalarDotWithCross(
+			r00, r10, r20,
+			r01, r11, r21,
+			b.x, b.y, b.z,
+		),
+	)
+}
+
+/**
+ * Solve A * x = b, where b is a column vector. This is more efficient
+ * than computing the inverse in one-shot cases.
+ */
+internal fun IMatrix3d<*>.solve(b: Vector2d): Vector2d {
+	var determinant = determinant!!
+
+	if (determinant != 0.0) {
+		determinant = 1.0 / determinant
+	}
+
+	return Vector2d(
+		x = determinant * (r11 * b.x - r01 * b.y),
+		y = determinant * (r00 * b.y - r10 * b.x),
+	)
+}
+
+/**
+ * Solve A * x = b, where b is a column vector. This is more efficient
+ * than computing the inverse in one-shot cases.
+ */
+internal fun IMatrix2d<*>.solve(b: Vector2d): Vector2d {
+	var determinant = determinant!!
+
+	if (determinant != 0.0) {
+		determinant = 1.0 / determinant
+	}
+
+	return Vector2d(
+		x = determinant * (r11 * b.x - r01 * b.y),
+		y = determinant * (r00 * b.y - r10 * b.x),
+	)
+}

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

@@ -1,7 +1,7 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 data class MassData(
 	val mass: Double,

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

@@ -1,8 +1,7 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.client.gl.GLStateTracker
+import ru.dbotthepony.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.Vector2d
 
 /**
  * The world class manages all physics entities, dynamic simulation,

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

@@ -1,6 +1,6 @@
 package ru.dbotthepony.kbox2d.api
 
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 /**
  * Implement this interface to provide collision filtering. In other words, you can implement

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

@@ -1,9 +1,37 @@
 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.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
+
+private class IntArrayList {
+	private var buffer = IntArray(16)
+	var size: Int = 0
+		private set
+
+	operator fun set(index: Int, value: Int) {
+		require(index in 0 until size) { "Index out of bounds: $index" }
+		buffer[index] = value
+	}
+
+	operator fun get(index: Int): Int {
+		require(index in 0 until size) { "Index out of bounds: $index" }
+		return buffer[index]
+	}
+
+	fun add(value: Int) {
+		if (size == buffer.size) {
+			val old = buffer
+			buffer = IntArray(size * 2)
+
+			for (i in 0 until size) {
+				buffer[i] = old[i]
+			}
+		}
+
+		buffer[size++] = value
+	}
+}
 
 class BroadPhase : IBroadPhase {
 	private val moveBuffer = IntArrayList()
@@ -53,7 +81,7 @@ class BroadPhase : IBroadPhase {
 
 	private fun unBufferMove(proxyId: Int) {
 		for (i in 0 until moveCount) {
-			if (moveBuffer.getInt(i) == proxyId) {
+			if (moveBuffer[i] == proxyId) {
 				moveBuffer[i] = e_nullProxy
 			}
 		}
@@ -119,7 +147,7 @@ class BroadPhase : IBroadPhase {
 		pairBuffer.clear()
 
 		for (i in 0 until moveCount) {
-			val value = moveBuffer.getInt(i)
+			val value = moveBuffer[i]
 
 			if (value == e_nullProxy)
 				continue
@@ -139,7 +167,7 @@ class BroadPhase : IBroadPhase {
 
 		// Clear move flags
 		for (i in 0 until moveCount) {
-			val value = moveBuffer.getInt(i)
+			val value = moveBuffer[i]
 
 			if (value != e_nullProxy) {
 				tree.clearMoved(value)

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

@@ -1,10 +1,9 @@
 package ru.dbotthepony.kbox2d.collision
 
 import ru.dbotthepony.kbox2d.api.*
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.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.
@@ -61,7 +60,7 @@ class WorldManifold(manifold: Manifold, xfA: Transform, radiusA: Double, xfB: Tr
 		if (manifold.points.isNotEmpty()) {
 			when (manifold.type) {
 				Manifold.Type.CIRCLES -> {
-					normal = Vector2d.RIGHT
+					normal = Vector2d.POSITIVE_X
 
 					val pointA = b2Mul(xfA, manifold.localPoint);
 					val pointB = b2Mul(xfB, manifold.points[0].localPoint);

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

@@ -1,14 +1,12 @@
 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.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.crossProduct
+import ru.dbotthepony.kvector.vector.ndouble.times
-import ru.dbotthepony.kstarbound.math.times
 
 var b2_gjkCalls = 0
 	private set
@@ -67,10 +65,10 @@ class DistanceProxy : IDistanceProxy {
 
 	override fun getSupport(d: Vector2d): Int {
 		var bestIndex = 0
-		var bestValue = vertices[0].dotProduct(d)
+		var bestValue = b2Dot(vertices[0], d)
 
 		for (i in 1 until vertices.size) {
-			val value = vertices[i].dotProduct(d)
+			val value = b2Dot(vertices[i], d)
 
 			if (value > bestValue) {
 				bestIndex = i
@@ -271,7 +269,7 @@ class Simplex() {
 		val e12 = w2 - w1
 
 		// w1 region
-		val d12_2 = -w1.dotProduct(e12)
+		val d12_2 = b2Dot(-w1, e12)
 		if (d12_2 <= 0.0) {
 			// a2 <= 0, so we clamp it to 0
 			v1.a = 1.0
@@ -280,7 +278,7 @@ class Simplex() {
 		}
 
 		// w2 region
-		val d12_1 = w2.dotProduct(e12)
+		val d12_1 = b2Dot(w2, e12)
 		if (d12_1 <= 0.0) {
 			// a1 <= 0, so we clamp it to 0
 			v2.a = 1.0
@@ -313,8 +311,8 @@ class Simplex() {
 		// [w1.e12 w2.e12][a2] = [0]
 		// a3 = 0
 		val e12 = w2 - w1
-		val w1e12 = w1.dotProduct(e12)
+		val w1e12 = b2Dot(w1, e12)
-		val w2e12 = w2.dotProduct(e12)
+		val w2e12 = b2Dot(w2, e12)
 		val d12_1 = w2e12
 		val d12_2 = -w1e12
 
@@ -323,8 +321,8 @@ class Simplex() {
 		// [w1.e13 w3.e13][a3] = [0]
 		// a2 = 0
 		val e13 = w3 - w1
-		val w1e13 = w1.dotProduct(e13)
+		val w1e13 = b2Dot(w1, e13)
-		val w3e13 = w3.dotProduct(e13)
+		val w3e13 = b2Dot(w3, e13)
 		val d13_1 = w3e13
 		val d13_2 = -w1e13
 
@@ -333,8 +331,8 @@ class Simplex() {
 		// [w2.e23 w3.e23][a3] = [0]
 		// a1 = 0
 		val e23 = w3 - w2
-		val w2e23 = w2.dotProduct(e23)
+		val w2e23 = b2Dot(w2, e23)
-		val w3e23 = w3.dotProduct(e23)
+		val w3e23 = b2Dot(w3, e23)
 		val d23_1 = w3e23
 		val d23_2 = -w2e23
 
@@ -611,8 +609,8 @@ fun b2ShapeCast(
 		v = v.normalized
 
 		// Intersect ray with plane
-		val vp = v.dotProduct(p)
+		val vp = b2Dot(v, p)
-		val vr = v.dotProduct(r)
+		val vr = b2Dot(v, r)
 
 		if (vp - sigma > lambda * vr) {
 			if (vr <= 0.0) {

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

@@ -1,8 +1,8 @@
 package ru.dbotthepony.kbox2d.collision
 
 import ru.dbotthepony.kbox2d.api.*
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 import kotlin.math.absoluteValue
 
 const val b2_nullNode = -1
@@ -111,8 +111,8 @@ class DynamicTree : IDynamicTree {
 		check(nodes[proxyID].isLeaf) { "Can't move whole branch" }
 
 		// Extend AABB
-		val mins = aabb.mins.toMutableVector() - R
+		val mins = (aabb.mins - R).toMutableVector()
-		val maxs = aabb.maxs.toMutableVector() + R
+		val maxs = (aabb.maxs + R).toMutableVector()
 
 		// Predict AABB movement
 		val d = displacement * b2_aabbMultiplier
@@ -648,7 +648,7 @@ class DynamicTree : IDynamicTree {
 
 		// v is perpendicular to the segment.
 		val v = b2Cross(1.0, r)
-		val abs_v = v.absoluteVector
+		val abs_v = v.absoluteValue
 
 		// Separating axis for segment (Gino, p80).
 		// |dot(v, p1 - c)| > dot(|v|, h)
@@ -660,8 +660,8 @@ class DynamicTree : IDynamicTree {
 		var t = p1 + diff * maxFraction
 
 		var segmentAABB = AABB(
-			mins = p1.minimumPerComponent(t),
+			mins = p1.coerceAtMost(t),
-			maxs = p1.maximumPerComponent(t)
+			maxs = p1.coerceAtLeast(t)
 		)
 
 		val stack = ArrayDeque<Int>(256)
@@ -683,7 +683,7 @@ class DynamicTree : IDynamicTree {
 
 			// 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)) {
+			if (v.dot(p1 - nodeAABB.centre).absoluteValue > abs_v.dot(nodeAABB.extents)) {
 				continue
 			}
 
@@ -698,8 +698,8 @@ class DynamicTree : IDynamicTree {
 					maxFraction = value
 					t = p1 + diff * maxFraction
 					segmentAABB = AABB(
-						mins = p1.minimumPerComponent(t),
+						mins = p1.coerceAtMost(t),
-						maxs = p1.maximumPerComponent(t)
+						maxs = p1.coerceAtLeast(t)
 					)
 				}
 			} else {

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

@@ -1,8 +1,8 @@
 package ru.dbotthepony.kbox2d.collision
 
 import ru.dbotthepony.kbox2d.api.*
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.times
 import kotlin.math.absoluteValue
 
 var b2_toiCalls = 0
@@ -133,7 +133,7 @@ private class SeparationFunction(
 				return MinSeparationResult(
 					indexA = indexA,
 					indexB = indexB,
-					separation = (pointB - pointA).dotProduct(axis)
+					separation = (pointB - pointA).dot(axis)
 				)
 			}
 
@@ -149,7 +149,7 @@ private class SeparationFunction(
 				val localPointB = proxyB.vertices[indexB]
 				val pointB = xfB.times(localPointB)
 
-				val separation = (pointB - pointA).dotProduct(normal)
+				val separation = (pointB - pointA).dot(normal)
 				return MinSeparationResult(
 					indexA = indexA,
 					indexB = indexB,
@@ -169,7 +169,7 @@ private class SeparationFunction(
 				val localPointA = proxyA.vertices[indexA]
 				val pointA = xfA.times(localPointA)
 
-				val separation = (pointA - pointB).dotProduct(normal)
+				val separation = (pointA - pointB).dot(normal)
 				return MinSeparationResult(
 					indexA = indexA,
 					indexB = indexB,
@@ -191,7 +191,7 @@ private class SeparationFunction(
 				val pointA = xfA.times(localPointA)
 				val pointB = xfB.times(localPointB)
 
-				return (pointB - pointA).dotProduct(axis)
+				return (pointB - pointA).dot(axis)
 			}
 
 			Type.FACE_A -> {
@@ -201,7 +201,7 @@ private class SeparationFunction(
 				val localPointB = proxyB.vertices[indexB]
 				val pointB = xfB.times(localPointB)
 
-				return (pointB - pointA).dotProduct(normal)
+				return (pointB - pointA).dot(normal)
 			}
 
 			Type.FACE_B -> {
@@ -211,7 +211,7 @@ private class SeparationFunction(
 				val localPointA = proxyA.vertices[indexA]
 				val pointA = xfA.times(localPointA)
 
-				return (pointA - pointB).dotProduct(normal)
+				return (pointA - pointB).dot(normal)
 			}
 		}
 	}

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

@@ -5,7 +5,7 @@ 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
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 internal fun b2CollideCircles(
 	circleA: CircleShape,

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

@@ -7,8 +7,8 @@ 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.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.times
 import java.util.*
 import kotlin.collections.ArrayList
 

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

@@ -6,7 +6,7 @@ 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 ru.dbotthepony.kvector.vector.ndouble.times
 import java.util.*
 import kotlin.collections.ArrayList
 

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

@@ -1,8 +1,8 @@
 package ru.dbotthepony.kbox2d.collision.shapes
 
 import ru.dbotthepony.kbox2d.api.*
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 /**
  * A chain shape is a free form sequence of line segments.
@@ -27,8 +27,8 @@ class ChainShape : IShape<ChainShape> {
 			val v1 = vertices[i - 1]
 			val v2 = vertices[i]
 
-			v1.isFiniteOrThrow { "Vertex at ${i - 1} is invalid" }
+			v1.requireIsValid { "Vertex at ${i - 1} is invalid" }
-			v2.isFiniteOrThrow { "Vertex at $i is invalid" }
+			v2.requireIsValid { "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)"

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

@@ -1,9 +1,9 @@
 package ru.dbotthepony.kbox2d.collision.shapes
 
 import ru.dbotthepony.kbox2d.api.*
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.times
 import kotlin.math.PI
 import kotlin.math.sqrt
 

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

@@ -1,9 +1,9 @@
 package ru.dbotthepony.kbox2d.collision.shapes
 
 import ru.dbotthepony.kbox2d.api.*
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 /**
  * A line segment (edge) shape. These can be connected in chains or loops

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

@@ -1,10 +1,9 @@
 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.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 private const val inv3 = 1.0 / 3.0
 
@@ -100,7 +99,7 @@ class PolygonShape : IShape<PolygonShape> {
 			}
 		}
 
-		val hull = IntArrayList()
+		val hull = ArrayList<Int>()
 		var ih = i0
 
 		while (true) {
@@ -139,7 +138,7 @@ class PolygonShape : IShape<PolygonShape> {
 
 		// Copy vertices.
 		for (i in hull.indices) {
-			this.vertices.add(ps[hull.getInt(i)])
+			this.vertices.add(ps[hull[i]])
 		}
 
 		// Compute normals. Ensure the edges have non-zero length.
@@ -168,10 +167,10 @@ class PolygonShape : IShape<PolygonShape> {
 		vertices.add(Vector2d(hx, hy))
 		vertices.add(Vector2d(-hx, hy))
 
-		normals.add(Vector2d.DOWN)
+		normals.add(Vector2d.NEGATIVE_Y)
-		normals.add(Vector2d.RIGHT)
+		normals.add(Vector2d.POSITIVE_X)
-		normals.add(Vector2d.UP)
+		normals.add(Vector2d.POSITIVE_Y)
-		normals.add(Vector2d.LEFT)
+		normals.add(Vector2d.NEGATIVE_X)
 
 		centroid = Vector2d.ZERO
 	}
@@ -192,10 +191,10 @@ class PolygonShape : IShape<PolygonShape> {
 		vertices.add(Vector2d(hx, hy))
 		vertices.add(Vector2d(-hx, hy))
 
-		normals.add(Vector2d.DOWN)
+		normals.add(Vector2d.NEGATIVE_Y)
-		normals.add(Vector2d.RIGHT)
+		normals.add(Vector2d.POSITIVE_X)
-		normals.add(Vector2d.UP)
+		normals.add(Vector2d.POSITIVE_Y)
-		normals.add(Vector2d.LEFT)
+		normals.add(Vector2d.NEGATIVE_X)
 
 		centroid = center
 

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

@@ -10,10 +10,10 @@ 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.kvector.util2d.AABB
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.Color
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.util.Color
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 class B2World(override var gravity: Vector2d) : IB2World {
 	override var bodyCount: Int = 0
@@ -762,7 +762,7 @@ class B2World(override var gravity: Vector2d) : IB2World {
 				val circle = fixture.shape as CircleShape
 				val center = b2Mul(xf, circle.p)
 				val radius = circle.radius
-				val axis = b2Mul(xf.q, Vector2d.RIGHT)
+				val axis = b2Mul(xf.q, Vector2d.POSITIVE_X)
 
 				debugDraw?.drawSolidCircle(center, radius, axis, color)
 			}

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

@@ -1,7 +1,7 @@
 package ru.dbotthepony.kbox2d.dynamics
 
 import ru.dbotthepony.kbox2d.api.*
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 
 open class Body(def: BodyDef, world: IB2World) : IBody {
 	private var _world: IB2World? = world
@@ -62,7 +62,7 @@ open class Body(def: BodyDef, world: IB2World) : IBody {
 			if (type == BodyType.STATIC)
 				return
 
-			if (value.dotProduct(value) > 0.0)
+			if (value.dot(value) > 0.0)
 				isAwake = true
 
 			field = value
@@ -369,7 +369,7 @@ open class Body(def: BodyDef, world: IB2World) : IBody {
 
 		if (rotInertia > 0.0 && !isFixedRotation) {
 			// Center the inertia about the center of mass.
-			rotInertia -= mass * localCenter.dotProduct(localCenter)
+			rotInertia -= mass * localCenter.dot(localCenter)
 			check(rotInertia > 0.0)
 			rotInertiaInv = 1.0 / rotInertia
 		} else {
@@ -413,7 +413,7 @@ open class Body(def: BodyDef, world: IB2World) : IBody {
 			invMass = 1.0 / mass
 
 			if (value.inertia > 0.0 && !isFixedRotation) {
-				rotInertia = value.inertia - mass * value.center.dotProduct(value.center)
+				rotInertia = value.inertia - mass * value.center.dot(value.center)
 				check(rotInertia > 0.0)
 				rotInertiaInv = 1.0 / rotInertia
 			}
@@ -478,7 +478,7 @@ open class Body(def: BodyDef, world: IB2World) : IBody {
 	}
 
 	override val inertia: Double
-		get() = rotInertia + mass * sweep.localCenter.dotProduct(sweep.localCenter)
+		get() = rotInertia + mass * sweep.localCenter.dot(sweep.localCenter)
 
 	override val localCenter: Vector2d
 		get() = sweep.localCenter

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

@@ -1,11 +1,11 @@
 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
+import java.util.*
+import kotlin.collections.HashMap
 
 fun interface ContactFactory {
 	fun factorize(fixtureA: IFixture, childIndexA: Int, fixtureB: IFixture, childIndexB: Int): AbstractContact
@@ -161,11 +161,10 @@ sealed class AbstractContact(
 
 	companion object {
 		private val registry =
-			Object2ObjectArrayMap<IShape.Type, Object2ObjectArrayMap<IShape.Type, ContactFactory>>()
+			EnumMap<IShape.Type, EnumMap<IShape.Type, ContactFactory>>(IShape.Type::class.java)
 
 		internal fun register(type1: IShape.Type, type2: IShape.Type, factory: ContactFactory) {
-			registry.computeIfAbsent(type1, Object2ObjectFunction { Object2ObjectArrayMap() })
+			registry.computeIfAbsent(type1) { EnumMap(IShape.Type::class.java) }[type2] = factory
-				.put(type2, factory)
 		}
 
 		internal fun create(

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

@@ -4,9 +4,10 @@ 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.kvector.matrix.ndouble.Matrix2d
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.matrix.ndouble.MutableMatrix2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.times
 import java.lang.IllegalArgumentException
 import kotlin.math.absoluteValue
 
@@ -161,8 +162,8 @@ internal class ContactSolver(
 				invIA = bodyA.rotInertiaInv,
 				invIB = bodyB.rotInertiaInv,
 				contactIndex = i,
-				K = MutableMatrix2d(m00 = 0.0, m11 = 0.0),
+				K = MutableMatrix2d.zero(),
-				normalMass = MutableMatrix2d(m00 = 0.0, m11 = 0.0),
+				normalMass = MutableMatrix2d.zero(),
 				normal = Vector2d.ZERO,
 				points = Array(manifold.points.size) { VelocityCostantPoint() }
 			)
@@ -301,11 +302,11 @@ internal class ContactSolver(
 					// vc->K.ex.Set(k11, k12);
 					// vc->K.ey.Set(k12, k22);
 
-					vc.K.m00 = k11
+					vc.K.r00 = k11
-					vc.K.m10 = k12
+					vc.K.r10 = k12
-					vc.K.m01 = k12
+					vc.K.r01 = k12
-					vc.K.m11 = k22
+					vc.K.r11 = k22
-					vc.normalMass = vc.K.getInverse().asMutableMatrix()
+					vc.normalMass = (vc.K.inverse ?: Matrix2d.ZERO).toMutableMatrix2d()
 				} else {
 					// The constraints are redundant, just use one.
 					// TODO_ERIN use deepest?
@@ -514,8 +515,8 @@ internal class ContactSolver(
 							dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA)
 
 							// Compute normal velocity
-							vn1 = dv1.dotProduct(normal)
+							vn1 = b2Dot(dv1, normal)
-							vn2 = dv2.dotProduct(normal)
+							vn2 = b2Dot(dv2, normal)
 
 							check((vn1 - cp1.velocityBias).absoluteValue < k_errorTol) { (vn1 - cp1.velocityBias).absoluteValue }
 							check((vn2 - cp2.velocityBias).absoluteValue < k_errorTol) { (vn2 - cp2.velocityBias).absoluteValue }
@@ -534,7 +535,7 @@ internal class ContactSolver(
 
 					vn1 = 0.0
 					// vn2 = vc->K.ex.y * x.x + b.y;
-					vn2 = vc.K.m10 * x.x + b.y
+					vn2 = vc.K.r10 * x.x + b.y
 
 					if (x.x >= 0.0 && vn2 >= 0.0) {
 						// Get the incremental impulse
@@ -574,7 +575,7 @@ internal class ContactSolver(
 					//   0 = a21 * 0 + a22 * x2 + b2'
 					//
 					x = Vector2d(y = -cp2.normalMass * b.y)
-					vn1 = vc.K.m01 * x.y + b.x
+					vn1 = vc.K.r01 * x.y + b.x
 					vn2 = 0.0
 
 					if (x.y >= 0.0 && vn1 >= 0.0) {

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

@@ -6,8 +6,8 @@ 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.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.times
 import java.util.*
 import kotlin.collections.ArrayList
 import kotlin.math.absoluteValue
@@ -266,7 +266,7 @@ internal class Island(
 
 			// Check for large velocities
 			val translation = h * v
-			if (translation.dotProduct(translation) > b2_maxTranslationSquared) {
+			if (translation.dot(translation) > b2_maxTranslationSquared) {
 				v *= b2_maxTranslation / translation.length
 			}
 
@@ -330,7 +330,7 @@ internal class Island(
 				if (
 					!body.allowAutoSleep ||
 					body.angularVelocity * body.angularVelocity > angTolSqr ||
-					body.linearVelocity.dotProduct(body.linearVelocity) > linTolSqr
+					body.linearVelocity.dot(body.linearVelocity) > linTolSqr
 				) {
 					body.sleepTime = 0.0
 					minSleepTime = 0.0
@@ -434,7 +434,7 @@ internal class Island(
 
 			// Check for large velocities
 			val translation = h * v
-			if (translation.dotProduct(translation) > b2_maxTranslationSquared) {
+			if (translation.dot(translation) > b2_maxTranslationSquared) {
 				v *= b2_maxTranslation / translation.length
 			}
 

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

@@ -1,10 +1,9 @@
 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 ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import kotlin.math.PI
+import java.util.EnumMap
 
 fun interface JointFactory {
 	fun factorize(jointDef: IJointDef): AbstractJoint
@@ -85,7 +84,7 @@ sealed class AbstractJoint(def: IJointDef) : IJoint {
 	}
 
 	companion object {
-		private val registry = Object2ObjectArrayMap<JointType, JointFactory>()
+		private val registry = EnumMap<JointType, JointFactory>(JointType::class.java)
 
 		internal fun register(jointType: JointType, factory: JointFactory) {
 			require(registry.put(jointType, factory) == null) { "Re-registered $jointType factory" }

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

@@ -3,9 +3,9 @@ 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.kvector.vector.Color
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.util.Color
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 class DistanceJoint(def: DistanceJointDef) : AbstractJoint(def) {
 	var stiffness: Double = def.stiffness
@@ -254,7 +254,7 @@ class DistanceJoint(def: DistanceJointDef) : AbstractJoint(def) {
 		val rB = b2Mul(qB, localAnchorB - localCenterB)
 		var u = cB + rB - cA - rA
 
-		u.isFiniteOrThrow {
+		u.requireIsValid {
 			"u is invalid, $cB, $rB, $cA, $rA"
 		}
 
@@ -275,7 +275,7 @@ class DistanceJoint(def: DistanceJointDef) : AbstractJoint(def) {
 		val impulse = -mass * C
 		val P = impulse * u
 
-		P.isFiniteOrThrow {
+		P.requireIsValid {
 			"P is not finite, impulse: $impulse, u: $u, mass: $mass, C: $C"
 		}
 

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

@@ -3,9 +3,10 @@ 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.kvector.matrix.ndouble.Matrix2d
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.matrix.ndouble.MutableMatrix2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 // Point-to-point constraint
 // Cdot = v2 - v1
@@ -101,12 +102,12 @@ class FrictionJoint(def: FrictionJointDef) : AbstractJoint(def) {
 		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.r00 = 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.r10 = -iA * this.rA.x * this.rA.y - iB * this.rB.x * this.rB.y
-		K.m01 = K.m10
+		K.r01 = K.r10
-		K.m11 = mA + mB + iA * this.rA.x * this.rA.x + iB * this.rB.x * this.rB.x
+		K.r11 = mA + mB + iA * this.rA.x * this.rA.x + iB * this.rB.x * this.rB.x
 
-		this.linearMass = K.getInverse().asMutableMatrix()
+		this.linearMass = (K.inverse ?: Matrix2d.ZERO).toMutableMatrix2d()
 
 		this.angularMass = iA + iB
 		if (this.angularMass > 0.0) {

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

@@ -2,8 +2,8 @@ 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.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 // Gear Joint:
 // C0 = (coordinate1 + ratio * coordinate2)_initial

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

@@ -3,9 +3,10 @@ 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.kvector.matrix.ndouble.Matrix2d
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.matrix.ndouble.MutableMatrix2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 class MotorJoint(def: MotorJointDef) : AbstractJoint(def) {
 	// Solver shared
@@ -112,12 +113,12 @@ class MotorJoint(def: MotorJointDef) : AbstractJoint(def) {
 
 		// 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.r00 = 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.r10 = -iA * this.rA.x * this.rA.y - iB * this.rB.x * this.rB.y
-		K.m01 = K.m10
+		K.r01 = K.r10
-		K.m11 = mA + mB + iA * this.rA.x * this.rA.x + iB * this.rB.x * this.rB.x
+		K.r11 = mA + mB + iA * this.rA.x * this.rA.x + iB * this.rB.x * this.rB.x
 
-		this.linearMass = K.getInverse().asMutableMatrix()
+		this.linearMass = (K.inverse ?: Matrix2d.ZERO).toMutableMatrix2d()
 
 		this.angularMass = iA + iB
 		if (this.angularMass > 0.0) {

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

@@ -3,9 +3,10 @@ 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.kvector.matrix.ndouble.Matrix2d
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.matrix.ndouble.MutableMatrix2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 // p = attached point, m = mouse point
 // C = p - m
@@ -19,7 +20,7 @@ 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" }
+			value.requireIsValid { "Tried to set illegal target $value" }
 			field = value
 			bodyB.isAwake = true
 		}
@@ -40,7 +41,7 @@ class MouseJoint(def: MouseJointDef) : AbstractJoint(def) {
 	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 mass: MutableMatrix2d = MutableMatrix2d.zero()
 	private var C: Vector2d = Vector2d.ZERO
 
 	override fun initVelocityConstraints(data: B2SolverData) {
@@ -78,12 +79,12 @@ class MouseJoint(def: MouseJointDef) : AbstractJoint(def) {
 		//      = [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.r00 = invMassB + invIB * rB.y * rB.y + gamma
-		K.m10 = -invIB * rB.x * rB.y
+		K.r10 = -invIB * rB.x * rB.y
-		K.m01 = K.m10
+		K.r01 = K.r10
-		K.m11 = invMassB + invIB * rB.x * rB.x + gamma
+		K.r11 = invMassB + invIB * rB.x * rB.x + gamma
 
-		mass = K.getInverse().asMutableMatrix()
+		mass = (K.inverse ?: Matrix2d.ZERO).toMutableMatrix2d()
 
 		C = cB + rB - targetA
 		C *= beta

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

@@ -4,8 +4,12 @@ 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.kvector.matrix.ndouble.MutableMatrix2d
-import ru.dbotthepony.kstarbound.util.Color
+import ru.dbotthepony.kvector.matrix.ndouble.MutableMatrix3d
+import ru.dbotthepony.kvector.vector.Color
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector3d
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 // Linear constraint (point-to-line)
 // d = p2 - p1 = x2 + r2 - x1 - r1
@@ -145,11 +149,11 @@ class PrismaticJoint(def: PrismaticJointDef) : AbstractJoint(def) {
 				k22 = 1.0
 			}
 
-			this.K.m00 = k11
+			this.K.r00 = k11
-			this.K.m10 = k12
+			this.K.r10 = k12
 
-			this.K.m01 = k12
+			this.K.r01 = k12
-			this.K.m11 = k22
+			this.K.r11 = k22
 		}
 
 		if (enableLimit) {
@@ -374,17 +378,17 @@ class PrismaticJoint(def: PrismaticJointDef) : AbstractJoint(def) {
 
 			val K = MutableMatrix3d()
 
-			K.m00 = k11
+			K.r00 = k11
-			K.m10 = k12
+			K.r10 = k12
-			K.m20 = k13
+			K.r20 = k13
 
-			K.m01 = k12
+			K.r01 = k12
-			K.m11 = k22
+			K.r11 = k22
-			K.m21 = k23
+			K.r21 = k23
 
-			K.m02 = k13
+			K.r02 = k13
-			K.m12 = k23
+			K.r12 = k23
-			K.m22 = k33
+			K.r22 = k33
 
 			val C = Vector3d(
 				x = C1.x,
@@ -404,11 +408,11 @@ class PrismaticJoint(def: PrismaticJointDef) : AbstractJoint(def) {
 
 			val K = MutableMatrix2d()
 
-			K.m00 = k11
+			K.r00 = k11
-			K.m10 = k12
+			K.r10 = k12
 
-			K.m01 = k12
+			K.r01 = k12
-			K.m11 = k22
+			K.r11 = k22
 
 			val impulse1 = K.solve(-C1)
 			impulse = Vector3d(impulse1.x, impulse1.y)

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

@@ -3,8 +3,8 @@ 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.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 /**
  * The pulley joint is connected to two bodies and two fixed ground points.

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

@@ -3,10 +3,10 @@ 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.kvector.matrix.ndouble.MutableMatrix2d
-import ru.dbotthepony.kstarbound.math.Vector2d
+import ru.dbotthepony.kvector.vector.Color
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.util.Color
+import ru.dbotthepony.kvector.vector.ndouble.times
 import kotlin.math.cos
 import kotlin.math.sin
 
@@ -44,7 +44,7 @@ class RevoluteJoint(def: RevoluteJointDef) : AbstractJoint(def) {
 	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 K: MutableMatrix2d = MutableMatrix2d.zero()
 	private var angle: Double = 0.0
 	private var axialMass: Double = 0.0
 
@@ -84,10 +84,10 @@ class RevoluteJoint(def: RevoluteJointDef) : AbstractJoint(def) {
 		val iA = invIA
 		val iB = invIB
 
-		K.m00 = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB
+		K.r00 = 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.r01 = -rA.y * rA.x * iA - rB.y * rB.x * iB
-		K.m10 = K.m01
+		K.r10 = K.r01
-		K.m11 = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB
+		K.r11 = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB
 
 		axialMass = iA + iB
 		val fixedRotation: Boolean
@@ -266,10 +266,10 @@ class RevoluteJoint(def: RevoluteJointDef) : AbstractJoint(def) {
 			val iB = this.invIB
 
 			val K = MutableMatrix2d()
-			K.m00 = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y
+			K.r00 = 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.r10 = -iA * rA.x * rA.y - iB * rB.x * rB.y
-			K.m01 = K.m10
+			K.r01 = K.r10
-			K.m11 = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x
+			K.r11 = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x
 
 			val impulse = -K.solve(C)
 

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

@@ -3,7 +3,12 @@ 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.*
+import ru.dbotthepony.kvector.matrix.ndouble.Matrix2d
+import ru.dbotthepony.kvector.matrix.ndouble.Matrix3d
+import ru.dbotthepony.kvector.matrix.ndouble.MutableMatrix3d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector3d
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 class WeldJoint(def: WeldJointDef) : AbstractJoint(def) {
 	var stiffness: Double = def.stiffness
@@ -28,7 +33,7 @@ class WeldJoint(def: WeldJointDef) : AbstractJoint(def) {
 	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() }
+	private var mass: MutableMatrix3d = MutableMatrix3d.zero()
 
 	override fun initVelocityConstraints(data: B2SolverData) {
 		this.indexA = this.bodyA.islandIndex
@@ -69,20 +74,20 @@ class WeldJoint(def: WeldJointDef) : AbstractJoint(def) {
 		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.r00 = 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.r01 = -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.r02 = -this.rA.y * iA - this.rB.y * iB
 
-		K.m10 = K.m01
+		K.r10 = K.r01
-		K.m11 = mA + mB + this.rA.x * this.rA.x * iA + this.rB.x * this.rB.x * iB
+		K.r11 = 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.r12 = this.rA.x * iA + this.rB.x * iB
 
-		K.m20 = K.m02
+		K.r20 = K.r02
-		K.m21 = K.m12
+		K.r21 = K.r12
-		K.m22 = iA + iB
+		K.r22 = iA + iB
 
 		if (this.stiffness > 0.0) {
-			this.mass = K.getInverse2().asMutableMatrix()
+			this.mass = K.toMatrix2d().inverse?.toMutableMatrix3d() ?: MutableMatrix3d.zero()
 
 			var invM = iA + iB
 
@@ -101,13 +106,13 @@ class WeldJoint(def: WeldJointDef) : AbstractJoint(def) {
 			this.bias = C * h * k * this.gamma
 
 			invM += this.gamma
-			this.mass.m22 = if (invM != 0.0) 1.0 / invM else 0.0
+			this.mass.r22 = if (invM != 0.0) 1.0 / invM else 0.0
-		} else if (K.m22 == 0.0) {
+		} else if (K.r22 == 0.0) {
-			this.mass = K.getInverse2().asMutableMatrix()
+			this.mass = K.toMatrix2d().inverse?.toMutableMatrix3d() ?: MutableMatrix3d.zero()
 			this.gamma = 0.0
 			this.bias = 0.0
 		} else {
-			this.mass = K.getInverse().asMutableMatrix()
+			this.mass = K.inverse?.toMutableMatrix3d() ?: MutableMatrix3d.zero()
 			this.gamma = 0.0
 			this.bias = 0.0
 		}
@@ -147,7 +152,7 @@ class WeldJoint(def: WeldJointDef) : AbstractJoint(def) {
 		if (this.stiffness > 0.0) {
 			val Cdot2 = wB - wA
 
-			val impulse2 = -this.mass.m22 * (Cdot2 + this.bias + this.gamma * this.impulse.z)
+			val impulse2 = -this.mass.r22 * (Cdot2 + this.bias + this.gamma * this.impulse.z)
 			this.impulse += Vector3d(z = impulse2)
 
 			wA -= iA * impulse2
@@ -209,15 +214,15 @@ class WeldJoint(def: WeldJointDef) : AbstractJoint(def) {
 		val angularError: Double
 
 		val K = MutableMatrix3d()
-		K.m00 = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB
+		K.r00 = 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.r01 = -rA.y * rA.x * iA - rB.y * rB.x * iB
-		K.m02 = -rA.y * iA - rB.y * iB
+		K.r02 = -rA.y * iA - rB.y * iB
-		K.m10 = K.m01
+		K.r10 = K.r01
-		K.m11 = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB
+		K.r11 = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB
-		K.m12 = rA.x * iA + rB.x * iB
+		K.r12 = rA.x * iA + rB.x * iB
-		K.m20 = K.m02
+		K.r20 = K.r02
-		K.m21 = K.m12
+		K.r21 = K.r12
-		K.m22 = iA + iB
+		K.r22 = iA + iB
 
 		if (this.stiffness > 0.0f) {
 			val C1 = cB + rB - cA - rA
@@ -242,7 +247,7 @@ class WeldJoint(def: WeldJointDef) : AbstractJoint(def) {
 			val C = Vector3d(C1.x, C1.y, C2)
 
 			val impulse: Vector3d
-			if (K.m22 > 0.0) {
+			if (K.r22 > 0.0) {
 				impulse = -K.solve(C)
 			} else {
 				val impulse2 = -K.solve(C1)

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

@@ -3,9 +3,9 @@ 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.kvector.vector.Color
-import ru.dbotthepony.kstarbound.math.times
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
-import ru.dbotthepony.kstarbound.util.Color
+import ru.dbotthepony.kvector.vector.ndouble.times
 
 class WheelJoint(def: WheelJointDef) : AbstractJoint(def) {
 	val localAnchorA: Vector2d = def.localAnchorA

src/kvector/kotlin/ru/dbotthepony/kvector/api/DoubleMatrix.kt

@@ -3,6 +3,11 @@
 
 package ru.dbotthepony.kvector.api
 
+import ru.dbotthepony.kvector.api.concrete.IMatrix2d
+import java.nio.ByteBuffer
+import java.nio.DoubleBuffer
+import java.nio.FloatBuffer
+
 interface IMatrixGetterDouble : IMatrixLike {
 	/**
 	 * @return component of this matrix, as double
@@ -17,6 +22,9 @@ interface IMatrixSetterDouble : IMatrixLike {
 	operator fun set(column: Int, row: Int, value: Double)
 }
 
+/**
+ * Defines methods applicable to matrices working with [Double]s
+ */
 interface IDoubleMatrix<T : IDoubleMatrix<T>> : IMatrixGetterDouble {
 	/**
 	 * Multiplies all matrix components by [other], returning new matrix as result
@@ -58,12 +66,12 @@ interface IDoubleMatrix<T : IDoubleMatrix<T>> : IMatrixGetterDouble {
 	operator fun minus(other: IMatrixGetterDouble): T
 
 	/**
-	 * This matrix trace. If matrix is not square matrix, null is returned.
+	 * This matrix' trace. If matrix is not square matrix, null is returned.
 	 */
 	val trace: Double?
 
 	/**
-	 * This matrix determinant. If matrix is not square matrix, null is returned.
+	 * This matrix' determinant. If matrix is not square matrix, null is returned.
 	 */
 	val determinant: Double?
 
@@ -71,6 +79,7 @@ interface IDoubleMatrix<T : IDoubleMatrix<T>> : IMatrixGetterDouble {
 	 * This matrix' inverse matrix (this * I = identity).
 	 *
 	 * This operation is computation intense, do cache its result.
+	 * It is not recommended computing inverse matrix of matrices with order of 6 or bigger.
 	 *
 	 * If this matrix is not square matrix, or determinant is zero, null is returned.
 	 */
@@ -93,6 +102,73 @@ interface IDoubleMatrix<T : IDoubleMatrix<T>> : IMatrixGetterDouble {
 	 * If this matrix is not square matrix, null is returned.
 	 */
 	val adjugate: T?
+
+	/**
+	 * Copies memory of this matrix to new [DoubleArray] in either [rowMajor] or column-major
+	 * order.
+	 *
+	 * @return newly constructed [DoubleArray]
+	 */
+	fun toDoubleArray(rowMajor: Boolean = false): DoubleArray {
+		val memory = DoubleArray(columns * rows)
+		var i = 0
+
+		if (rowMajor) {
+			for (row in 0 until rows) {
+				for (column in 0 until columns) {
+					memory[i++] = this[column, row]
+				}
+			}
+		} else {
+			for (column in 0 until columns) {
+				for (row in 0 until rows) {
+					memory[i++] = this[column, row]
+				}
+			}
+		}
+
+		return memory
+	}
+
+	/**
+	 * Writes this matrix to specified [output] at its current position
+	 * in either [rowMajor] or column-major order.
+	 */
+	fun write(output: ByteBuffer, rowMajor: Boolean = false) {
+		if (rowMajor) {
+			for (row in 0 until rows) {
+				for (column in 0 until columns) {
+					output.putDouble(this[column, row])
+				}
+			}
+		} else {
+			for (column in 0 until columns) {
+				for (row in 0 until rows) {
+					output.putDouble(this[column, row])
+				}
+			}
+		}
+	}
+
+	/**
+	 * Writes this matrix to specified [output] at its current position
+	 * in either [rowMajor] or column-major order.
+	 */
+	fun write(output: DoubleBuffer, rowMajor: Boolean = false) {
+		if (rowMajor) {
+			for (row in 0 until rows) {
+				for (column in 0 until columns) {
+					output.put(this[column, row])
+				}
+			}
+		} else {
+			for (column in 0 until columns) {
+				for (row in 0 until rows) {
+					output.put(this[column, row])
+				}
+			}
+		}
+	}
 }
 
 interface IMutableDoubleMatrix<T : IMutableDoubleMatrix<T>> : IMatrixSetterDouble {

src/kvector/kotlin/ru/dbotthepony/kvector/api/FloatMatrix.kt

@@ -3,6 +3,9 @@
 
 package ru.dbotthepony.kvector.api
 
+import java.nio.ByteBuffer
+import java.nio.FloatBuffer
+
 interface IMatrixGetterFloat : IMatrixLike {
 	/**
 	 * @return component of this matrix, as float
@@ -17,6 +20,9 @@ interface IMatrixSetterFloat : IMatrixLike {
 	operator fun set(column: Int, row: Int, value: Float)
 }
 
+/**
+ * Defines methods applicable to matrices working with [Float]s
+ */
 interface IFloatMatrix<T : IFloatMatrix<T>> : IMatrixGetterFloat {
 	/**
 	 * Multiplies all matrix components by [other], returning new matrix as result
@@ -93,6 +99,73 @@ interface IFloatMatrix<T : IFloatMatrix<T>> : IMatrixGetterFloat {
 	 * If this matrix is not square matrix, null is returned.
 	 */
 	val adjugate: T?
+
+	/**
+	 * Copies memory of this matrix to new [FloatArray] in either [rowMajor] or column-major
+	 * order.
+	 *
+	 * @return newly constructed [FloatArray]
+	 */
+	fun toFloatArray(rowMajor: Boolean = false): FloatArray {
+		val memory = FloatArray(columns * rows)
+		var i = 0
+
+		if (rowMajor) {
+			for (row in 0 until rows) {
+				for (column in 0 until columns) {
+					memory[i++] = this[column, row]
+				}
+			}
+		} else {
+			for (column in 0 until columns) {
+				for (row in 0 until rows) {
+					memory[i++] = this[column, row]
+				}
+			}
+		}
+
+		return memory
+	}
+
+	/**
+	 * Writes this matrix to specified [output] at its current position
+	 * in either [rowMajor] or column-major order.
+	 */
+	fun write(output: ByteBuffer, rowMajor: Boolean = false) {
+		if (rowMajor) {
+			for (row in 0 until rows) {
+				for (column in 0 until columns) {
+					output.putFloat(this[column, row])
+				}
+			}
+		} else {
+			for (column in 0 until columns) {
+				for (row in 0 until rows) {
+					output.putFloat(this[column, row])
+				}
+			}
+		}
+	}
+
+	/**
+	 * Writes this matrix to specified [output] at its current position
+	 * in either [rowMajor] or column-major order.
+	 */
+	fun write(output: FloatBuffer, rowMajor: Boolean = false) {
+		if (rowMajor) {
+			for (row in 0 until rows) {
+				for (column in 0 until columns) {
+					output.put(this[column, row])
+				}
+			}
+		} else {
+			for (column in 0 until columns) {
+				for (row in 0 until rows) {
+					output.put(this[column, row])
+				}
+			}
+		}
+	}
 }
 
 interface IMutableFloatMatrix<T : IMutableFloatMatrix<T>> : IMatrixSetterFloat {

src/kvector/kotlin/ru/dbotthepony/kvector/api/Matrix.kt

@@ -374,7 +374,7 @@ interface IMatrixComplement<T> {
 /**
  * Defines common operations with concrete square matrices, such as [translation], [scale]ing, etc
  */
-interface ISquareMatrix<T : ISquareMatrix<T, V>, V> {
+interface ISquareMatrix<T : ISquareMatrix<T, V, F>, V, F> {
 	/**
 	 * Current translation of this matrix.
 	 */
@@ -405,13 +405,13 @@ interface ISquareMatrix<T : ISquareMatrix<T, V>, V> {
 	 *
 	 * @return new matrix
 	 */
-	fun scale(vector: V): T
+	fun scale(vector: F): T
 }
 
 /**
  * Defines common mutable operations with concrete square matrices, such as [translation], [scale]ing, etc
  */
-interface IMutableSquareMatrix<T : IMutableSquareMatrix<T, V>, V> : ISquareMatrix<T, V> {
+interface IMutableSquareMatrix<T : IMutableSquareMatrix<T, V, F>, V, F> : ISquareMatrix<T, V, F> {
 	/**
 	 * Current translation of this matrix.
 	 *
@@ -419,6 +419,17 @@ interface IMutableSquareMatrix<T : IMutableSquareMatrix<T, V>, V> : ISquareMatri
 	 */
 	override var translation: V
 
+	/**
+	 * Does raw translation of this matrix by specified [vector],
+	 * writing result into this matrix.
+	 *
+	 * If you scaled and/or rotated this matrix, and want to move it
+	 * with scale and translation, use [translateWithMultiplication].
+	 *
+	 * @return this matrix
+	 */
+	fun translateMut(vector: V): T
+
 	/**
 	 * Does translation of this matrix by specified [vector],
 	 * with accounting of it's state, writing result into this matrix.
@@ -433,5 +444,5 @@ interface IMutableSquareMatrix<T : IMutableSquareMatrix<T, V>, V> : ISquareMatri
 	 *
 	 * @return this matrix
 	 */
-	fun scaleMut(vector: V): T
+	fun scaleMut(vector: F): T
 }

src/kvector/kotlin/ru/dbotthepony/kvector/api/concrete/DoubleMatrix.kt

@@ -187,7 +187,7 @@ interface IMatrix2d<T : IMatrix2d<T>> : IMatrix<T>, IDoubleMatrix<T>, ITransposa
 /**
  * [Matrix3d] and [MutableMatrix3d] implement this
  */
-interface IMatrix3d<T : IMatrix3d<T>> : IMatrix<T>, IDoubleMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector2d> {
+interface IMatrix3d<T : IMatrix3d<T>> : IMatrix<T>, IDoubleMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector2d, Vector3d> {
 	val c00: Double
 	val c10: Double
 	val c20: Double
@@ -383,7 +383,7 @@ interface IMatrix3d<T : IMatrix3d<T>> : IMatrix<T>, IDoubleMatrix<T>, ITransposa
 /**
  * [Matrix4d] and [MutableMatrix4d] implement this
  */
-interface IMatrix4d<T : IMatrix4d<T>> : IMatrix<T>, IDoubleMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector3d> {
+interface IMatrix4d<T : IMatrix4d<T>> : IMatrix<T>, IDoubleMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector3d, Vector4d> {
 	val c00: Double
 	val c10: Double
 	val c20: Double

src/kvector/kotlin/ru/dbotthepony/kvector/api/concrete/FloatMatrix.kt

@@ -11,6 +11,8 @@ import ru.dbotthepony.kvector.matrix.*
 import ru.dbotthepony.kvector.matrix.ndouble.*
 import ru.dbotthepony.kvector.matrix.nfloat.*
 import ru.dbotthepony.kvector.vector.nfloat.*
+import java.nio.ByteBuffer
+import java.nio.FloatBuffer
 
 /**
  * [Matrix2f] and [MutableMatrix2f] implement this
@@ -191,7 +193,7 @@ interface IMatrix2f<T : IMatrix2f<T>> : IMatrix<T>, IFloatMatrix<T>, ITransposab
 /**
  * [Matrix3f] and [MutableMatrix3f] implement this
  */
-interface IMatrix3f<T : IMatrix3f<T>> : IMatrix<T>, IFloatMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector2f> {
+interface IMatrix3f<T : IMatrix3f<T>> : IMatrix<T>, IFloatMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector2f, Vector3f> {
 	val c00: Float
 	val c10: Float
 	val c20: Float
@@ -387,7 +389,7 @@ interface IMatrix3f<T : IMatrix3f<T>> : IMatrix<T>, IFloatMatrix<T>, ITransposab
 /**
  * [Matrix4f] and [MutableMatrix4f] implement this
  */
-interface IMatrix4f<T : IMatrix4f<T>> : IMatrix<T>, IFloatMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector3f> {
+interface IMatrix4f<T : IMatrix4f<T>> : IMatrix<T>, IFloatMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector3f, Vector4f> {
 	val c00: Float
 	val c10: Float
 	val c20: Float

src/kvector/kotlin/ru/dbotthepony/kvector/matrix/Matrix4fStack.kt

@@ -0,0 +1,170 @@
+package ru.dbotthepony.kvector.matrix
+
+import ru.dbotthepony.kvector.api.IMatrixGetterFloat
+import ru.dbotthepony.kvector.matrix.nfloat.MutableMatrix4f
+import ru.dbotthepony.kvector.vector.nfloat.Vector3f
+import ru.dbotthepony.kvector.vector.nfloat.Vector4f
+
+/**
+ * This class represent Matrix stack using [MutableMatrix4f]. It is useful for 3D
+ * rendering because it allows to create single stack, push new matrix in any
+ * applicable context, pass stack to all rendering methods which will further push
+ * transformation matrices, etc.
+ *
+ * All operations are mutating last matrix in stack.
+ */
+class Matrix4fStack {
+	private val stack = ArrayDeque<MutableMatrix4f>()
+
+	/**
+	 * Last matrix in stack, you are free to modify it,
+	 * but if you need to do contained modifications, such
+	 * as inside your render subroutine (e.g. inside your entity drawer),
+	 * please do use [push] first with no arguments, perform (render and) matrix operations,
+	 * and then [pop] it.
+	 *
+	 * @return [MutableMatrix4f]
+	 */
+	val last get() = stack.last()
+
+	init {
+		stack.add(MutableMatrix4f())
+	}
+
+	/**
+	 * Pops last matrix from stack.
+	 *
+	 * @return this stack
+	 */
+	fun pop(): Matrix4fStack {
+		stack.removeLast()
+		return this
+	}
+
+	/**
+	 * Pushes matrix to stack.
+	 * By default, pushes a copy of last matrix, which is desirable in most cases.
+	 *
+	 * @return this stack
+	 */
+	fun push(matrix: MutableMatrix4f = last.toMutableMatrix4f()): Matrix4fStack {
+		stack.add(matrix)
+		return this
+	}
+
+	/**
+	 * Clears stack and pushes specified [matrix4f] into it as first element.
+	 * By default, pushes identity matrix.
+	 *
+	 * @return this stack
+	 */
+	fun clear(matrix4f: MutableMatrix4f = MutableMatrix4f()): Matrix4fStack {
+		stack.clear()
+		stack.add(matrix4f)
+		return this
+	}
+
+	/**
+	 * Calls [MutableMatrix4f.plusMut] on last matrix.
+	 *
+	 * @return this stack
+	 */
+	operator fun plus(other: IMatrixGetterFloat): Matrix4fStack {
+		last.plusMut(other)
+		return this
+	}
+
+	/**
+	 * Calls [MutableMatrix4f.minusMut] on last matrix.
+	 *
+	 * @return this stack
+	 */
+	operator fun minus(other: IMatrixGetterFloat): Matrix4fStack {
+		last.minusMut(other)
+		return this
+	}
+
+	/**
+	 * Calls [MutableMatrix4f.timesMut] on last matrix.
+	 *
+	 * @return this stack
+	 */
+	operator fun times(other: IMatrixGetterFloat): Matrix4fStack {
+		last.timesMut(other)
+		return this
+	}
+
+	/**
+	 * Calls [MutableMatrix4f.scaleMut] on last matrix.
+	 *
+	 * @return this stack
+	 */
+	fun scale(vector: Vector4f): Matrix4fStack {
+		last.scaleMut(vector)
+		return this
+	}
+
+	/**
+	 * Calls [MutableMatrix4f.scaleMut] on last matrix.
+	 * This is a convenience function, as it just create new [Vector4f].
+	 *
+	 * @return this stack
+	 */
+	fun scale(x: Float = 1f, y: Float = 1f, z: Float = 1f, w: Float = 1f): Matrix4fStack {
+		last.scaleMut(Vector4f(x, y, z, w))
+		return this
+	}
+
+	/**
+	 * Calls [MutableMatrix4f.translate] on last matrix.
+	 *
+	 * @return this stack
+	 */
+	fun translate(vector: Vector3f): Matrix4fStack {
+		last.translateMut(vector)
+		return this
+	}
+
+	/**
+	 * Sets [MutableMatrix4f.translation] on last matrix.
+	 * This is a convenience function, as it just create new [Vector3f].
+	 *
+	 * @return this stack
+	 */
+	fun translate(x: Float = 0f, y: Float = 0f, z: Float = 0f): Matrix4fStack {
+		last.translateMut(Vector3f(x, y, z))
+		return this
+	}
+
+	/**
+	 * Calls [MutableMatrix4f.translateWithMultiplicationMut] on last matrix.
+	 *
+	 * @return this stack
+	 */
+	fun translateWithMultiplication(vector: Vector3f): Matrix4fStack {
+		last.translateWithMultiplicationMut(vector)
+		return this
+	}
+
+	/**
+	 * Calls [MutableMatrix4f.translateWithMultiplicationMut] on last matrix.
+	 * This is a convenience function, as it just create new [Vector3f].
+	 *
+	 * @return this stack
+	 */
+	fun translateWithMultiplication(x: Float = 0f, y: Float = 0f, z: Float = 0f): Matrix4fStack {
+		last.translateWithMultiplicationMut(Vector3f(x, y, z))
+		return this
+	}
+
+	/**
+	 * Removes last matrix from stack, and pushes [matrix] to stack.
+	 *
+	 * @return this stack
+	 */
+	fun replace(matrix: MutableMatrix4f): Matrix4fStack {
+		stack.removeLast()
+		stack.add(matrix)
+		return this
+	}
+}

src/kvector/kotlin/ru/dbotthepony/kvector/matrix/functions.kt

@@ -37,10 +37,10 @@ fun multiplyMatrix(matrix1: IMatrixGetterDouble, matrix2: IMatrixGetterDouble):
 			var sum = 0.0
 
 			for (rowColumn in 0 until vectorized) {
-				sum += matrix1[row, rowColumn] * matrix2[rowColumn, column]
+				sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
 			}
 
-			output[row, column] = sum
+			output[column, row] = sum
 		}
 	}
 
@@ -75,10 +75,10 @@ fun multiplyMatrix(matrix1: IMatrixGetterFloat, matrix2: IMatrixGetterFloat): Fl
 			var sum = 0f
 
 			for (rowColumn in 0 until vectorized) {
-				sum += matrix1[row, rowColumn] * matrix2[rowColumn, column]
+				sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
 			}
 
-			output[row, column] = sum
+			output[column, row] = sum
 		}
 	}
 
@@ -113,10 +113,10 @@ fun multiplyMatrix(matrix1: IMatrixGetterInt, matrix2: IMatrixGetterInt): Int2Di
 			var sum = 0
 
 			for (rowColumn in 0 until vectorized) {
-				sum += matrix1[row, rowColumn] * matrix2[rowColumn, column]
+				sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
 			}
 
-			output[row, column] = sum
+			output[column, row] = sum
 		}
 	}
 
@@ -151,10 +151,10 @@ fun multiplyMatrix(matrix1: IMatrixGetterLong, matrix2: IMatrixGetterLong): Long
 			var sum = 0L
 
 			for (rowColumn in 0 until vectorized) {
-				sum += matrix1[row, rowColumn] * matrix2[rowColumn, column]
+				sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
 			}
 
-			output[row, column] = sum
+			output[column, row] = sum
 		}
 	}
 
@@ -189,10 +189,10 @@ fun multiplyMatrix(matrix1: IMatrixGetterShort, matrix2: IMatrixGetterShort): Sh
 			var sum = 0
 
 			for (rowColumn in 0 until vectorized) {
-				sum += matrix1[row, rowColumn] * matrix2[rowColumn, column]
+				sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
 			}
 
-			output[row, column] = sum.toShort()
+			output[column, row] = sum.toShort()
 		}
 	}
 
@@ -227,10 +227,10 @@ fun multiplyMatrix(matrix1: IMatrixGetterByte, matrix2: IMatrixGetterByte): Byte
 			var sum = 0L
 
 			for (rowColumn in 0 until vectorized) {
-				sum += matrix1[row, rowColumn] * matrix2[rowColumn, column]
+				sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
 			}
 
-			output[row, column] = sum.toByte()
+			output[column, row] = sum.toByte()
 		}
 	}
 

src/kvector/kotlin/ru/dbotthepony/kvector/matrix/ndouble/Matrix3d.kt

@@ -107,11 +107,11 @@ class Matrix3d : AbstractMatrixVd<Matrix3d>, IMatrix3d<Matrix3d> {
 		)
 	}
 
-	override fun scale(vector: Vector2d): Matrix3d {
+	override fun scale(vector: Vector3d): Matrix3d {
 		return rm(
 			r00 * vector.x, r01, r02,
 			r10, r11 * vector.y, r12,
-			r20, r21, r22
+			r20, r21, r22 * vector.z
 		)
 	}
 
@@ -164,7 +164,7 @@ class Matrix3d : AbstractMatrixVd<Matrix3d>, IMatrix3d<Matrix3d> {
  *
  * Vectorized access use [MutableVector3d], generic interface is represented by [IMatrix3d].
  */
-class MutableMatrix3d : AbstractMutableMatrixVd<MutableMatrix3d>, IMatrix3d<MutableMatrix3d>, IMutableSquareMatrix<MutableMatrix3d, Vector2d> {
+class MutableMatrix3d : AbstractMutableMatrixVd<MutableMatrix3d>, IMatrix3d<MutableMatrix3d>, IMutableSquareMatrix<MutableMatrix3d, Vector2d, Vector3d> {
 	constructor(
 		c00: Double = 1.0, c01: Double = 0.0, c02: Double = 0.0,
 		c10: Double = 0.0, c11: Double = 1.0, c12: Double = 0.0,
@@ -198,9 +198,10 @@ class MutableMatrix3d : AbstractMutableMatrixVd<MutableMatrix3d>, IMatrix3d<Muta
 		return this
 	}
 
-	override fun scaleMut(vector: Vector2d): MutableMatrix3d {
+	override fun scaleMut(vector: Vector3d): MutableMatrix3d {
 		r00 *= vector.x
 		r11 *= vector.y
+		r22 *= vector.z
 		return this
 	}
 
@@ -345,6 +346,12 @@ class MutableMatrix3d : AbstractMutableMatrixVd<MutableMatrix3d>, IMatrix3d<Muta
 		)
 	}
 
+	override fun translateMut(vector: Vector2d): MutableMatrix3d {
+		r02 += vector.x
+		r12 += vector.y
+		return this
+	}
+
 	override fun translateWithMultiplication(vector: Vector2d): MutableMatrix3d {
 		return rm(
 			r00, r01, r02 + vector.x * r00 + vector.y * r01,
@@ -353,11 +360,11 @@ class MutableMatrix3d : AbstractMutableMatrixVd<MutableMatrix3d>, IMatrix3d<Muta
 		)
 	}
 
-	override fun scale(vector: Vector2d): MutableMatrix3d {
+	override fun scale(vector: Vector3d): MutableMatrix3d {
 		return rm(
 			r00 * vector.x, r01, r02,
 			r10, r11 * vector.y, r12,
-			r20, r21, r22
+			r20, r21, r22 * vector.z
 		)
 	}
 

src/kvector/kotlin/ru/dbotthepony/kvector/matrix/ndouble/Matrix4d.kt

@@ -165,12 +165,12 @@ class Matrix4d : AbstractMatrixVd<Matrix4d>, IMatrix4d<Matrix4d> {
 		)
 	}
 
-	override fun scale(vector: Vector3d): Matrix4d {
+	override fun scale(vector: Vector4d): Matrix4d {
 		return rm(
 			r00 * vector.x, r01, r02, r03,
 			r10, r11 * vector.y, r12, r13,
 			r20, r21, r22 * vector.z, r23,
-			r30, r31, r32, r33,
+			r30, r31, r32, r33 * vector.w,
 		)
 	}
 
@@ -238,7 +238,7 @@ class Matrix4d : AbstractMatrixVd<Matrix4d>, IMatrix4d<Matrix4d> {
  * - Vectorized access has setter, and when writing [Vector4d] to it, it will update associated column/row values
  *
  */
-class MutableMatrix4d : AbstractMutableMatrixVd<MutableMatrix4d>, IMatrix4d<MutableMatrix4d>, IMutableSquareMatrix<MutableMatrix4d, Vector3d> {
+class MutableMatrix4d : AbstractMutableMatrixVd<MutableMatrix4d>, IMatrix4d<MutableMatrix4d>, IMutableSquareMatrix<MutableMatrix4d, Vector3d, Vector4d> {
 	constructor(
 		c00: Double = 1.0, c01: Double = 0.0, c02: Double = 0.0, c03: Double = 0.0,
 		c10: Double = 0.0, c11: Double = 1.0, c12: Double = 0.0, c13: Double = 0.0,
@@ -292,12 +292,12 @@ class MutableMatrix4d : AbstractMutableMatrixVd<MutableMatrix4d>, IMatrix4d<Muta
 		)
 	}
 
-	override fun scale(vector: Vector3d): MutableMatrix4d {
+	override fun scale(vector: Vector4d): MutableMatrix4d {
 		return rm(
 			r00 * vector.x, r01, r02, r03,
 			r10, r11 * vector.y, r12, r13,
 			r20, r21, r22 * vector.z, r23,
-			r30, r31, r32, r33,
+			r30, r31, r32, r33 * vector.w,
 		)
 	}
 
@@ -308,10 +308,11 @@ class MutableMatrix4d : AbstractMutableMatrixVd<MutableMatrix4d>, IMatrix4d<Muta
 		return this
 	}
 
-	override fun scaleMut(vector: Vector3d): MutableMatrix4d {
+	override fun scaleMut(vector: Vector4d): MutableMatrix4d {
 		c00 *= vector.x
 		c11 *= vector.y
 		c22 *= vector.z
+		c33 *= vector.w
 		return this
 	}
 
@@ -323,6 +324,13 @@ class MutableMatrix4d : AbstractMutableMatrixVd<MutableMatrix4d>, IMatrix4d<Muta
 			r23 = value.z
 		}
 
+	override fun translateMut(vector: Vector3d): MutableMatrix4d {
+		r03 += vector.x
+		r13 += vector.y
+		r23 += vector.z
+		return this
+	}
+
 	override var c00: Double
 		get() = memory[0, 0]
 		set(value) { memory[0, 0] = value }

src/kvector/kotlin/ru/dbotthepony/kvector/matrix/ndouble/MatrixVd.kt

@@ -176,6 +176,85 @@ abstract class AbstractMatrixVd<T : AbstractMatrixVd<T>> protected constructor(
 
 			return factorize(adjugateMatrix(memory))
 		}
+
+	override fun equals(other: Any?): Boolean {
+		if (this === other) return true
+		if (javaClass != other?.javaClass) return false
+		other as AbstractMatrixVd<*>
+
+		if (other.columns != columns || other.rows != rows) {
+			return false
+		}
+
+		for (column in 0 until columns) {
+			for (row in 0 until rows) {
+				if (memory[column, row] != other.memory[column, row]) {
+					return false
+				}
+			}
+		}
+
+		return true
+	}
+
+	override fun hashCode(): Int {
+		if (columns == 0 || rows == 0) {
+			return 0
+		}
+
+		var result = 0
+
+		for (column in 0 until columns) {
+			for (row in 0 until rows) {
+				result = 31 * result + memory[column, row].hashCode()
+			}
+		}
+
+		return result
+	}
+
+	override fun toString(): String {
+		val builder = StringBuilder("[\n")
+
+		val strings = arrayOfNulls<String>(columns * rows)
+
+		var width = 0
+		val columnWidths = IntArray(columns)
+
+		for (column in 0 until columns) {
+			var columnWidth = 0
+
+			for (row in 0 until rows) {
+				strings[column + row * columns] = memory[row, column].toString()
+				columnWidth = columnWidth.coerceAtLeast(strings[column + row * columns]!!.length)
+			}
+
+			columnWidths[column] = columnWidth + 1
+			width += columnWidth + 1
+		}
+
+		for (row in 0 until rows) {
+			builder.append("    ")
+
+			for (column in 0 until columns) {
+				builder.append(strings[column + row * columns]!!)
+
+				if (column == columns - 1) {
+					builder.append(";")
+				} else {
+					builder.append(",")
+				}
+
+				builder.append(" ".repeat(columnWidths[column] - strings[column + row * columns]!!.length))
+			}
+
+			builder.append("\n")
+		}
+
+		builder.append("]")
+
+		return builder.toString()
+	}
 }
 
 /**

src/kvector/kotlin/ru/dbotthepony/kvector/matrix/nfloat/Matrix3f.kt

@@ -109,11 +109,11 @@ class Matrix3f : AbstractMatrixVf<Matrix3f>, IMatrix3f<Matrix3f> {
 		)
 	}
 
-	override fun scale(vector: Vector2f): Matrix3f {
+	override fun scale(vector: Vector3f): Matrix3f {
 		return rm(
 			r00 * vector.x, r01, r02,
 			r10, r11 * vector.y, r12,
-			r20, r21, r22
+			r20, r21, r22 * vector.z
 		)
 	}
 
@@ -166,7 +166,7 @@ class Matrix3f : AbstractMatrixVf<Matrix3f>, IMatrix3f<Matrix3f> {
  *
  * Vectorized access use [MutableVector3f], generic interface is represented by [IMatrix3f].
  */
-class MutableMatrix3f : AbstractMutableMatrixVf<MutableMatrix3f>, IMutableSquareMatrix<MutableMatrix3f, Vector2f>, IMatrix3f<MutableMatrix3f> {
+class MutableMatrix3f : AbstractMutableMatrixVf<MutableMatrix3f>, IMutableSquareMatrix<MutableMatrix3f, Vector2f, Vector3f>, IMatrix3f<MutableMatrix3f> {
 	constructor(
 		c00: Float = 1f, c01: Float = 0f, c02: Float = 0f,
 		c10: Float = 0f, c11: Float = 1f, c12: Float = 0f,
@@ -200,9 +200,16 @@ class MutableMatrix3f : AbstractMutableMatrixVf<MutableMatrix3f>, IMutableSquare
 		return this
 	}
 
-	override fun scaleMut(vector: Vector2f): MutableMatrix3f {
+	override fun scaleMut(vector: Vector3f): MutableMatrix3f {
 		r00 *= vector.x
 		r11 *= vector.y
+		r22 *= vector.z
+		return this
+	}
+
+	override fun translateMut(vector: Vector2f): MutableMatrix3f {
+		r02 += vector.x
+		r12 += vector.y
 		return this
 	}
 
@@ -355,11 +362,11 @@ class MutableMatrix3f : AbstractMutableMatrixVf<MutableMatrix3f>, IMutableSquare
 		)
 	}
 
-	override fun scale(vector: Vector2f): MutableMatrix3f {
+	override fun scale(vector: Vector3f): MutableMatrix3f {
 		return rm(
 			r00 * vector.x, r01, r02,
 			r10, r11 * vector.y, r12,
-			r20, r21, r22
+			r20, r21, r22 * vector.z
 		)
 	}
 

src/kvector/kotlin/ru/dbotthepony/kvector/matrix/nfloat/Matrix4f.kt

@@ -9,6 +9,7 @@ import ru.dbotthepony.kvector.matrix.ndouble.Matrix4d
 import ru.dbotthepony.kvector.matrix.ndouble.MutableMatrix4d
 import ru.dbotthepony.kvector.narray.Float2Dimensional
 import ru.dbotthepony.kvector.vector.nfloat.*
+import kotlin.math.tan
 
 /**
  * Represents immutable concrete matrix with 4x4 dimensions, storing values as [Float]s.
@@ -150,12 +151,12 @@ class Matrix4f : AbstractMatrixVf<Matrix4f>, IMatrix4f<Matrix4f> {
 		)
 	}
 
-	override fun scale(vector: Vector3f): Matrix4f {
+	override fun scale(vector: Vector4f): Matrix4f {
 		return rm(
 			r00 * vector.x, r01, r02, r03,
 			r10, r11 * vector.y, r12, r13,
 			r20, r21, r22 * vector.z, r23,
-			r30, r31, r32, r33,
+			r30, r31, r32, r33 * vector.w,
 		)
 	}
 
@@ -201,6 +202,52 @@ class Matrix4f : AbstractMatrixVf<Matrix4f>, IMatrix4f<Matrix4f> {
 		 * Main diagonal of this matrix is 1s
 		 */
 		@JvmField val IDENTITY = Matrix4f()
+
+		/**
+		 * Returns new ortho projection matrix, with Y coordinate inverted (useful for OpenGL GUI drawing).
+		 *
+		 * Inversion of Y means that X 0 Y 0 will be top left position on screen (in OpenGL), not bottom left.
+		 */
+		fun ortho(left: Float, right: Float, bottom: Float, top: Float, zNear: Float, zFar: Float): Matrix4f {
+			return rm(
+				r00 = 2f / (right - left),
+				r11 = -2f / (top - bottom),
+				r22 = 2f / (zFar - zNear),
+				r03 = -(right + left) / (right - left),
+				r13 = -(top + bottom) / (top - bottom) + 2f,
+				r23 = -(zFar + zNear) / (zFar - zNear)
+			)
+		}
+
+		/**
+		 * Returns new ortho projection matrix.
+		 */
+		fun orthoDirect(left: Float, right: Float, bottom: Float, top: Float, zNear: Float, zFar: Float): Matrix4f {
+			return rm(
+				r00 = 2f / (right - left),
+				r11 = 2f / (top - bottom),
+				r22 = 2f / (zFar - zNear),
+				r03 = -(right + left) / (right - left),
+				r13 = -(top + bottom) / (top - bottom),
+				r23 = -(zFar + zNear) / (zFar - zNear)
+			)
+		}
+
+		/**
+		 * Returns new perspective matrix
+		 */
+		fun perspective(fov: Float, zFar: Float, zNear: Float): Matrix4f {
+			val scale = (1.0 / (tan(Math.toRadians(fov.toDouble()) / 2.0))).toFloat()
+			val r = zFar - zNear
+
+			return rm(
+				r00 = scale,
+				r11 = scale,
+				r22 = -zFar / r,
+				r23 = -1f,
+				r32 = -zFar * zNear / r,
+			)
+		}
 	}
 }
 
@@ -211,7 +258,7 @@ class Matrix4f : AbstractMatrixVf<Matrix4f>, IMatrix4f<Matrix4f> {
  *
  * Vectorized access use [MutableVector4f], generic interface is represented by [IMatrix4f].
  */
-class MutableMatrix4f : AbstractMutableMatrixVf<MutableMatrix4f>, IMatrix4f<MutableMatrix4f>, IMutableSquareMatrix<MutableMatrix4f, Vector3f> {
+class MutableMatrix4f : AbstractMutableMatrixVf<MutableMatrix4f>, IMatrix4f<MutableMatrix4f>, IMutableSquareMatrix<MutableMatrix4f, Vector3f, Vector4f> {
 	constructor(
 		c00: Float = 1f, c01: Float = 0f, c02: Float = 0f, c03: Float = 0f,
 		c10: Float = 0f, c11: Float = 1f, c12: Float = 0f, c13: Float = 0f,
@@ -257,6 +304,13 @@ class MutableMatrix4f : AbstractMutableMatrixVf<MutableMatrix4f>, IMatrix4f<Muta
 		)
 	}
 
+	override fun translateMut(vector: Vector3f): MutableMatrix4f {
+		r03 += vector.x
+		r13 += vector.y
+		r23 += vector.z
+		return this
+	}
+
 	override fun translateWithMultiplication(vector: Vector3f): MutableMatrix4f {
 		return rm(
 			r00, r01, r02, r03 + vector.x * r00 + vector.y * r01 + vector.z * r02,
@@ -266,12 +320,12 @@ class MutableMatrix4f : AbstractMutableMatrixVf<MutableMatrix4f>, IMatrix4f<Muta
 		)
 	}
 
-	override fun scale(vector: Vector3f): MutableMatrix4f {
+	override fun scale(vector: Vector4f): MutableMatrix4f {
 		return rm(
 			r00 * vector.x, r01, r02, r03,
 			r10, r11 * vector.y, r12, r13,
 			r20, r21, r22 * vector.z, r23,
-			r30, r31, r32, r33,
+			r30, r31, r32, r33 * vector.w,
 		)
 	}
 
@@ -282,10 +336,11 @@ class MutableMatrix4f : AbstractMutableMatrixVf<MutableMatrix4f>, IMatrix4f<Muta
 		return this
 	}
 
-	override fun scaleMut(vector: Vector3f): MutableMatrix4f {
+	override fun scaleMut(vector: Vector4f): MutableMatrix4f {
 		c00 *= vector.x
 		c11 *= vector.y
 		c22 *= vector.z
+		c33 *= vector.w
 		return this
 	}
 
@@ -539,5 +594,51 @@ class MutableMatrix4f : AbstractMutableMatrixVf<MutableMatrix4f>, IMatrix4f<Muta
 				c03 = 0f, c13 = 0f, c23 = 0f, c33 = 0f,
 			)
 		}
+
+		/**
+		 * Returns new ortho projection matrix, with Y coordinate inverted (useful for OpenGL GUI drawing).
+		 *
+		 * Inversion of Y means that X 0 Y 0 will be top left position on screen (in OpenGL), not bottom left.
+		 */
+		fun ortho(left: Float, right: Float, bottom: Float, top: Float, zNear: Float, zFar: Float): MutableMatrix4f {
+			return rm(
+				r00 = 2f / (right - left),
+				r11 = -2f / (top - bottom),
+				r22 = 2f / (zFar - zNear),
+				r03 = -(right + left) / (right - left),
+				r13 = -(top + bottom) / (top - bottom) + 2f,
+				r23 = -(zFar + zNear) / (zFar - zNear)
+			)
+		}
+
+		/**
+		 * Returns new ortho projection matrix.
+		 */
+		fun orthoDirect(left: Float, right: Float, bottom: Float, top: Float, zNear: Float, zFar: Float): MutableMatrix4f {
+			return rm(
+				r00 = 2f / (right - left),
+				r11 = 2f / (top - bottom),
+				r22 = 2f / (zFar - zNear),
+				r03 = -(right + left) / (right - left),
+				r13 = -(top + bottom) / (top - bottom),
+				r23 = -(zFar + zNear) / (zFar - zNear)
+			)
+		}
+
+		/**
+		 * Returns new perspective matrix
+		 */
+		fun perspective(fov: Float, zFar: Float, zNear: Float): MutableMatrix4f {
+			val scale = (1.0 / (tan(Math.toRadians(fov.toDouble()) / 2.0))).toFloat()
+			val r = zFar - zNear
+
+			return rm(
+				r00 = scale,
+				r11 = scale,
+				r22 = -zFar / r,
+				r23 = -1f,
+				r32 = -zFar * zNear / r,
+			)
+		}
 	}
 }

src/kvector/kotlin/ru/dbotthepony/kvector/matrix/nfloat/MatrixVf.kt

@@ -176,6 +176,85 @@ abstract class AbstractMatrixVf<T : AbstractMatrixVf<T>> protected constructor(
 
 			return factorize(adjugateMatrix(memory))
 		}
+
+	override fun equals(other: Any?): Boolean {
+		if (this === other) return true
+		if (javaClass != other?.javaClass) return false
+		other as AbstractMatrixVf<*>
+
+		if (other.columns != columns || other.rows != rows) {
+			return false
+		}
+
+		for (column in 0 until columns) {
+			for (row in 0 until rows) {
+				if (memory[column, row] != other.memory[column, row]) {
+					return false
+				}
+			}
+		}
+
+		return true
+	}
+
+	override fun hashCode(): Int {
+		if (columns == 0 || rows == 0) {
+			return 0
+		}
+
+		var result = 0
+
+		for (column in 0 until columns) {
+			for (row in 0 until rows) {
+				result = 31 * result + memory[column, row].hashCode()
+			}
+		}
+
+		return result
+	}
+
+	override fun toString(): String {
+		val builder = StringBuilder("[\n")
+
+		val strings = arrayOfNulls<String>(columns * rows)
+
+		var width = 0
+		val columnWidths = IntArray(columns)
+
+		for (column in 0 until columns) {
+			var columnWidth = 0
+
+			for (row in 0 until rows) {
+				strings[column + row * columns] = memory[row, column].toString()
+				columnWidth = columnWidth.coerceAtLeast(strings[column + row * columns]!!.length)
+			}
+
+			columnWidths[column] = columnWidth + 1
+			width += columnWidth + 1
+		}
+
+		for (row in 0 until rows) {
+			builder.append("    ")
+
+			for (column in 0 until columns) {
+				builder.append(strings[column + row * columns]!!)
+
+				if (column == columns - 1) {
+					builder.append(";")
+				} else {
+					builder.append(",")
+				}
+
+				builder.append(" ".repeat(columnWidths[column] - strings[column + row * columns]!!.length))
+			}
+
+			builder.append("\n")
+		}
+
+		builder.append("]")
+
+		return builder.toString()
+	}
 }
 
 /**

src/kvector/kotlin/ru/dbotthepony/kvector/util2d/AABB.kt

@@ -0,0 +1,475 @@
+
+@file:Suppress("unused")
+
+package ru.dbotthepony.kvector.util2d
+
+import ru.dbotthepony.kvector.api.IStruct2d
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
+import ru.dbotthepony.kvector.vector.nint.Vector2i
+import kotlin.math.absoluteValue
+
+data class IntersectionTime(
+	val invEntry: Vector2d,
+	val invExit: Vector2d,
+	val entry: Vector2d,
+	val exit: Vector2d,
+) {
+	companion object {
+		val ZERO = IntersectionTime(Vector2d.ZERO, Vector2d.ZERO, Vector2d.ZERO, Vector2d.ZERO)
+	}
+}
+
+data class SweepResult(
+	val normal: Vector2d,
+	val collisionTime: Double,
+	val intersectionTime: IntersectionTime
+) {
+	companion object {
+		val ZERO = SweepResult(Vector2d.ZERO, 1.0, IntersectionTime.ZERO)
+		val INTERSECT = SweepResult(Vector2d.ZERO, 0.0, IntersectionTime.ZERO)
+	}
+}
+
+/**
+ * Axis Aligned Bounding Box, represented by two points, [mins] as lowermost corner of BB,
+ * and [maxs] as uppermost corner of BB
+ */
+data class AABB(val mins: Vector2d, val maxs: Vector2d) {
+	init {
+		require(mins.x <= maxs.x) { "mins.x ${mins.x} is more than maxs.x ${maxs.x}" }
+		require(mins.y <= maxs.y) { "mins.y ${mins.y} is more than maxs.y ${maxs.y}" }
+	}
+
+	operator fun plus(other: AABB) = AABB(mins + other.mins, maxs + other.maxs)
+	operator fun minus(other: AABB) = AABB(mins - other.mins, maxs - other.maxs)
+	operator fun times(other: AABB) = AABB(mins * other.mins, maxs * other.maxs)
+	operator fun div(other: AABB) = AABB(mins / other.mins, maxs / other.maxs)
+
+	operator fun plus(other: Vector2d) = AABB(mins + other, maxs + other)
+	operator fun minus(other: Vector2d) = AABB(mins - other, maxs - other)
+	operator fun times(other: Vector2d) = AABB(mins * other, maxs * other)
+	operator fun div(other: Vector2d) = AABB(mins / other, maxs / other)
+
+	operator fun times(other: Double) = AABB(mins * other, maxs * other)
+	operator fun div(other: Double) = AABB(mins / other, maxs / other)
+
+	val xSpan get() = maxs.x - mins.x
+	val ySpan get() = maxs.y - mins.y
+	val centre get() = (mins + maxs) * 0.5
+
+	val A get() = mins
+	val B get() = Vector2d(mins.x, maxs.y)
+	val C get() = maxs
+	val D get() = Vector2d(maxs.x, mins.y)
+
+	val bottomLeft get() = A
+	val topLeft get() = B
+	val topRight get() = C
+	val bottomRight get() = D
+
+	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
+	}
+
+	/**
+	 * Checks whenever is this AABB intersect with [other]
+	 *
+	 * This method consider they intersect even if only one of axis are equal
+	 */
+	fun intersect(other: AABB): Boolean {
+		val intersectX: Boolean
+
+		if (xSpan <= other.xSpan)
+			intersectX = mins.x in other.mins.x .. other.maxs.x || maxs.x in other.mins.x .. other.maxs.x
+		else
+			intersectX = other.mins.x in mins.x .. maxs.x || other.maxs.x in mins.x .. maxs.x
+
+		if (!intersectX)
+			return false
+
+		val intersectY: Boolean
+
+		if (ySpan <= other.ySpan)
+			intersectY = mins.y in other.mins.y .. other.maxs.y || maxs.y in other.mins.y .. other.maxs.y
+		else
+			intersectY = other.mins.y in mins.y .. maxs.y || other.maxs.y in mins.y .. maxs.y
+
+		return intersectY
+	}
+
+	/**
+	 * Returns whenever [other] is contained (encased) inside this 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]
+	 *
+	 * Считается, что они НЕ пересекаются, если у них просто равна одна из осей
+	 */
+	fun intersectWeak(other: AABB): Boolean {
+		if (maxs.x == other.mins.x || mins.x == other.maxs.x || maxs.y == other.mins.y || mins.y == other.maxs.y)
+			return false
+
+		val intersectX: Boolean
+
+		if (xSpan <= other.xSpan)
+			intersectX = mins.x in other.mins.x .. other.maxs.x || maxs.x in other.mins.x .. other.maxs.x
+		else
+			intersectX = other.mins.x in mins.x .. maxs.x || other.maxs.x in mins.x .. maxs.x
+
+		if (!intersectX)
+			return false
+
+		val intersectY: Boolean
+
+		if (ySpan <= other.ySpan)
+			intersectY = mins.y in other.mins.y .. other.maxs.y || maxs.y in other.mins.y .. other.maxs.y
+		else
+			intersectY = other.mins.y in mins.y .. maxs.y || other.maxs.y in mins.y .. maxs.y
+
+		return intersectY
+	}
+
+	fun intersectionDepth(other: AABB): Vector2d {
+		val xDepth: Double
+		val yDepth: Double
+
+		val thisCentre = centre
+		val otherCentre = other.centre
+
+		if (thisCentre.x > otherCentre.x) {
+			// считаем, что мы вошли справа
+			xDepth = mins.x - other.maxs.x
+		} else {
+			// считаем, что мы вошли слева
+			xDepth = maxs.x - other.mins.x
+		}
+
+		if (thisCentre.y > otherCentre.y) {
+			// считаем, что мы вошли сверху
+			yDepth = mins.y - other.maxs.y
+		} else {
+			// считаем, что мы вошли снизу
+			yDepth = maxs.x - other.mins.x
+		}
+
+		return Vector2d(xDepth, yDepth)
+	}
+
+	fun distance(other: AABB): Double {
+		val intersectX: Boolean
+		val intersectY: Boolean
+
+		if (ySpan <= other.ySpan)
+			intersectY = mins.y in other.mins.y .. other.maxs.y || maxs.y in other.mins.y .. other.maxs.y
+		else
+			intersectY = other.mins.y in mins.y .. maxs.y || other.maxs.y in mins.y .. maxs.y
+
+		if (xSpan <= other.xSpan)
+			intersectX = mins.x in other.mins.x .. other.maxs.x || maxs.x in other.mins.x .. other.maxs.x
+		else
+			intersectX = other.mins.x in mins.x .. maxs.x || other.maxs.x in mins.x .. maxs.x
+
+		if (intersectY && intersectX) {
+			return 0.0
+		}
+
+		if (intersectX) {
+			return (mins.y - other.maxs.y).absoluteValue.coerceAtMost((maxs.y - other.mins.y).absoluteValue)
+		} else {
+			return (mins.x - other.maxs.x).absoluteValue.coerceAtMost((maxs.x - other.mins.x).absoluteValue)
+		}
+	}
+
+	fun pushOutFrom(other: AABB): Vector2d {
+		if (!intersect(other))
+			return Vector2d.ZERO
+
+		val depth = intersectionDepth(other)
+
+		if (depth.x.absoluteValue < depth.y.absoluteValue) {
+			return Vector2d(x = depth.x)
+		} else {
+			return Vector2d(y = depth.y)
+		}
+	}
+
+	/**
+	 * Рассчитывает "время" пересечения
+	 *
+	 * https://www.gamedev.net/tutorials/programming/general-and-gameplay-programming/swept-aabb-collision-detection-and-response-r3084/
+	 *
+	 * Исправленный комментатором той же статьи от hypernewbie
+	 */
+	fun intersectionTime(other: AABB, velocity: Vector2d): IntersectionTime {
+		val xInvEntry: Double
+		val yInvEntry: Double
+		val xInvExit: Double
+		val yInvExit: Double
+
+		if (velocity.x > 0.0) {
+			xInvEntry = other.mins.x - maxs.x
+			xInvExit = other.maxs.x - mins.x
+		} else {
+			xInvEntry = other.maxs.x - mins.x
+			xInvExit = other.mins.x - maxs.x
+		}
+
+		if (velocity.y > 0.0) {
+			yInvEntry = other.mins.y - maxs.y
+			yInvExit = other.maxs.y - mins.y
+		} else {
+			yInvEntry = other.maxs.y - mins.y
+			yInvExit = other.mins.y - maxs.y
+		}
+
+		var xEntry: Double
+		var yEntry: Double
+		val xExit: Double
+		val yExit: Double
+
+		if (velocity.x == 0.0) {
+			xEntry = Double.NEGATIVE_INFINITY
+			xExit = Double.POSITIVE_INFINITY
+		} else {
+			xEntry = xInvEntry / velocity.x
+			xExit = xInvExit / velocity.x
+		}
+
+		if (velocity.y == 0.0) {
+			yEntry = Double.NEGATIVE_INFINITY
+			yExit = Double.POSITIVE_INFINITY
+		} else {
+			yEntry = yInvEntry / velocity.y
+			yExit = yInvExit / velocity.y
+		}
+
+		if (yEntry > 1.0) yEntry = Double.NEGATIVE_INFINITY
+		if (xEntry > 1.0) xEntry = Double.NEGATIVE_INFINITY
+
+		return IntersectionTime(
+			Vector2d(xInvEntry, yInvEntry),
+			Vector2d(xInvExit, yInvExit),
+			Vector2d(xEntry, yEntry),
+			Vector2d(xExit, yExit),
+		)
+	}
+
+	/**
+	 * Рассчитывает нормаль пересечения и процент пути ("время"), на котором произошло столкновение.
+	 *
+	 * Если столкновение не произошло, то возвращается [SweepResult.ZERO]
+	 *
+	 * Внимание: Если пересечение уже произошло (т.е. другой AABB пересекается с this), то данный метод
+	 * вернёт заведомо ложный результат (т.е. "нет пересечения")
+	 *
+	 * https://www.gamedev.net/tutorials/programming/general-and-gameplay-programming/swept-aabb-collision-detection-and-response-r3084/
+	 *
+	 * Исправленный комментатором той же статьи от hypernewbie
+	 */
+	fun sweep(other: AABB, velocity: Vector2d): SweepResult {
+		val time = intersectionTime(other, velocity)
+		val (near, far, entry, exit) = time
+		val (xEntry, yEntry) = entry
+		val (xExit, yExit) = exit
+
+		val entryTime = xEntry.coerceAtLeast(yEntry)
+		val exitTime = xExit.coerceAtLeast(yExit)
+
+		// гарантированно нет столкновения
+		if (entryTime > exitTime || xEntry < 0.0 && yEntry < 0.0) {
+			return SweepResult.ZERO
+		}
+
+		if (xEntry < 0.0) {
+			if (maxs.x < other.mins.x || mins.x > other.maxs.x)
+				return SweepResult.ZERO
+		}
+
+		if (yEntry < 0.0) {
+			if (maxs.y < other.mins.y || mins.y > other.maxs.y)
+				return SweepResult.ZERO
+		}
+
+		val (xInvEntry, yInvEntry) = near
+		val normal: Vector2d
+
+		if (xEntry > yEntry) {
+			if (xInvEntry <= 0.0) { // исправление от меня: <= для тех случаев, когда мы уже на той же оси
+				normal = Vector2d.POSITIVE_X
+			} else {
+				normal = Vector2d.NEGATIVE_X
+			}
+		} else {
+			if (yInvEntry <= 0.0) { // исправление от меня: <= для тех случаев, когда мы уже на той же оси
+				normal = Vector2d.POSITIVE_Y
+			} else {
+				normal = Vector2d.NEGATIVE_Y
+			}
+		}
+
+		return SweepResult(normal, entryTime, time)
+	}
+
+	/**
+	 * Рассчитывает нормаль пересечения и процент пути ("время"), на котором произошло столкновение.
+	 *
+	 * Если столкновение не произошло, то возвращается [SweepResult.ZERO]
+	 *
+	 * Если данный AABB уже столкнулся с [other], возвращается [SweepResult.INTERSECT]
+	 *
+	 * https://www.gamedev.net/tutorials/programming/general-and-gameplay-programming/swept-aabb-collision-detection-and-response-r3084/
+	 */
+	fun safeSweep(other: AABB, velocity: Vector2d): SweepResult {
+		if (intersect(other)) {
+			return SweepResult.INTERSECT
+		}
+
+		return sweep(other, velocity)
+	}
+
+	/**
+	 * Возвращает AABB, который содержит в себе оба AABB
+	 */
+	fun combine(other: AABB): AABB {
+		val minX = mins.x.coerceAtMost(other.mins.x)
+		val minY = mins.y.coerceAtMost(other.mins.y)
+		val maxX = maxs.x.coerceAtLeast(other.maxs.x)
+		val maxY = maxs.y.coerceAtLeast(other.maxs.y)
+
+		return AABB(Vector2d(minX, minY), Vector2d(maxX, maxY))
+	}
+
+	companion object {
+		fun rectangle(pos: IStruct2d, width: Double, height: Double = width): AABB {
+			val (x, y) = pos
+
+			return AABB(
+				Vector2d(x - width / 2.0, y - height / 2.0),
+				Vector2d(x + width / 2.0, y + height / 2.0),
+			)
+		}
+	}
+}
+
+data class AABBi(val mins: Vector2i, val maxs: Vector2i) {
+	init {
+		require(mins.x <= maxs.x) { "mins.x ${mins.x} is more than maxs.x ${maxs.x}" }
+		require(mins.y <= maxs.y) { "mins.y ${mins.y} is more than maxs.y ${maxs.y}" }
+	}
+
+	operator fun plus(other: AABBi) = AABBi(mins + other.mins, maxs + other.maxs)
+	operator fun minus(other: AABBi) = AABBi(mins - other.mins, maxs - other.maxs)
+	operator fun times(other: AABBi) = AABBi(mins * other.mins, maxs * other.maxs)
+	operator fun div(other: AABBi) = AABBi(mins / other.mins, maxs / other.maxs)
+
+	operator fun plus(other: Vector2i) = AABBi(mins + other, maxs + other)
+	operator fun minus(other: Vector2i) = AABBi(mins - other, maxs - other)
+	operator fun times(other: Vector2i) = AABBi(mins * other, maxs * other)
+	operator fun div(other: Vector2i) = AABBi(mins / other, maxs / other)
+
+	operator fun times(other: Int) = AABBi(mins * other, maxs * other)
+	operator fun div(other: Int) = AABBi(mins / other, maxs / other)
+
+	val xSpan get() = maxs.x - mins.x
+	val ySpan get() = maxs.y - mins.y
+	val centre get() = (mins.toDoubleVector() + maxs.toDoubleVector()) * 0.5
+
+	val A get() = mins
+	val B get() = Vector2i(mins.x, maxs.y)
+	val C get() = maxs
+	val D get() = Vector2i(maxs.x, mins.y)
+
+	val bottomLeft get() = A
+	val topLeft get() = B
+	val topRight get() = C
+	val bottomRight get() = D
+
+	val width get() = (maxs.x - mins.x) / 2
+	val height get() = (maxs.y - mins.y) / 2
+
+	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
+	}
+
+	/**
+	 * Есть ли пересечение между этим AABB и [other]
+	 *
+	 * Считается, что они пересекаются, даже если у них просто равна одна из осей
+	 */
+	fun intersect(other: AABBi): Boolean {
+		val intersectX: Boolean
+
+		if (xSpan <= other.xSpan)
+			intersectX = mins.x in other.mins.x .. other.maxs.x || maxs.x in other.mins.x .. other.maxs.x
+		else
+			intersectX = other.mins.x in mins.x .. maxs.x || other.maxs.x in mins.x .. maxs.x
+
+		if (!intersectX)
+			return false
+
+		val intersectY: Boolean
+
+		if (ySpan <= other.ySpan)
+			intersectY = mins.y in other.mins.y .. other.maxs.y || maxs.y in other.mins.y .. other.maxs.y
+		else
+			intersectY = other.mins.y in mins.y .. maxs.y || other.maxs.y in mins.y .. maxs.y
+
+		return intersectY
+	}
+
+	/**
+	 * Есть ли пересечение между этим AABB и [other]
+	 *
+	 * Считается, что они НЕ пересекаются, если у них просто равна одна из осей
+	 */
+	fun intersectWeak(other: AABBi): Boolean {
+		if (maxs.x == other.mins.x || mins.x == other.maxs.x || maxs.y == other.mins.y || mins.y == other.maxs.y)
+			return false
+
+		val intersectX: Boolean
+
+		if (xSpan <= other.xSpan)
+			intersectX = mins.x in other.mins.x .. other.maxs.x || maxs.x in other.mins.x .. other.maxs.x
+		else
+			intersectX = other.mins.x in mins.x .. maxs.x || other.maxs.x in mins.x .. maxs.x
+
+		if (!intersectX)
+			return false
+
+		val intersectY: Boolean
+
+		if (ySpan <= other.ySpan)
+			intersectY = mins.y in other.mins.y .. other.maxs.y || maxs.y in other.mins.y .. other.maxs.y
+		else
+			intersectY = other.mins.y in mins.y .. maxs.y || other.maxs.y in mins.y .. maxs.y
+
+		return intersectY
+	}
+
+	fun toDoubleAABB() = AABB(mins.toDoubleVector(), maxs.toDoubleVector())
+}

src/kvector/kotlin/ru/dbotthepony/kvector/vector/Color.kt

@@ -0,0 +1,46 @@
+package ru.dbotthepony.kvector.vector
+
+import ru.dbotthepony.kvector.api.IStruct4f
+import ru.dbotthepony.kvector.vector.nfloat.Vector4f
+import java.util.Collections
+
+/**
+ * Represents RGBA color in [0, 1] range using [Float].
+ */
+class Color(r: Float, g: Float, b: Float, a: Float = 1f) : Vector4f(r, g, b, a) {
+	constructor(input: IStruct4f) : this(input.component1(), input.component2(), input.component3(), input.component4())
+
+	constructor(input: Int) : this(
+		((input ushr 16) and 0xFF).toFloat() / 255f,
+		((input ushr 8) and 0xFF).toFloat() / 255f,
+		(input and 0xFF).toFloat() / 255f,
+	)
+
+	fun copy(r: Float = this.r, g: Float = this.g, b: Float = this.b, a: Float = this.a): Color {
+		return Color(r, g, b, a)
+	}
+
+	companion object {
+		val WHITE = Color(1f, 1f, 1f)
+
+		val RED = Color(1f, 0f, 0f)
+		val GREEN = Color(0f, 1f, 0f)
+		val BLUE = Color(0f, 0f, 1f)
+
+		val SLATE_GREY = Color(0.2f, 0.2f, 0.2f)
+
+		val PRE_DEFINED_MAP = mapOf(
+			"red" to RED,
+			"green" to GREEN,
+			"blue" to BLUE,
+		)
+
+		val SHADES_OF_GRAY = ArrayList<Color>().let {
+			for (i in 0 .. 256) {
+				it.add(Color(i / 256f, i / 256f, i / 256f))
+			}
+
+			return@let Collections.unmodifiableList(it) as List<Color>
+		}
+	}
+}

src/kvector/kotlin/ru/dbotthepony/kvector/vector/ndouble/Vector2d.kt

@@ -6,8 +6,6 @@ package ru.dbotthepony.kvector.vector.ndouble
 import ru.dbotthepony.kvector.api.*
 import ru.dbotthepony.kvector.vector.nfloat.Vector2f
 import kotlin.math.absoluteValue
-import kotlin.math.pow
-import kotlin.math.sqrt
 
 /**
  * 2D Vector, representing two-dimensional coordinates as [Double]s
@@ -126,14 +124,14 @@ open class Vector2d(
 	}
 
 	/**
-	 * Calculates vector vector * vector, returning result as [Double]
+	 * Calculates vector vector * vector, returning result as [Double].
 	 */
 	fun cross(other: Vector2d): Double {
 		return x * other.y - y * other.x
 	}
 
 	/**
-	 * Calculates 2D cross product between scalar and vector
+	 * Calculates 2D cross product between scalar and vector.
 	 *
 	 * @return new vector
 	 */
@@ -142,21 +140,31 @@ open class Vector2d(
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Double],
+	 * but using sine instead of cosine.
+	 *
+	 * See [dot] and this method code for better explanation.
+	 */
+	fun pseudoDot(other: Vector2d): Double {
+		return other.x * y + other.y * x
+	}
+
+	/**
+	 * Calculates scalar vector * vector, returning result as [Double].
 	 */
 	fun dot(other: Vector2d): Double {
 		return other.x * x + other.y * y
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Double].
 	 */
 	fun dot(other: Vector3d): Double {
 		return other.x * x + other.y * y
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Double].
 	 */
 	fun dot(other: Vector4d): Double {
 		return other.x * x + other.y * y
@@ -189,6 +197,13 @@ open class Vector2d(
 		return 31 * x.hashCode() + y.hashCode()
 	}
 
+	/**
+	 * Returns copy of this vector as [MutableVector2d]
+	 */
+	fun toMutableVector(): MutableVector2d {
+		return MutableVector2d(x, y)
+	}
+
 	inline val xx get() = Vector2d(x, x)
 	inline val xy get() = Vector2d(x, y)
 	inline val yx get() = Vector2d(y, x)
@@ -280,14 +295,17 @@ open class Vector2d(
 		@JvmField val NEGATIVE_X = Vector2d(x = -1.0)
 		@JvmField val POSITIVE_Y = Vector2d(y = 1.0)
 		@JvmField val NEGATIVE_Y = Vector2d(y = -1.0)
+
+		@JvmField val POSITIVE_XY = Vector2d(1.0, 1.0)
+		@JvmField val NEGATIVE_XY = Vector2d(-1.0, -1.0)
 	}
 }
 
-fun Double.div(other: Vector2d): Vector2d {
+operator fun Double.div(other: Vector2d): Vector2d {
 	return Vector2d(this / other.x, this / other.y)
 }
 
-fun Double.times(other: Vector2d): Vector2d {
+operator fun Double.times(other: Vector2d): Vector2d {
 	return Vector2d(this * other.x, this * other.y)
 }
 
@@ -404,4 +422,11 @@ open class MutableVector2d(
 		this.y = -other * x
 		return this
 	}
+
+	/**
+	 * Returns copy of this vector as [Vector2d]
+	 */
+	fun toVector(): Vector2d {
+		return Vector2d(x, y)
+	}
 }

src/kvector/kotlin/ru/dbotthepony/kvector/vector/ndouble/Vector3d.kt

@@ -549,11 +549,11 @@ open class Vector3d(
 	}
 }
 
-fun Double.div(other: Vector3d): Vector3d {
+operator fun Double.div(other: Vector3d): Vector3d {
 	return Vector3d(this / other.x, this / other.y, this / other.z)
 }
 
-fun Double.times(other: Vector3d): Vector3d {
+operator fun Double.times(other: Vector3d): Vector3d {
 	return Vector3d(this * other.x, this * other.y, this * other.z)
 }
 

src/kvector/kotlin/ru/dbotthepony/kvector/vector/ndouble/Vector4d.kt

@@ -1242,11 +1242,11 @@ open class Vector4d(
 	}
 }
 
-fun Double.div(other: Vector4d): Vector4d {
+operator fun Double.div(other: Vector4d): Vector4d {
 	return Vector4d(this / other.x, this / other.y, this / other.z, this / other.w)
 }
 
-fun Double.times(other: Vector4d): Vector4d {
+operator fun Double.times(other: Vector4d): Vector4d {
 	return Vector4d(this * other.x, this * other.y, this * other.z, this * other.w)
 }
 

src/kvector/kotlin/ru/dbotthepony/kvector/vector/nfloat/Vector2f.kt

@@ -173,24 +173,34 @@ open class Vector2f(
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Float],
+	 * but using sine instead of cosine.
+	 *
+	 * See [dot] and this method code for better explanation.
 	 */
-	fun dot(other: Vector2f): Double {
+	fun pseudoDot(other: Vector2f): Float {
-		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble()
+		return other.x * y + other.y * x
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Float]
 	 */
-	fun dot(other: Vector3f): Double {
+	fun dot(other: Vector2f): Float {
-		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble()
+		return other.x * x + other.y * y
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Float]
 	 */
-	fun dot(other: Vector4f): Double {
+	fun dot(other: Vector3f): Float {
-		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble()
+		return other.x * x + other.y * y
+	}
+
+	/**
+	 * Calculates scalar vector * vector, returning result as [Float]
+	 */
+	fun dot(other: Vector4f): Float {
+		return other.x * x + other.y * y
 	}
 
 	/**
@@ -291,6 +301,9 @@ open class Vector2f(
 		@JvmField val NEGATIVE_X = Vector2f(x = -1f)
 		@JvmField val POSITIVE_Y = Vector2f(y = 1f)
 		@JvmField val NEGATIVE_Y = Vector2f(y = -1f)
+
+		@JvmField val POSITIVE_XY = Vector2f(1f, 1f)
+		@JvmField val NEGATIVE_XY = Vector2f(-1f, -1f)
 	}
 }
 

src/kvector/kotlin/ru/dbotthepony/kvector/vector/nfloat/Vector3f.kt

@@ -147,24 +147,24 @@ open class Vector3f(
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Float]
 	 */
-	fun dot(other: Vector4f): Double {
+	fun dot(other: Vector4f): Float {
-		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble() + other.z.toDouble() * z.toDouble()
+		return other.x * x + other.y * y + other.z * z
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Float]
 	 */
-	fun dot(other: Vector3f): Double {
+	fun dot(other: Vector3f): Float {
-		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble() + other.z.toDouble() * z.toDouble()
+		return other.x * x + other.y * y + other.z * z
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Float]
 	 */
-	fun dot(other: Vector2f): Double {
+	fun dot(other: Vector2f): Float {
-		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble()
+		return other.x * x + other.y * y
 	}
 
 	/**

src/kvector/kotlin/ru/dbotthepony/kvector/vector/nfloat/Vector4f.kt

@@ -165,24 +165,24 @@ open class Vector4f(
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Float]
 	 */
-	fun dot(other: Vector4f): Double {
+	fun dot(other: Vector4f): Float {
-		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble() + other.z.toDouble() * z.toDouble() + other.w.toDouble() * w.toDouble()
+		return other.x * x + other.y * y + other.z * z + other.w * w
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Float]
 	 */
-	fun dot(other: Vector3f): Double {
+	fun dot(other: Vector3f): Float {
-		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble() + other.z.toDouble() * z.toDouble()
+		return other.x * x + other.y * y + other.z * z
 	}
 
 	/**
-	 * Calculates scalar vector * vector, returning result as [Double]
+	 * Calculates scalar vector * vector, returning result as [Float]
 	 */
-	fun dot(other: Vector2f): Double {
+	fun dot(other: Vector2f): Float {
-		return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble()
+		return other.x * x + other.y * y
 	}
 
 	override fun equals(other: Any?): Boolean {

src/kvector/kotlin/ru/dbotthepony/kvector/vector/nint/Vector2i.kt

@@ -4,6 +4,7 @@
 package ru.dbotthepony.kvector.vector.nint
 
 import ru.dbotthepony.kvector.api.*
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 import kotlin.math.absoluteValue
 
 /**
@@ -149,6 +150,10 @@ open class Vector2i(
 		return "[${x}i ${y}i]"
 	}
 
+	fun toDoubleVector(): Vector2d {
+		return Vector2d(x.toDouble(), y.toDouble())
+	}
+
 	inline val xx get() = Vector2i(x, x)
 	inline val xy get() = Vector2i(x, y)
 	inline val yx get() = Vector2i(y, x)
@@ -240,6 +245,7 @@ open class Vector2i(
 		@JvmField val NEGATIVE_X = Vector2i(x = -1)
 		@JvmField val POSITIVE_Y = Vector2i(y = 1)
 		@JvmField val NEGATIVE_Y = Vector2i(y = -1)
+		@JvmField val POSITIVE_XY = Vector2i(1, 1)
 	}
 }
 

src/main/java/ru/dbotthepony/kstarbound/client/freetype/struct/FT_Vector.java

@@ -1,7 +1,7 @@
 package ru.dbotthepony.kstarbound.client.freetype.struct;
 
 import com.sun.jna.Structure;
-import ru.dbotthepony.kstarbound.math.Vector2i;
+import ru.dbotthepony.kvector.vector.nint.Vector2i;
 
 @Structure.FieldOrder({"x", "y"})
 public class FT_Vector extends Structure {

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

@@ -10,10 +10,10 @@ 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.math.Vector2d
 import ru.dbotthepony.kstarbound.world.Chunk
 import ru.dbotthepony.kstarbound.world.entities.Move
 import ru.dbotthepony.kstarbound.world.entities.PlayerEntity
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 import java.io.File
 import java.util.*
 import kotlin.collections.ArrayList
@@ -104,7 +104,7 @@ fun main() {
 	val mouseJoints = ArrayList<MouseJoint>()
 
 	run {
-		val stripes = 0
+		val stripes = 4
 
 		for (stripe in 0 until stripes) {
 			for (x in 0 .. (stripes - stripe)) {
@@ -232,7 +232,7 @@ fun main() {
 			bodyA = base,
 			bodyB = wheel1,
 			anchor = Vector2d(x = -2.0, y = 15.0),
-			axis = Vector2d.UP,
+			axis = Vector2d.POSITIVE_Y,
 			enableLimit = true,
 			upperTranslation = 0.25,
 			lowerTranslation = -0.25,
@@ -242,7 +242,7 @@ fun main() {
 			bodyA = base,
 			bodyB = wheel2,
 			anchor = Vector2d(x = 2.0, y = 15.0),
-			axis = Vector2d.UP,
+			axis = Vector2d.POSITIVE_Y,
 			enableLimit = true,
 			upperTranslation = 0.25,
 			lowerTranslation = -0.25,

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

@@ -9,11 +9,15 @@ import ru.dbotthepony.kstarbound.defs.*
 import ru.dbotthepony.kstarbound.defs.projectile.ConfigurableProjectile
 import ru.dbotthepony.kstarbound.defs.projectile.ConfiguredProjectile
 import ru.dbotthepony.kstarbound.defs.projectile.ProjectilePhysics
-import ru.dbotthepony.kstarbound.io.StarboundPak
+import ru.dbotthepony.kstarbound.io.*
 import ru.dbotthepony.kstarbound.math.*
-import ru.dbotthepony.kstarbound.util.Color
-import ru.dbotthepony.kstarbound.util.ColorTypeAdapter
 import ru.dbotthepony.kstarbound.util.CustomEnumTypeAdapter
+import ru.dbotthepony.kvector.util2d.AABB
+import ru.dbotthepony.kvector.util2d.AABBi
+import ru.dbotthepony.kvector.vector.Color
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
+import ru.dbotthepony.kvector.vector.nfloat.Vector2f
+import ru.dbotthepony.kvector.vector.nint.Vector2i
 import java.io.*
 import java.nio.ByteBuffer
 import java.text.DateFormat

src/main/kotlin/ru/dbotthepony/kstarbound/api/Structs.kt

@@ -1,40 +0,0 @@
-package ru.dbotthepony.kstarbound.api
-
-interface IStruct2f {
-	operator fun component1(): Float
-	operator fun component2(): Float
-}
-
-interface IStruct3f : IStruct2f {
-	operator fun component3(): Float
-}
-
-interface IStruct4f : IStruct3f {
-	operator fun component4(): Float
-}
-
-interface IStruct2d {
-	operator fun component1(): Double
-	operator fun component2(): Double
-}
-
-interface IStruct3d : IStruct2d {
-	operator fun component3(): Double
-}
-
-interface IStruct4d : IStruct3d {
-	operator fun component4(): Double
-}
-
-interface IStruct2i {
-	operator fun component1(): Int
-	operator fun component2(): Int
-}
-
-interface IStruct3i : IStruct2i {
-	operator fun component3(): Int
-}
-
-interface IStruct4i : IStruct3i {
-	operator fun component4(): Int
-}

src/main/kotlin/ru/dbotthepony/kstarbound/client/ClientChunk.kt

@@ -5,10 +5,10 @@ import ru.dbotthepony.kstarbound.client.render.BakedStaticMesh
 import ru.dbotthepony.kstarbound.client.render.EntityRenderer
 import ru.dbotthepony.kstarbound.client.render.ILayeredRenderer
 import ru.dbotthepony.kstarbound.client.render.TileLayerList
-import ru.dbotthepony.kstarbound.math.Matrix4fStack
-import ru.dbotthepony.kstarbound.math.Vector2d
 import ru.dbotthepony.kstarbound.world.*
 import ru.dbotthepony.kstarbound.world.entities.Entity
+import ru.dbotthepony.kvector.matrix.Matrix4fStack
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 import java.io.Closeable
 
 /**
@@ -227,7 +227,7 @@ class ClientChunk(world: ClientWorld, pos: ChunkPos) : Chunk<ClientWorld, Client
 		if (layerQueue.isEmpty())
 			return -1
 
-		stack.push().translateWithScale(x = pos.x * CHUNK_SIZEf, y = pos.y * CHUNK_SIZEf)
+		stack.push().translateWithMultiplication(x = pos.x * CHUNK_SIZEf, y = pos.y * CHUNK_SIZEf)
 		var pair = layerQueue.last()
 
 		while (pair.second >= zPos) {
@@ -272,7 +272,7 @@ class ClientChunk(world: ClientWorld, pos: ChunkPos) : Chunk<ClientWorld, Client
 		for (renderer in entityRenderers.values) {
 			layerQueue.add(lambda@{ it: Matrix4fStack ->
 				val relative = renderer.renderPos - posVector2d
-				it.push().translateWithScale(relative.x.toFloat(), relative.y.toFloat())
+				it.push().translateWithMultiplication(relative.x.toFloat(), relative.y.toFloat())
 				renderer.render(it)
 				it.pop()
 				return@lambda

src/main/kotlin/ru/dbotthepony/kstarbound/client/ClientWorld.kt

@@ -1,8 +1,9 @@
 package ru.dbotthepony.kstarbound.client
 
 import ru.dbotthepony.kstarbound.client.render.renderLayeredList
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kstarbound.math.encasingChunkPosAABB
 import ru.dbotthepony.kstarbound.world.*
+import ru.dbotthepony.kvector.util2d.AABB
 
 class ClientWorld(val client: StarboundClient, seed: Long = 0L) : World<ClientWorld, ClientChunk>(seed) {
 	override fun chunkFactory(pos: ChunkPos): ClientChunk {

src/main/kotlin/ru/dbotthepony/kstarbound/client/StarboundClient.kt

@@ -12,14 +12,14 @@ import ru.dbotthepony.kstarbound.PIXELS_IN_STARBOUND_UNITf
 import ru.dbotthepony.kstarbound.Starbound
 import ru.dbotthepony.kstarbound.client.gl.GLStateTracker
 import ru.dbotthepony.kstarbound.client.input.UserInput
-import ru.dbotthepony.kstarbound.math.Matrix4f
 import ru.dbotthepony.kstarbound.client.render.Camera
 import ru.dbotthepony.kstarbound.client.render.TextAlignX
 import ru.dbotthepony.kstarbound.client.render.TextAlignY
-import ru.dbotthepony.kstarbound.math.AABB
-import ru.dbotthepony.kstarbound.math.Vector2f
-import ru.dbotthepony.kstarbound.util.Color
 import ru.dbotthepony.kstarbound.util.formatBytesShort
+import ru.dbotthepony.kvector.matrix.nfloat.Matrix4f
+import ru.dbotthepony.kvector.util2d.AABB
+import ru.dbotthepony.kvector.vector.Color
+import ru.dbotthepony.kvector.vector.nfloat.Vector3f
 
 class StarboundClient : AutoCloseable {
 	val window: Long
@@ -197,12 +197,12 @@ class StarboundClient : AutoCloseable {
 			world?.think(frameRenderTime)
 
 		glClear(GL_COLOR_BUFFER_BIT or GL_DEPTH_BUFFER_BIT)
-		gl.matrixStack.clear(viewportMatrixGame.toMutableMatrix())
+		gl.matrixStack.clear(viewportMatrixGame.toMutableMatrix4f())
 
 		gl.matrixStack.push()
-			.translateWithScale(viewportWidth / 2f, viewportHeight / 2f, 2f) // центр экрана + координаты отрисовки мира
+			.translateWithMultiplication(viewportWidth / 2f, viewportHeight / 2f, 2f) // центр экрана + координаты отрисовки мира
 			.scale(x = settings.scale * PIXELS_IN_STARBOUND_UNITf, y = settings.scale * PIXELS_IN_STARBOUND_UNITf) // масштабируем до нужного размера
-			.translateWithScale(-camera.pos.x, -camera.pos.y) // перемещаем вид к камере
+			.translateWithMultiplication(-camera.pos.x, -camera.pos.y) // перемещаем вид к камере
 
 		for (lambda in onPreDrawWorld) {
 			lambda.invoke()
@@ -213,7 +213,8 @@ class StarboundClient : AutoCloseable {
 			onPostDrawWorldOnce.removeAt(i)
 		}
 
-		world?.render(AABB.rectangle(
+		world?.render(
+			AABB.rectangle(
 			camera.pos.toDoubleVector(),
 			viewportWidth / settings.scale / PIXELS_IN_STARBOUND_UNIT,
 			viewportHeight / settings.scale / PIXELS_IN_STARBOUND_UNIT))
@@ -224,7 +225,7 @@ class StarboundClient : AutoCloseable {
 
 		gl.matrixStack.pop()
 
-		gl.matrixStack.clear(viewportMatrixGUI.toMutableMatrix().translate(z = 2f))
+		gl.matrixStack.clear(viewportMatrixGUI.toMutableMatrix4f().translate(Vector3f(z = 2f)))
 
 		val thisTime = System.currentTimeMillis()
 
@@ -236,12 +237,12 @@ class StarboundClient : AutoCloseable {
 			}
 
 			gl.matrixStack.push()
-			gl.matrixStack.translateWithScale(y = viewportHeight.toFloat())
+			gl.matrixStack.translateWithMultiplication(y = viewportHeight.toFloat())
 			var shade = 255
 
 			for (i in startupTextList.size - 1 downTo 0) {
-				val size = gl.font.render(startupTextList[i], alignY = TextAlignY.BOTTOM, scale = 0.4f, color = Color.SHADES_OF_GRAY[shade].copy(alpha = alpha))
+				val size = gl.font.render(startupTextList[i], alignY = TextAlignY.BOTTOM, scale = 0.4f, color = Color.SHADES_OF_GRAY[shade].copy(a = alpha))
-				gl.matrixStack.translateWithScale(y = -size.height * 1.2f)
+				gl.matrixStack.translateWithMultiplication(y = -size.height * 1.2f)
 
 				if (shade > 120) {
 					shade -= 10

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

@@ -3,11 +3,11 @@ package ru.dbotthepony.kstarbound.client.gl
 import it.unimi.dsi.fastutil.objects.Object2ObjectArrayMap
 import it.unimi.dsi.fastutil.objects.Object2ObjectFunction
 import org.lwjgl.opengl.GL46.*
-import ru.dbotthepony.kstarbound.api.IStruct3f
+import ru.dbotthepony.kvector.api.IFloatMatrix
-import ru.dbotthepony.kstarbound.api.IStruct4f
+import ru.dbotthepony.kvector.api.IStruct3f
-import ru.dbotthepony.kstarbound.math.AbstractMatrix3f
+import ru.dbotthepony.kvector.api.IStruct4f
-import ru.dbotthepony.kstarbound.math.AbstractMatrix4f
+import java.nio.ByteBuffer
-import ru.dbotthepony.kstarbound.math.FloatMatrix
+import java.nio.ByteOrder
 import java.util.*
 import kotlin.collections.HashSet
 
@@ -37,29 +37,28 @@ class GLUniformLocation(val program: GLShaderProgram, val name: String, val poin
 		return this
 	}
 
-	fun set(value: FloatMatrix<*>): GLUniformLocation {
+	private val buff3x3 by lazy { ByteBuffer.allocateDirect(4 * 3 * 3).also { it.order(ByteOrder.nativeOrder()) }.asFloatBuffer() }
+	private val buff4x4 by lazy { ByteBuffer.allocateDirect(4 * 4 * 4).also { it.order(ByteOrder.nativeOrder()) }.asFloatBuffer() }
+
+	fun set(value: IFloatMatrix<*>): GLUniformLocation {
 		program.state.ensureSameThread()
 
 		if (value.rows == 3 && value.columns == 3) {
 			// Матрица 3x3
-			if (value is AbstractMatrix3f) {
+			buff3x3.position(0)
-				glProgramUniformMatrix3fv(program.pointer, pointer, false, value.toFloatBuffer())
+			value.write(buff3x3)
-			} else {
+			buff3x3.position(0)
-				glProgramUniformMatrix3fv(program.pointer, pointer, false, value.toFloatArray())
+			glProgramUniformMatrix3fv(program.pointer, pointer, false, buff3x3)
-			}
-
 			checkForGLError()
 		} else if (value.rows == 4 && value.columns == 4) {
 			// Матрица 4x4
-			if (value is AbstractMatrix4f) {
+			buff4x4.position(0)
-				glProgramUniformMatrix4fv(program.pointer, pointer, false, value.toFloatBuffer())
+			value.write(buff4x4)
-			} else {
+			buff4x4.position(0)
-				glProgramUniformMatrix4fv(program.pointer, pointer, false, value.toFloatArray())
+			glProgramUniformMatrix4fv(program.pointer, pointer, false, buff4x4)
-			}
-
 			checkForGLError()
 		} else {
-			throw IllegalArgumentException("Can not use matrix with these dimensions: ${value.rows}x${value.columns}")
+			throw IllegalArgumentException("Can not use matrix with these dimensions: ${value.columns}x${value.rows}")
 		}
 
 		return this
@@ -101,7 +100,7 @@ open class GLShaderProgram(val state: GLStateTracker, vararg shaders: GLShader)
 	operator fun set(name: String, value: IStruct4f) = this[name]?.set(value)
 	operator fun set(name: String, value: IStruct3f) = this[name]?.set(value)
 	operator fun set(name: String, value: Int) = this[name]?.set(value)
-	operator fun set(name: String, value: FloatMatrix<*>) = this[name]?.set(value)
+	operator fun set(name: String, value: IFloatMatrix<*>) = this[name]?.set(value)
 
 	fun attach(shader: GLShader) {
 		state.ensureSameThread()

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

@@ -4,16 +4,15 @@ import org.apache.logging.log4j.LogManager
 import org.lwjgl.opengl.GL
 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
 import ru.dbotthepony.kstarbound.client.render.TileRenderers
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kvector.api.IStruct4f
-import ru.dbotthepony.kstarbound.util.Color
+import ru.dbotthepony.kvector.matrix.Matrix4fStack
-import java.io.File
+import ru.dbotthepony.kvector.matrix.nfloat.Matrix4f
+import ru.dbotthepony.kvector.util2d.AABB
+import ru.dbotthepony.kvector.vector.Color
 import java.io.FileNotFoundException
 import java.lang.ref.Cleaner
 import java.util.concurrent.ThreadFactory

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

@@ -3,7 +3,7 @@ package ru.dbotthepony.kstarbound.client.gl
 import org.apache.logging.log4j.LogManager
 import org.lwjgl.opengl.GL46.*
 import org.lwjgl.stb.STBImage
-import ru.dbotthepony.kstarbound.math.Vector2i
+import ru.dbotthepony.kvector.vector.nint.Vector2i
 import java.io.File
 import java.io.FileNotFoundException
 import java.nio.ByteBuffer

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

@@ -1,7 +1,7 @@
 package ru.dbotthepony.kstarbound.client.gl
 
 import org.lwjgl.opengl.GL46.*
-import ru.dbotthepony.kstarbound.math.AABB
+import ru.dbotthepony.kvector.util2d.AABB
 import java.io.Closeable
 import java.nio.ByteBuffer
 import java.nio.ByteOrder

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

@@ -5,8 +5,8 @@ import ru.dbotthepony.kstarbound.client.gl.GLShaderProgram
 import ru.dbotthepony.kstarbound.client.gl.GLVertexArrayObject
 import ru.dbotthepony.kstarbound.client.gl.DynamicVertexBuilder
 import ru.dbotthepony.kstarbound.client.gl.checkForGLError
-import ru.dbotthepony.kstarbound.math.FloatMatrix
+import ru.dbotthepony.kvector.api.IFloatMatrix
-import ru.dbotthepony.kstarbound.math.Matrix4fStack
+import ru.dbotthepony.kvector.matrix.Matrix4fStack
 
 /**
  * Служит для быстрой настройки состояния для будущей отрисовки
@@ -23,7 +23,7 @@ open class BakedProgramState(
 ) {
 	private val transformLocation = program["_transform"]
 
-	open fun setTransform(value: FloatMatrix<*>) {
+	open fun setTransform(value: IFloatMatrix<*>) {
 		transformLocation?.set(value)
 	}
 
@@ -71,7 +71,7 @@ class BakedStaticMesh(
 		ebo.unbind()
 	}
 
-	fun render(transform: FloatMatrix<*>? = null) {
+	fun render(transform: IFloatMatrix<*>? = null) {
 		check(isValid) { "$this is no longer valid" }
 		programState.setup()
 

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

@@ -5,8 +5,8 @@ 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.kvector.vector.Color
-import ru.dbotthepony.kstarbound.util.Color
+import ru.dbotthepony.kvector.vector.ndouble.Vector2d
 import kotlin.math.cos
 import kotlin.math.sin
 
@@ -69,7 +69,7 @@ class Box2DRenderer(val state: GLStateTracker) : IDebugDraw {
 	}
 
 	override fun drawSolidPolygon(vertices: List<Vector2d>, color: Color) {
-		drawSolid(vertices, color.copy(alpha = 0.5f))
+		drawSolid(vertices, color.copy(a = 0.5f))
 		drawPolygon(vertices, color)
 	}
 
@@ -104,7 +104,7 @@ class Box2DRenderer(val state: GLStateTracker) : IDebugDraw {
 			))
 		}
 
-		drawSolidPolygon(vertexList, color.copy(alpha = 0.5f))
+		drawSolidPolygon(vertexList, color.copy(a = 0.5f))
 		drawPolygon(vertexList, color)
 		drawPolygon(listOf(center, center + axis * radius), color)
 	}

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

@@ -2,7 +2,7 @@ package ru.dbotthepony.kstarbound.client.render
 
 import org.lwjgl.glfw.GLFW.*
 import ru.dbotthepony.kstarbound.client.StarboundClient
-import ru.dbotthepony.kstarbound.math.*
+import ru.dbotthepony.kvector.vector.nfloat.MutableVector2f
 
 class Camera(val client: StarboundClient) {
 	/**