DBot/KStarbound
DBot/KStarbound
3
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

Move kvector and kbox2d to their own repos

Browse Source
This commit is contained in:
DBotThePony 2022-02-27 21:00:57 +07:00
parent b1ee5bf66d
commit bf458c2921
Signed by: DBot
GPG Key ID: DCC23B5715498507
94 changed files with 7 additions and 57552 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
build.gradle.kts
View File

@ -15,6 +15,10 @@ val lwjglNatives = "natives-windows"
repositories {
mavenCentral()
maven {
url = uri("https://maven.dbotthepony.ru")
}
}
application {
@ -30,33 +34,6 @@ tasks.compileKotlin {
}
}
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 {
implementation("org.jetbrains.kotlin:kotlin-stdlib:1.6.10")
implementation("org.jetbrains.kotlin:kotlin-reflect:1.6.10")
@ -101,6 +78,9 @@ dependencies {
runtimeOnly("org.lwjgl", "lwjgl-stb", classifier = lwjglNatives)
implementation("net.java.dev.jna:jna:5.10.0")
implementation("ru.dbotthepony:kbox2d:2.4.1.+")
implementation("ru.dbotthepony:kvector:1.0.+")
}
tasks.getByName<Test>("test") {
@ -119,7 +99,3 @@ tasks {
}
}
tasks.jar {
this.from(sourceSets["kvector"].output)
this.from(sourceSets["kbox2d"].output)
}

155
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/B2TimeStep.kt
View File

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

152
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Body.kt
View File

@ -1,152 +0,0 @@
package ru.dbotthepony.kbox2d.api
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
/**
* The body type.
* - static: zero mass, zero velocity, may be manually moved
* - kinematic: zero mass, non-zero velocity set by user, moved by solver
* - dynamic: positive mass, non-zero velocity determined by forces, moved by solver
*/
enum class BodyType {
STATIC,
KINEMATIC,
DYNAMIC
}
/// A body definition holds all the data needed to construct a rigid body.
/// You can safely re-use body definitions. Shapes are added to a body after construction.
data class BodyDef(
/**
* The world position of the body. Avoid creating bodies at the origin
* since this can lead to many overlapping shapes.
*/
var position: Vector2d = Vector2d.ZERO,
/**
* The world angle of the body in radians.
*/
var angle: Double = 0.0,
/**
* The linear velocity of the body's origin in world co-ordinates.
*/
var linearVelocity: Vector2d = Vector2d.ZERO,
/**
* The angular velocity of the body.
*/
var angularVelocity: Double = 0.0,
/**
* Linear damping is use to reduce the linear velocity. The damping parameter
* can be larger than 1.0f but the damping effect becomes sensitive to the
* time step when the damping parameter is large.
* Units are 1/time
*/
var linearDamping: Double = 0.0,
/**
* Angular damping is use to reduce the angular velocity. The damping parameter
* can be larger than 1.0f but the damping effect becomes sensitive to the
* time step when the damping parameter is large.
* Units are 1/time
*/
var angularDamping: Double = 0.0,
/**
* Set this flag to false if this body should never fall asleep. Note that
* this increases CPU usage.
*/
var allowSleep: Boolean = true,
/**
* Is this body initially awake or sleeping?
*/
var awake: Boolean = true,
/**
* Should this body be prevented from rotating? Useful for characters.
*/
var fixedRotation: Boolean = false,
/**
* Is this a fast moving body that should be prevented from tunneling through
* other moving bodies? Note that all bodies are prevented from tunneling through
* kinematic and static bodies. This setting is only considered on dynamic bodies.
* @warning You should use this flag sparingly since it increases processing time.
*/
var bullet: Boolean = false,
/**
* The body type: static, kinematic, or dynamic.
* Note: if a dynamic body would have zero mass, the mass is set to one.
*/
var type: BodyType = BodyType.STATIC,
/**
* Does this body start out enabled?
*/
var enabled: Boolean = true,
/**
* Use this to store application specific body data.
*/
var userData: Any? = null,
/**
* Scale the gravity applied to this body.
*/
var gravityScale: Double = 1.0,
) {
fun validate() {
require(angularVelocity.isFinite()) { "Angular velocity is infinite" }
require(linearDamping.isFinite()) { "Linear damping is infinite" }
require(angularDamping.isFinite()) { "Angular damping is infinite" }
require(angle.isFinite()) { "Angular velocity is infinite" }
require(gravityScale.isFinite()) { "Gravity scale is infinite" }
require(position.isFinite) { "Position is infinite" }
require(!angularVelocity.isNaN()) { "Angular velocity is NaN" }
require(!linearDamping.isNaN()) { "Linear damping is NaN" }
require(!angularDamping.isNaN()) { "Angular damping is NaN" }
require(!angle.isNaN()) { "Angular velocity is NaN" }
require(!gravityScale.isNaN()) { "Gravity scale is NaN" }
require(angularDamping >= 0.0) { "Angular damping must be non negative, $angularDamping given" }
require(linearDamping >= 0.0) { "Linear damping must be non negative, $linearDamping given" }
}
}
enum class BodyFlags(val bitmask: Int) {
ISLAND(0x1),
AWAKE(0x2),
AUTO_SLEEP(0x4),
BULLET(0x8),
FIXED_ROTATION(0x10),
ENABLED(0x20),
TOI(0x40);
fun isit(other: Int): Boolean {
return (other and bitmask) == bitmask
}
fun update(other: Int, state: Boolean): Int {
if (state)
return or(other)
else
return not(other)
}
fun and(other: Int): Int {
return other and bitmask
}
fun or(other: Int): Int {
return other or bitmask
}
fun not(other: Int): Int {
return other and bitmask.inv()
}
}

126
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Collision.kt
View File

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

85
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Constants.kt
View File

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

72
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Contact.kt
View File

@ -1,72 +0,0 @@
package ru.dbotthepony.kbox2d.api
import ru.dbotthepony.kbox2d.dynamics.B2Body
import ru.dbotthepony.kbox2d.dynamics.contact.AbstractContact
import kotlin.math.sqrt
/// Friction mixing law. The idea is to allow either fixture to drive the friction to zero.
/// For example, anything slides on ice.
fun b2MixFriction(friction1: Double, friction2: Double): Double {
return sqrt(friction1 * friction2)
}
/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.
/// For example, a superball bounces on anything.
fun b2MixRestitution(restitution1: Double, restitution2: Double): Double {
return if (restitution1 > restitution2) restitution1 else restitution2
}
/// Restitution mixing law. This picks the lowest value.
fun b2MixRestitutionThreshold(threshold1: Double, threshold2: Double): Double {
return if (threshold1 < threshold2) threshold1 else threshold2
}
data class ContactRegister(
val primary: Boolean,
)
/**
* A contact edge is used to connect bodies and contacts together
* in a contact graph where each body is a node and each contact
* is an edge. A contact edge belongs to a doubly linked list
* maintained in each attached body. Each contact has two contact
* nodes, one for each attached body.
*/
data class ContactEdge(
val other: B2Body, ///< provides quick access to the other body attached.
val contact: AbstractContact, ///< the contact
var prev: ContactEdge? = null, ///< the previous contact edge in the body's contact list
var next: ContactEdge? = null, ///< the next contact edge in the body's contact list
)
enum class ContactFlags(val bitmask: Int) {
ISLAND(0x1),
TOUCHING(0x2),
ENABLED(0x4),
FILTER(0x8),
BULLET_HIT(0x10),
TOI(0x20);
fun isit(other: Int): Boolean {
return (other and bitmask) == bitmask
}
fun update(other: Int, state: Boolean): Int {
if (state)
return or(other)
else
return not(other)
}
fun and(other: Int): Int {
return other and bitmask
}
fun or(other: Int): Int {
return other or bitmask
}
fun not(other: Int): Int {
return other and bitmask.inv()
}
}

54
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Distance.kt
View File

@ -1,54 +0,0 @@
package ru.dbotthepony.kbox2d.api
import ru.dbotthepony.kbox2d.collision.DistanceProxy
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
/**
* Used to warm start b2Distance.
* Set count to zero on first call.
*/
data class SimplexCache(
val metric: Double = 0.0, ///< length or area
val count: Int = 0,
val indexA: IntArray = IntArray(0), ///< vertices on shape A
val indexB: IntArray = IntArray(0), ///< vertices on shape B
)
/**
* Input for b2Distance.
* You have to option to use the shape radii
* in the computation. Even
*/
data class DistanceInput(
var proxyA: DistanceProxy,
var proxyB: DistanceProxy,
var transformA: Transform = Transform(),
var transformB: Transform = Transform(),
var useRadii: Boolean = false
)
/**
* Output for b2Distance.
*/
data class DistanceOutput(
val pointA: Vector2d, ///< closest point on shapeA
val pointB: Vector2d, ///< closest point on shapeB
val distance: Double,
val iterations: Int, ///< number of GJK iterations used
val newCache: SimplexCache
)
data class ShapeCastInput(
var proxyA: DistanceProxy,
var proxyB: DistanceProxy,
var transformA: Transform,
var transformB: Transform,
var translationB: Vector2d,
)
data class ShapeCastOutput(
var point: Vector2d = Vector2d.ZERO,
var normal: Vector2d = Vector2d.ZERO,
var lambda: Double = 1.0,
var iterations: Int = 0,
)

98
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Fixture.kt
View File

@ -1,98 +0,0 @@
package ru.dbotthepony.kbox2d.api
import ru.dbotthepony.kbox2d.collision.e_nullProxy
import ru.dbotthepony.kbox2d.dynamics.B2Fixture
import ru.dbotthepony.kvector.util2d.AABB
sealed interface IFilter {
/**
* The collision category bits. Normally you would just set one bit.
*/
val categoryBits: Long
/**
* The collision mask bits. This states the categories that this
* shape would accept for collision.
*/
val maskBits: Long
/**
* Collision groups allow a certain group of objects to never collide (negative)
* or always collide (positive). Zero means no collision group. Non-zero group
* filtering always wins against the mask bits.
*/
val groupIndex: Int
}
data class Filter(
override var categoryBits: Long = 0x1L,
override var maskBits: Long = 0xFFFFL,
override var groupIndex: Int = 0,
) : IFilter {
fun immutable(): ImmutableFilter {
return ImmutableFilter(categoryBits, maskBits, groupIndex)
}
}
data class ImmutableFilter(
override val categoryBits: Long,
override val maskBits: Long,
override val groupIndex: Int,
) : IFilter
data class FixtureDef(
/**
* The shape, this must be set. The shape will be cloned.
*/
var shape: IShape<*>? = null,
var friction: Double = 0.2,
/**
* The restitution (elasticity) usually in the range [0,1].
*/
var restitution: Double = 0.0,
/**
* Restitution velocity threshold, usually in m/s. Collisions above this
* speed have restitution applied (will bounce).
*/
var restitutionThreshold: Double = 1.0 * b2_lengthUnitsPerMeter,
/**
* The density, usually in kg/m^2.
*/
var density: Double = 0.0,
/**
* A sensor shape collects contact information but never generates a collision
* response.
*/
var isSensor: Boolean = false,
/**
* Use this to store application specific fixture data.
*/
var userData: Any? = null,
val filter: Filter = Filter()
)
data class FixtureProxy(
var aabb: AABB,
val fixture: B2Fixture,
val childIndex: Int,
) {
private var setProxyID = false
var proxyId: Int = e_nullProxy
set(value) {
if (!setProxyID) {
field = value
setProxyID = true
return
}
throw IllegalStateException("FixtureProxy should be immutable (tried to set value $value, already having $field)")
}
}

52
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/IDebugDraw.kt
View File

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

12
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/IMovable.kt
View File

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

81
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/IProxieable.kt
View File

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

695
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Joint.kt
View File

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

642
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Math.kt
View File

@ -1,642 +0,0 @@
@file:Suppress("unused")
package ru.dbotthepony.kbox2d.api
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
/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
/// largest power of 2. For a 32-bit value:"
fun b2NextPowerOfTwo(x: Int): Int {
var x = x
x = x or (x ushr 1)
x = x or (x ushr 2)
x = x or (x ushr 4)
x = x or (x ushr 8)
x = x or (x ushr 16)
return x + 1
}
fun b2IsPowerOfTwo(x: Int): Boolean {
return x > 0 && (x and (x - 1)) == 0
}
/// Rotation
class Rotation(
s: Double,
c: Double,
) {
/// Initialize from an angle in radians
constructor(angle: Double) : this(sin(angle), cos(angle))
override fun toString(): String {
return "Rotation($s, $c)"
}
var s: Double = s
set(value) {
if (!value.isFinite()) {
throw IllegalArgumentException("Tried to set illegal non-finite sinus $value")
}
if (value.isNaN()) {
throw IllegalArgumentException("Tried to set illegal NaN sinus")
}
field = value
}
var c: Double = c
set(value) {
if (!value.isFinite()) {
throw IllegalArgumentException("Tried to set illegal non-finite cosine $value")
}
if (value.isNaN()) {
throw IllegalArgumentException("Tried to set illegal NaN cosine")
}
field = value
}
init {
if (!s.isFinite()) {
throw IllegalArgumentException("Sinus is infinite")
}
if (!c.isFinite()) {
throw IllegalArgumentException("Cosines is infinite")
}
if (s.isNaN()) {
throw IllegalArgumentException("Sinus is NaN")
}
if (c.isNaN()) {
throw IllegalArgumentException("Cosines is NaN")
}
}
/// Set to the identity rotation
fun setIdentity() {
s = 0.0
c = 1.0
}
/// Set using an angle in radians.
fun set(angle: Double) {
s = sin(angle)
c = cos(angle)
}
/// Get the angle in radians
val angle: Double get() = atan2(s, c)
/// Get the x-axis
val xAxis: MutableVector2d get() = MutableVector2d(c, s)
/// Get the u-axis
val yAxis: MutableVector2d get() = MutableVector2d(-s, c)
/// Multiply two rotations: q * r
operator fun times(other: Rotation): Rotation {
return Rotation(
s = s * other.c + c * other.s,
c = c * other.c - s * other.s,
)
}
/// Transpose multiply two rotations: qT * r
fun timesT(other: Rotation): Rotation {
return Rotation(
s = c * other.s - s * other.c,
c = c * other.c + s * other.s,
)
}
operator fun times(v: Vector2d): Vector2d {
return Vector2d(c * v.x - s * v.y, s * v.x + c * v.y)
}
fun timesT(v: Vector2d): Vector2d {
return Vector2d(c * v.x + s * v.y, -s * v.x + c * v.y)
}
}
/**
* A transform contains translation and rotation. It is used to represent
* the position and orientation of rigid frames.
*/
class Transform(
position: Vector2d,
val rotation: Rotation,
) {
var position: Vector2d = position
set(value) {
if (!value.isFinite) {
throw IllegalArgumentException("Tried to set illegal position $value")
}
field = value
}
override fun toString(): String {
return "Transform($position, $rotation)"
}
constructor(position: Vector2d, rotation: Double) : this(position, Rotation(rotation))
constructor() : this(Vector2d.ZERO, Rotation(0.0))
// aliases for box2d C++ code
val q by this::rotation
var p by this::position
/// Set this to the identity transform.
fun setIdentity() {
position = Vector2d.ZERO
rotation.setIdentity()
}
/// Set this based on the position and angle.
fun set(position: Vector2d, angle: Double) {
this.position = Vector2d(position.x, position.y)
rotation.set(angle)
}
operator fun times(other: Transform): Transform {
return Transform(
rotation = rotation * other.rotation,
position = rotation.times(other.position) + position
)
}
fun timesT(other: Transform): Transform {
return Transform(
rotation = rotation.timesT(other.rotation),
position = rotation.timesT(other.position - position)
)
}
operator fun times(v: 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: Vector2d): Vector2d {
val px = v.x - position.x
val py = v.y - position.y
return Vector2d(
x = (rotation.c * px + rotation.s * py),
y = (-rotation.s * px + rotation.c * py)
)
}
}
/**
* This describes the motion of a body/shape for TOI computation.
* Shapes are defined with respect to the body origin, which may
* no coincide with the center of mass. However, to support dynamics
* we must interpolate the center of mass position.
*/
class Sweep {
/**
* local center of mass position
*/
var localCenter: Vector2d = Vector2d.ZERO
set(value) {
if (!value.isFinite) {
throw IllegalArgumentException("Tried to set illegal local center $value")
}
field = value
}
/**
* center world positions
*/
var c0: Vector2d = Vector2d.ZERO
set(value) {
if (!value.isFinite) {
throw IllegalArgumentException("Tried to set illegal center world position $value")
}
field = value
}
/**
* center world positions
*/
var c: Vector2d = Vector2d.ZERO
set(value) {
if (!value.isFinite) {
throw IllegalArgumentException("Tried to set illegal center world position $value")
}
field = value
}
var oldCenter by this::c0
var newCenter by this::c
/**
* world angles
*/
var a0: Double = 0.0
set(value) {
if (!value.isFinite()) {
throw IllegalArgumentException("Tried to set illegal non finite $value")
}
if (value.isNaN()) {
throw IllegalArgumentException("Tried to set illegal NaN value")
}
field = value
}
/**
* world angles
*/
var a: Double = 0.0
set(value) {
if (!value.isFinite()) {
throw IllegalArgumentException("Tried to set illegal non finite $value")
}
if (value.isNaN()) {
throw IllegalArgumentException("Tried to set illegal NaN value")
}
field = value
}
var oldAngles by this::a0
var newAngles by this::a
/**
* Fraction of the current time step in the range [0,1]
* [c0] and [a0] are the positions at alpha0.
*/
var alpha0: Double = 0.0
set(value) {
if (!value.isFinite()) {
throw IllegalArgumentException("Tried to set illegal non finite $value")
}
if (value.isNaN()) {
throw IllegalArgumentException("Tried to set illegal NaN value")
}
field = value
}
/** Get the interpolated transform at a specific time.
* @param transform the output transform
* @param beta is a factor in [0,1], where 0 indicates [alpha0].
* https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
*/
fun getTransform(transform: Transform, beta: Double) {
transform.position = c0 * (1.0 - beta) + c * beta
val angle = (1.0 - beta) * a0 + beta * a
transform.rotation.set(angle)
transform.position -= transform.position * localCenter
}
/** Get the interpolated transform at a specific time.
* @param beta is a factor in [0,1], where 0 indicates [alpha0].
* https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
*/
fun getTransform(beta: Double): Transform {
val v = Transform()
getTransform(v, beta)
return v
}
/**
* Advance the sweep forward, yielding a new initial state.
* @param alpha the new initial time.
*/
fun advance(alpha: Double) {
require(alpha < 1.0) { "Bad advance value $alpha" }
val beta = (alpha - alpha0) / (1.0 - alpha0)
c0 += (c - c0) * beta
a0 += (a - a0) * beta
alpha0 = alpha
}
/**
* Normalize the angles.
*/
fun normalize() {
val d = floor(a0 / (PI * 2.0)) * 2.0 * PI
a0 -= d
a -= d
}
fun copy(): Sweep {
return Sweep().also {
it.localCenter = localCenter
it.c0 = c0
it.c = c
it.a0 = a0
it.a = a
it.alpha0 = alpha0
}
}
fun load(from: Sweep) {
this.localCenter = from.localCenter
this.c0 = from.c0
this.c = from.c
this.a0 = from.a0
this.a = from.a
this.alpha0 = from.alpha0
}
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Dot(a: Vector2d, b: Vector2d): Double {
return a.dot(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Cross(a: Vector2d, b: Vector2d): Double {
return a.cross(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Cross(a: Vector2d, b: Double): Vector2d {
return a.cross(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Cross(a: Double, b: Vector2d): Vector2d {
return a.cross(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Abs(a: Vector2d): Vector2d {
return a.absoluteValue
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Abs(a: Double): Double {
return a.absoluteValue
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Mul(q: Rotation, v: Vector2d): Vector2d {
return q.times(v)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2MulT(q: Rotation, v: Vector2d): Vector2d {
return q.timesT(v)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Mul(t: Transform, v: Vector2d): Vector2d {
return t.times(v)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2MulT(t: Transform, v: Vector2d): Vector2d {
return t.timesT(v)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Mul22(t: IMatrix3d<*>, v: Vector2d): Vector2d {
val result = multiplyMatrix(t.toMatrix2d(), 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: Transform, v: Transform): Transform {
return t.times(v)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2MulT(t: Transform, v: Transform): Transform {
return t.timesT(v)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Mul(t: IMatrix2d<*>, v: Vector2d): Vector2d {
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: IMatrix3d<*>, v: Vector3d): Vector3d {
val result = multiplyMatrix(t, v)
return Vector3d(result[0, 0], result[0, 1], result[0, 2])
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Min(a: Double, b: Double): Double {
return a.coerceAtMost(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Min(a: Vector2d, b: Vector2d): Vector2d {
return a.coerceAtMost(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Max(a: Int, b: Int): Int {
return a.coerceAtLeast(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Max(a: Double, b: Double): Double {
return a.coerceAtLeast(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Max(a: Vector2d, b: Vector2d): Vector2d {
return a.coerceAtLeast(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Distance(a: Vector2d, b: Vector2d): Double {
return a.distance(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2DistanceSquared(a: Vector2d, b: Vector2d): Double {
return a.distanceSquared(b)
}
/**
* Shortcut for faster porting of C++ code
*/
@Suppress("NOTHING_TO_INLINE")
internal inline fun b2Clamp(a: Double, min: Double, max: Double): Double {
return b2Max(min, b2Min(a, max))
}
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),
)
}

87
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/Shape.kt
View File

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

30
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/TimeOfImpact.kt
View File

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

142
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/api/WorldCallbacks.kt
View File

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

210
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/BroadPhase.kt
View File

@ -1,210 +0,0 @@
package ru.dbotthepony.kbox2d.collision
import ru.dbotthepony.kbox2d.api.*
import ru.dbotthepony.kvector.util2d.AABB
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
typealias b2Pair = Pair<Int, Int>
const val e_nullProxy = -1
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
}
}
/**
* The broad-phase is used for computing pairs and performing volume queries and ray casts.
* This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
* It is up to the client to consume the new pairs and to track subsequent overlap.
*/
class BroadPhase : IProxieable, IMovable {
private val moveBuffer = IntArrayList()
private val pairBuffer = ArrayList<b2Pair>()
private var moveCount = 0
private val tree = DynamicTree()
/**
* Get the number of proxies.
*/
var proxyCount: Int = 0
private set
/**
* Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.
*/
fun touchProxy(proxyID: Int) {
bufferMove(proxyID)
}
override fun createProxy(aabb: AABB, userData: Any?): Int {
val proxyId = tree.createProxy(aabb, userData)
proxyCount++
bufferMove(proxyId)
return proxyId
}
override fun destroyProxy(proxyID: Int) {
unBufferMove(proxyID)
proxyCount--
tree.destroyProxy(proxyID)
}
override fun moveProxy(proxyID: Int, aabb: AABB, displacement: Vector2d): Boolean {
if (tree.moveProxy(proxyID, aabb, displacement)) {
bufferMove(proxyID)
return true
}
return false
}
private fun bufferMove(proxyId: Int) {
if (moveBuffer.size > moveCount) {
moveBuffer[moveCount] = proxyId
} else {
moveBuffer.add(proxyId)
}
moveCount++
}
private fun unBufferMove(proxyId: Int) {
for (i in 0 until moveCount) {
if (moveBuffer[i] == proxyId) {
moveBuffer[i] = e_nullProxy
}
}
}
override fun query(aabb: AABB, callback: ProxyQueryCallback): Boolean {
return tree.query(aabb, callback)
}
override fun rayCast(input: RayCastInput, callback: ProxyRayCastCallback) {
tree.rayCast(input, callback)
}
override fun shiftOrigin(newOrigin: Vector2d) {
tree.shiftOrigin(newOrigin)
}
override fun getUserData(proxyID: Int): Any? {
return tree.getUserData(proxyID)
}
override fun getFatAABB(proxyID: Int): AABB {
return tree.getFatAABB(proxyID)
}
/**
* Get the height of the embedded tree.
*/
val treeHeight: Int
get() = tree.height
/**
* Get the balance of the embedded tree.
*/
val treeBalance: Int
get() = tree.maxBalance
/**
* Get the quality metric of the embedded tree.
*/
val treeQuality: Double
get() = tree.getAreaRatio
/**
* Test overlap of fat AABBs.
*/
fun testOverlap(proxyIDA: Int, proxyIDB: Int): Boolean {
return tree.getFatAABB(proxyIDA).intersect(tree.getFatAABB(proxyIDB))
}
private var queryProxy: Int = -1
private fun queryCallback(proxyId: Int, userData: Any?): Boolean {
if (proxyId == queryProxy) {
return true
}
val moved = tree.wasMoved(proxyId)
if (moved && proxyId > queryProxy) {
// Both proxies are moving. Avoid duplicate pairs.
return true
}
if (proxyId > queryProxy) {
pairBuffer.add(queryProxy to proxyId)
} else {
pairBuffer.add(proxyId to queryProxy)
}
return true
}
/**
* Update the pairs. This results in pair callbacks. This can only add pairs.
*/
fun updatePairs(callback: (Any?, Any?) -> Unit) {
pairBuffer.clear()
for (i in 0 until moveCount) {
val value = moveBuffer[i]
if (value == e_nullProxy)
continue
val fatAABB = tree.getFatAABB(value)
queryProxy = value
tree.query(fatAABB, this::queryCallback)
}
// Send pairs to caller
for (primaryPair in pairBuffer) {
val userDataA = tree.getUserData(primaryPair.first)
val userDataB = tree.getUserData(primaryPair.second)
callback.invoke(userDataA, userDataB)
}
// Clear move flags
for (i in 0 until moveCount) {
val value = moveBuffer[i]
if (value != e_nullProxy) {
tree.clearMoved(value)
}
}
// Reset move buffer
moveCount = 0
}
}

183
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/Collision.kt
View File

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

692
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/Distance.kt
View File

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

759
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/DynamicTree.kt
View File

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

409
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/TimeOfImpact.kt
View File

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

149
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideCircle.kt
View File

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

469
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideEdge.kt
View File

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

250
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollidePolygon.kt
View File

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

212
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/shapes/ChainShape.kt
View File

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

86
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/shapes/CircleShape.kt
View File

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

203
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/shapes/EdgeShape.kt
View File

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

466
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/collision/shapes/PolygonShape.kt
View File

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

914
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/B2Body.kt
View File

@ -1,914 +0,0 @@
@file:Suppress("unused")
package ru.dbotthepony.kbox2d.dynamics
import ru.dbotthepony.kbox2d.api.*
import ru.dbotthepony.kvector.util2d.AABB
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
class B2Body(def: BodyDef, world: B2World) {
private var _world: B2World? = world
/**
* Get the parent world of this body.
*/
val world: B2World
get() = _world ?: throw IllegalStateException("Tried to use removed body")
internal var flags: Int = 0
internal var isOnIsland = false
internal var islandIndex: Int = 0
internal fun unlink() {
_world = null
}
init {
def.validate()
if (def.bullet) {
flags = BodyFlags.BULLET.or(flags)
}
if (def.fixedRotation) {
flags = BodyFlags.FIXED_ROTATION.or(flags)
}
if (def.allowSleep) {
flags = BodyFlags.AUTO_SLEEP.or(flags)
}
if (def.awake && def.type != BodyType.STATIC) {
flags = BodyFlags.AWAKE.or(flags)
}
if (def.enabled) {
flags = BodyFlags.ENABLED.or(flags)
}
}
internal val sweep: Sweep = Sweep()
/**
* Get the body transform for the body's origin.
* @return the world transform of the body's origin.
*/
val transform: Transform = Transform(def.position, def.angle)
val xf by this::transform
/**
* Get the world body origin position.
* @return the world position of the body's origin.
*/
val position: Vector2d
get() = transform.position
/**
* Get the angle in radians.
* @return the current world rotation angle in radians.
*/
val angle: Double
get() = sweep.a
val userData: Any? = def.userData
internal var sleepTime: Double = 0.0
internal var torque: Double = 0.0
internal var force: Vector2d = Vector2d.ZERO
/**
* The linear velocity of the body's origin in world co-ordinates.
*/
var linearVelocity: Vector2d = def.linearVelocity
set(value) {
if (type == BodyType.STATIC)
return
if (value.dot(value) > 0.0)
isAwake = true
field = value
}
/**
* The angular velocity of the body.
*/
var angularVelocity: Double = def.angularVelocity
set(value) {
if (type == BodyType.STATIC)
return
if (value * value > 0.0)
isAwake = true
field = value
}
var fixtureList: B2Fixture? = null
protected set
var fixtureCount: Int = 0
private set
/**
* Get the list of all joints attached to this body.
*
* Kotlin: This is not a list, but, as in C++ impl, a custom
* linked list.
*/
var jointList: JointEdge? = null
internal set
/**
* Get the list of all contacts attached to this body.
* @warning this list changes during the time step and you may
* miss some collisions if you don't use b2ContactListener.
*
* Kotlin: This is not a list, but, as in C++ impl, a custom
* linked list.
*/
var contactEdge: ContactEdge? = null
internal set
val contactEdgeIterator: Iterator<ContactEdge> get() {
return object : Iterator<ContactEdge> {
private var node = contactEdge
override fun hasNext(): Boolean {
return node != null
}
override fun next(): ContactEdge {
val old = node!!
node = old.next
return old
}
}
}
val jointIterator: Iterator<JointEdge> get() {
return object : Iterator<JointEdge> {
private var node = jointList
override fun hasNext(): Boolean {
return node != null
}
override fun next(): JointEdge {
val old = node!!
node = old.next
return old
}
}
}
var next: B2Body? = null
internal set
var prev: B2Body? = null
internal set
val fixtureIterator: Iterator<B2Fixture> get() {
return object : Iterator<B2Fixture> {
private var node = fixtureList
override fun hasNext(): Boolean {
return node != null
}
override fun next(): B2Fixture {
val old = node!!
node = old.next
return old
}
}
}
var linearDamping: Double = def.linearDamping
var angularDamping: Double = def.angularDamping
var gravityScale: Double = def.gravityScale
/**
* Get the total mass of the body.
* @return the mass, usually in kilograms (kg).
*/
var mass: Double = 0.0
private set
internal var invMass: Double = 0.0
/**
* Rotational inertia about the center of mass.
*/
internal var rotInertia: Double = 0.0
/**
* Rotational inertia about the center of mass.
*/
internal var rotInertiaInv: Double = 0.0
internal var I by this::rotInertia
internal var invI by this::rotInertiaInv
/**
* Should this body be treated like a bullet for continuous collision detection?
*/
var isBullet: Boolean
get() = BodyFlags.BULLET.isit(flags)
set(value) { flags = BodyFlags.BULLET.update(flags, value) }
/**
* Set the sleep state of the body. A sleeping body has very
* low CPU cost.
* @param flag set to true to wake the body, false to put it to sleep.
*/
var isAwake: Boolean
get() = BodyFlags.AWAKE.isit(flags)
set(value) {
if (type == BodyType.STATIC || BodyFlags.AWAKE.isit(flags) == value)
return
if (value) {
flags = flags or BodyFlags.AWAKE.bitmask
sleepTime = 0.0
} else {
flags = flags and BodyFlags.AWAKE.bitmask.inv()
sleepTime = 0.0
linearVelocity = Vector2d.ZERO
angularVelocity = 0.0
force = Vector2d.ZERO
torque = 0.0
}
}
/**
* Allow a body to be disabled. A disabled body is not simulated and cannot
* be collided with or woken up.
*
* If you pass a flag of true, all fixtures will be added to the broad-phase.
*
* If you pass a flag of false, all fixtures will be removed from the
* broad-phase and all contacts will be destroyed.
*
* Fixtures and joints are otherwise unaffected. You may continue
* to create/destroy fixtures and joints on disabled bodies.
*
* Fixtures on a disabled body are implicitly disabled and will
* not participate in collisions, ray-casts, or queries.
*
* Joints connected to a disabled body are implicitly disabled.
*
* An diabled body is still owned by a b2World object and remains
* in the body list.
*/
var isEnabled: Boolean
get() = BodyFlags.ENABLED.isit(flags)
set(value) {
if (world.isLocked)
throw ConcurrentModificationException()
if (value == isEnabled)
return
if (value) {
flags = BodyFlags.ENABLED.not(flags)
// Create all proxies.
val broadPhase = world.contactManager.broadPhase
for (fixture in fixtureIterator) {
fixture.createProxies(broadPhase, transform)
}
world.notifyNewContacts()
} else {
flags = BodyFlags.ENABLED.or(flags)
// Destroy all proxies.
val broadPhase = world.contactManager.broadPhase
for (fixture in fixtureIterator) {
fixture.destroyProxies(broadPhase)
}
// Destroy the attached contacts.
for (edge in contactEdgeIterator) {
world.contactManager.destroy(edge.contact)
}
contactEdge = null
}
}
/**
* Set this body to have fixed rotation. This causes the mass to be reset.
*/
var isFixedRotation: Boolean
get() = BodyFlags.FIXED_ROTATION.isit(flags)
set(value) {
if (value == isFixedRotation)
return
flags = BodyFlags.FIXED_ROTATION.update(flags, value)
angularVelocity = 0.0
resetMassData()
}
/**
* You can disable sleeping on this body. If you disable sleeping, the
* body will be woken.
*/
var allowAutoSleep: Boolean
get() = BodyFlags.AUTO_SLEEP.isit(flags)
set(value) {
flags = BodyFlags.AUTO_SLEEP.update(flags, value)
if (!value) {
isAwake = true
}
}
/**
* Set the type of this body. This may alter the mass and velocity.
*/
var type: BodyType = def.type
set(value) {
if (world.isLocked)
throw ConcurrentModificationException()
field = value
resetMassData()
if (value == BodyType.STATIC) {
linearVelocity = Vector2d.ZERO
angularVelocity = 0.0
sweep.a0 = sweep.a
sweep.c0 = sweep.c
flags = BodyFlags.AWAKE.not(flags)
synchronizeFixtures()
}
isAwake = true
force = Vector2d.ZERO
torque = 0.0
var edge = contactEdge
while (edge != null) {
// TODO: проверить, что делает destroy
world.contactManager.destroy(edge.contact)
edge = edge.next
}
contactEdge = null
val broadPhase = world.contactManager.broadPhase
var f: B2Fixture? = fixtureList
while (f != null) {
for (proxy in f.proxies) {
broadPhase.touchProxy(proxy.proxyId)
}
f = f.next
}
}
/** Creates a fixture and attach it to this body. Use this function if you need
* to set some fixture parameters, like friction. Otherwise you can create the
* fixture directly from a shape.
* If the density is non-zero, this function automatically updates the mass of the body.
* Contacts are not created until the next time step.
* @param def the fixture definition.
* @warning This function is locked during callbacks.
*/
fun createFixture(def: FixtureDef): B2Fixture {
if (world.isLocked)
throw ConcurrentModificationException()
val fixture = B2Fixture(this, def)
if (isEnabled) {
fixture.createProxies(world.contactManager.broadPhase, transform)
}
fixture.next = fixtureList
fixtureList?.prev = fixture
fixtureList = fixture
fixtureCount++
// Adjust mass properties if needed.
if (fixture.density > 0.0) {
resetMassData()
}
// Let the world know we have a new fixture. This will cause new contacts
// to be created at the beginning of the next time step.
world.notifyNewContacts()
return fixture
}
/**
* Creates a fixture from a shape and attach it to this body.
* This is a convenience function. Use b2FixtureDef if you need to set parameters
* like friction, restitution, user data, or filtering.
* If the density is non-zero, this function automatically updates the mass of the body.
* @param shape the shape to be cloned.
* @param density the shape density (set to zero for static bodies).
* @warning This function is locked during callbacks.
*/
fun createFixture(shape: IShape<*>, density: Double): B2Fixture {
return createFixture(
FixtureDef(
shape = shape,
density = density
)
)
}
/**
* Destroy a fixture. This removes the fixture from the broad-phase and
* destroys all contacts associated with this fixture. This will
* automatically adjust the mass of the body if the body is dynamic and the
* fixture has positive density.
* All fixtures attached to a body are implicitly destroyed when the body is destroyed.
* @param fixture the fixture to be removed.
* @warning This function is locked during callbacks.
*/
fun destroyFixture(fixture: B2Fixture) {
if (world.isLocked)
throw ConcurrentModificationException()
require(fixture.body == this) { "$fixture does not belong to $this (belongs to ${fixture.body})" }
check(fixtureCount > 0) { "Having no tracked fixtures, but $fixture belongs to us" }
val prev = fixture.prev
val next = fixture.next
prev?.next = next
next?.prev = prev
if (fixture == fixtureList) {
fixtureList = next ?: prev
}
val density = fixture.density
// Destroy any contacts associated with the fixture.
var edge = contactEdge
while (edge != null) {
val contact = edge.contact
edge = edge.next
val fixtureA = contact.fixtureA
val fixtureB = contact.fixtureB
if (fixtureA == fixture || fixtureB == fixture) {
// This destroys the contact and removes it from
// this body's contact list.
world.contactManager.destroy(contact)
}
}
if (isEnabled) {
fixture.destroyProxies(world.contactManager.broadPhase)
}
fixture.unlink()
fixtureCount--
// Reset the mass data
if (density > 0.0) {
resetMassData()
}
}
/**
* This resets the mass properties to the sum of the mass properties of the fixtures.
* This normally does not need to be called unless you called SetMassData to override
* the mass and you later want to reset the mass.
*/
fun resetMassData() {
// Compute mass data from shapes. Each shape has its own density.
mass = 0.0
invMass = 0.0
rotInertia = 0.0
rotInertiaInv = 0.0
sweep.localCenter = Vector2d.ZERO
if (type == BodyType.STATIC || type == BodyType.KINEMATIC) {
sweep.c0 = transform.position
sweep.c = transform.position
sweep.a0 = sweep.a
return
}
check(type == BodyType.DYNAMIC)
// Accumulate mass over all fixtures.
var localCenter = Vector2d.ZERO
for (fixture in fixtureIterator) {
if (fixture.density == 0.0) {
continue
}
val massData = fixture.getMassData()
mass += massData.mass
localCenter += massData.center * massData.mass
rotInertia += massData.inertia
}
// Compute center of mass.
if (mass > 0.0) {
invMass = 1.0 / mass
localCenter *= invMass
}
if (rotInertia > 0.0 && !isFixedRotation) {
// Center the inertia about the center of mass.
rotInertia -= mass * localCenter.dot(localCenter)
check(rotInertia > 0.0)
rotInertiaInv = 1.0 / rotInertia
} else {
rotInertia = 0.0
rotInertiaInv = 0.0
}
// Move center of mass.
val oldCenter = sweep.c
sweep.localCenter = localCenter
sweep.c = transform.times(localCenter)
sweep.c0 = sweep.c
// Update center of mass velocity.
linearVelocity += b2Cross(angularVelocity, sweep.c - oldCenter)
}
/**
* **KBox2D extension.**
*
* Computes full local space AABB of all fixtures attached, with zero rotation.
* If no fixtures are attached, returns zero sized AABB.
*/
val localSpaceAABB: AABB get() {
var combined = AABB.ZERO
for (fixture in fixtureIterator) {
for (i in 0 until fixture.shape.childCount) {
combined = combined.combine(fixture.shape.computeAABB(i))
}
}
return combined
}
/**
* **KBox2D extension.**
*
* Computes full world space AABB of all fixtures attached, with zero rotation.
* If no fixtures are attached, returns zero sized AABB positioned at [position].
*/
val worldSpaceAABB: AABB get() {
var combined = AABB(position, position)
for (fixture in fixtureIterator) {
for (i in 0 until fixture.shape.childCount) {
combined = combined.combine(fixture.shape.computeAABB(xf, i))
}
}
return combined
}
/**
* Get the mass data of the body.
* @return a struct containing the mass, inertia and center of the body.
* Set the mass properties to override the mass properties of the fixtures.
* Note that this changes the center of mass position.
* Note that creating or destroying fixtures can also alter the mass.
* This function has no effect if the body isn't dynamic.
* @param data the mass properties.
*/
var massData: MassData
get() = MassData(
mass = mass,
inertia = inertia,
center = sweep.localCenter
)
set(value) {
if (world.isLocked)
throw ConcurrentModificationException()
if (type != BodyType.DYNAMIC)
return
invMass = 0.0
rotInertia = 0.0
rotInertiaInv = 0.0
mass = value.mass
if (mass <= 0.0) {
mass = 1.0
}
invMass = 1.0 / mass
if (value.inertia > 0.0 && !isFixedRotation) {
rotInertia = value.inertia - mass * value.center.dot(value.center)
check(rotInertia > 0.0)
rotInertiaInv = 1.0 / rotInertia
}
// Move center of mass.
val oldCenter = sweep.c
sweep.localCenter = value.center
sweep.c = transform.times(sweep.localCenter)
sweep.c0 = sweep.c
// Update center of mass velocity.
linearVelocity += b2Cross(angularVelocity, sweep.c - oldCenter)
}
internal fun shouldCollide(other: B2Body): Boolean {
// At least one body should be dynamic.
if (type != BodyType.DYNAMIC && other.type != BodyType.DYNAMIC)
return false
// Does a joint prevent collision?
var joint = jointList
while (joint != null) {
if (joint.otherNullable == other && !joint.joint.collideConnected) {
return false
}
joint = joint.next
}
return true
}
init {
sweep.c0 = transform.position
sweep.c = transform.position
sweep.a0 = def.angle
sweep.a = def.angle
}
/**
* Set the position of the body's origin and rotation.
* Manipulating a body's transform may cause non-physical behavior.
* Note: contacts are updated on the next call to b2World::Step.
* @param position the world position of the body's local origin.
* @param angle the world rotation in radians.
*/
fun setTransform(position: Vector2d, angle: Double) {
if (world.isLocked)
throw ConcurrentModificationException()
transform.rotation.set(angle)
transform.position = position
sweep.c = transform.times(sweep.localCenter)
sweep.a = angle
sweep.c0 = sweep.c
sweep.a0 = angle
val broadPhase = world.contactManager.broadPhase
for (fixture in fixtureIterator) {
(fixture as B2Fixture?)?.synchronize(broadPhase, transform, transform)
}
// Check for new contacts the next step
world.notifyNewContacts()
}
/**
* Get the rotational inertia of the body about the local origin.
* @return the rotational inertia, usually in kg-m^2.
*/
val inertia: Double
get() = rotInertia + mass * sweep.localCenter.dot(sweep.localCenter)
/**
* Get the local position of the center of mass.
*/
val localCenter: Vector2d
get() = sweep.localCenter
/**
* Get the world position of the center of mass.
*/
val worldCenter: Vector2d
get() = sweep.c
/**
* Get the world coordinates of a point given the local coordinates.
* @param localPoint a point on the body measured relative the the body's origin.
* @return the same point expressed in world coordinates.
*/
fun getWorldPoint(localPoint: Vector2d): Vector2d {
return transform.times(localPoint)
}
/**
* Get the world coordinates of a vector given the local coordinates.
* @param localVector a vector fixed in the body.
* @return the same vector expressed in world coordinates.
*/
fun getWorldVector(localPoint: Vector2d): Vector2d {
return transform.rotation.times(localPoint)
}
/**
* Gets a local point relative to the body's origin given a world point.
* @param worldPoint a point in world coordinates.
* @return the corresponding local point relative to the body's origin.
*/
fun getLocalPoint(worldPoint: Vector2d): Vector2d {
return transform.timesT(worldPoint)
}
/**
* Gets a local vector given a world vector.
* @param worldVector a vector in world coordinates.
* @return the corresponding local vector.
*/
fun getLocalVector(worldVector: Vector2d): Vector2d {
return transform.rotation.timesT(worldVector)
}
/**
* Get the world linear velocity of a world point attached to this body.
* @param worldPoint a point in world coordinates.
* @return the world velocity of a point.
*/
fun getLinearVelocityFromWorldPoint(worldPoint: Vector2d): Vector2d {
return linearVelocity + b2Cross(angularVelocity, worldPoint - sweep.c)
}
/**
* Get the world velocity of a local point.
* @param localPoint a point in local coordinates.
* @return the world velocity of a point.
*/
fun getLinearVelocityFromLocalPoint(localPoint: Vector2d): Vector2d {
return getLinearVelocityFromWorldPoint(getWorldPoint(localPoint))
}
/**
* Apply a force at a world point. If the force is not
* applied at the center of mass, it will generate a torque and
* affect the angular velocity. This wakes up the body.
* @param force the world force vector, usually in Newtons (N).
* @param point the world position of the point of application.
* @param wake also wake up the body
*/
fun applyForce(force: Vector2d, point: Vector2d, wake: Boolean = true) {
if (type != BodyType.DYNAMIC)
return
if (wake && !isAwake)
isAwake = true
// Don't accumulate a force if the body is sleeping.
if (isAwake) {
this.force += force
this.torque += b2Cross(point - sweep.c, force)
}
}
/**
* Apply a force to the center of mass. This wakes up the body.
* @param force the world force vector, usually in Newtons (N).
* @param wake also wake up the body
*/
fun applyForceToCenter(force: Vector2d, wake: Boolean = true) {
if (type != BodyType.DYNAMIC)
return
if (wake && !isAwake)
isAwake = true
// Don't accumulate a force if the body is sleeping.
if (isAwake) {
this.force += force
}
}
/**
* Apply a torque. This affects the angular velocity
* without affecting the linear velocity of the center of mass.
* @param torque about the z-axis (out of the screen), usually in N-m.
* @param wake also wake up the body
*/
fun applyTorque(torque: Double, wake: Boolean = true) {
if (type != BodyType.DYNAMIC)
return
if (wake && !isAwake)
isAwake = true
if (isAwake) {
this.torque += torque
}
}
/**
* Apply an impulse at a point. This immediately modifies the velocity.
* It also modifies the angular velocity if the point of application
* is not at the center of mass. This wakes up the body.
* @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
* @param point the world position of the point of application.
* @param wake also wake up the body
*/
fun applyLinearImpulse(impulse: Vector2d, point: Vector2d, wake: Boolean = true) {
if (type != BodyType.DYNAMIC)
return
if (wake && !isAwake)
isAwake = true
if (isAwake) {
linearVelocity += impulse * invMass
angularVelocity += rotInertiaInv * b2Cross(point - sweep.c, impulse)
}
}
/**
* Apply an impulse to the center of mass. This immediately modifies the velocity.
* @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
* @param wake also wake up the body
*/
fun applyLinearImpulseToCenter(impulse: Vector2d, wake: Boolean = true) {
if (type != BodyType.DYNAMIC)
return
if (wake && !isAwake)
isAwake = true
if (isAwake) {
linearVelocity += impulse * invMass
}
}
/**
* Apply an angular impulse.
* @param impulse the angular impulse in units of kg*m*m/s
* @param wake also wake up the body
*/
fun applyAngularImpulse(impulse: Double, wake: Boolean = true) {
if (type != BodyType.DYNAMIC)
return
if (wake && !isAwake)
isAwake = true
if (isAwake) {
angularVelocity += impulse * rotInertiaInv
}
}
internal fun synchronizeTransform() {
transform.rotation.set(sweep.a)
transform.position = sweep.c - transform.rotation.times(sweep.localCenter)
}
internal fun advance(alpha: Double) {
sweep.advance(alpha)
sweep.c = sweep.c0
sweep.a = sweep.a0
synchronizeTransform()
}
internal fun synchronizeFixtures() {
val broadPhase = world.contactManager.broadPhase
if (isAwake) {
val transform1 = Transform()
transform1.rotation.set(sweep.a0)
transform1.position = sweep.c0 - transform1.rotation.times(sweep.localCenter)
for (fixture in fixtureIterator) {
fixture.synchronize(broadPhase, transform1, transform)
}
} else {
for (fixture in fixtureIterator) {
fixture.synchronize(broadPhase, transform, transform)
}
}
}
fun dump() {
TODO("Not yet implemented")
}
}

267
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/B2Fixture.kt
View File

@ -1,267 +0,0 @@
package ru.dbotthepony.kbox2d.dynamics
import ru.dbotthepony.kbox2d.api.*
import ru.dbotthepony.kbox2d.collision.BroadPhase
import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
import ru.dbotthepony.kvector.util2d.AABB
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
import java.util.*
import kotlin.collections.ArrayList
/**
* A fixture is used to attach a shape to a body for collision detection. A fixture
* inherits its transform from its parent. Fixtures hold additional non-geometric data
* such as friction, collision filters, etc.
* Fixtures are created via b2Body::CreateFixture.
* @warning you cannot reuse fixtures.
*/
class B2Fixture(
body: B2Body,
def: FixtureDef
) {
/**
* Get the parent body of this fixture. This is nullptr if the fixture is not attached.
* @return the parent body.
*/
var body: B2Body? = body
private set
/**
* Get the next fixture in the parent body's fixture list.
* @return the next shape.
*/
var next: B2Fixture? = null
internal set
/**
* Get the previous fixture in the parent body's fixture list.
* @return the next shape.
*/
var prev: B2Fixture? = null
internal set
/**
* Get the user data that was assigned in the fixture definition. Use this to
* store your application specific data.
*/
var userData: Any? = def.userData
/**
* Get the coefficient of friction.
* Set the coefficient of friction. This will _not_ change the friction of
* existing contacts.
*/
var friction: Double = def.friction
/**
* Get the coefficient of restitution.
* Set the coefficient of restitution. This will _not_ change the restitution of
* existing contacts.
*/
var restitution: Double = def.restitution
/**
* Get the restitution velocity threshold.
* Set the restitution threshold. This will _not_ change the restitution threshold of
* existing contacts.
*/
var restitutionThreshold: Double = def.restitutionThreshold
/**
* Get the type of the child shape. You can use this to down cast to the concrete shape.
* @return the shape type.
*/
val type: IShape.Type get() = shape.type
/**
* Set if this fixture is a sensor.
* Is this fixture a sensor (non-solid)?
* @return the true if the shape is a sensor.
*/
var isSensor: Boolean = def.isSensor
set(value) {
if (field != value) {
checkNotNull(body) { "Already destroyed" }.isAwake = true
field = value
}
}
/**
* Get the child shape. You can modify the child shape, however you should not change the
* number of vertices because this will crash some collision caching mechanisms.
* Manipulating the shape may lead to non-physical behavior.
*/
val shape: IShape<*> = requireNotNull(def.shape) { "null shape provided" }.copy()
init {
if (shape is PolygonShape) {
shape.validate(true)
}
if (shape.childCount < 0) {
throw IllegalArgumentException("Shape $shape has ${shape.childCount} children")
}
}
private val internalProxies = ArrayList<FixtureProxy>(shape.childCount)
val proxies: List<FixtureProxy> = Collections.unmodifiableList(internalProxies)
/**
* Set the density of this fixture. This will _not_ automatically adjust the mass
* of the body. You must call b2Body::ResetMassData to update the body's mass.
* Get the density of this fixture.
*/
var density: Double = def.density
set(value) {
require(value.isFinite()) { "Infinite density" }
require(!value.isNaN()) { "NaN density" }
require(value >= 0.0) { "Negative density of $value" }
field = value
}
/**
* Set the contact filtering data. This will not update contacts until the next time
* step when either parent body is active and awake.
* This automatically calls Refilter.
*/
var filter: IFilter = def.filter.immutable()
set(value) {
if (value is ImmutableFilter)
field = value
else
field = (value as Filter).immutable()
refilter()
}
/**
* **KBox2D extension**.
*
* Convenience function for quickly destroying this fixture.
*/
fun destroy() {
checkNotNull(body) { "Already destroyed" }.destroyFixture(this)
}
internal fun unlink() {
check(body != null) { "Already destroyed" }
check(internalProxies.isEmpty()) { "Still having proxies" }
body = null
}
/**
* These support body activation/deactivation.
*/
internal fun createProxies(broadPhase: BroadPhase, xf: Transform) {
check(body != null) { "Already destroyed" }
check(internalProxies.isEmpty()) { "Already having proxies" }
for (i in 0 until shape.childCount) {
val aabb = shape.computeAABB(xf, i)
val proxy = FixtureProxy(
fixture = this,
childIndex = i,
aabb = aabb,
)
proxy.proxyId = broadPhase.createProxy(aabb, proxy)
internalProxies.add(proxy)
}
}
/**
* These support body activation/deactivation.
*/
internal fun destroyProxies(broadPhase: BroadPhase) {
check(body != null) { "Already destroyed" }
// Destroy proxies in the broad-phase.
for (proxy in internalProxies) {
broadPhase.destroyProxy(proxy.proxyId)
}
internalProxies.clear()
}
internal fun synchronize(broadPhase: BroadPhase, transform1: Transform, transform2: Transform) {
check(body != null) { "Already destroyed" }
for (proxy in internalProxies) {
val aabb1 = shape.computeAABB(transform1, proxy.childIndex)
val aabb2 = shape.computeAABB(transform2, proxy.childIndex)
proxy.aabb = aabb1.combine(aabb2)
val displacement = aabb2.centre - aabb1.centre
broadPhase.moveProxy(proxy.proxyId, proxy.aabb, displacement)
}
}
/**
* Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide.
*/
fun refilter() {
val body = body
check(body != null) { "Already destroyed" }
var edge = body.contactEdge
while (edge != null) {
val contact = edge.contact
val fixtureA = contact.fixtureA
val fixtureB = contact.fixtureB
if (fixtureA == this || fixtureB == this)
contact.flagForFiltering()
edge = edge.next
}
val world = body.world
val broadPhase = world.contactManager.broadPhase
for (proxy in internalProxies) {
broadPhase.touchProxy(proxy.proxyId)
}
}
/**
* Test a point for containment in this fixture.
* @param p a point in world coordinates.
*/
fun testPoint(p: Vector2d): Boolean {
return shape.testPoint(checkNotNull(body) { "Tried to use detached fixture" }.transform, p)
}
/**
* Cast a ray against this shape.
* @param output the ray-cast results.
* @param input the ray-cast input parameters.
* @param childIndex the child shape index (e.g. edge index)
*/
fun rayCast(input: RayCastInput, childIndex: Int): RayCastOutput {
return shape.rayCast(input, checkNotNull(body) { "Tried to use detached fixture" }.transform, childIndex)
}
/**
* Get the mass data for this fixture. The mass data is based on the density and
* the shape. The rotational inertia is about the shape's origin. This operation
* may be expensive.
*/
fun getMassData(): MassData {
return shape.computeMass(density)
}
/**
* Get the fixture's AABB. This AABB may be enlarge and/or stale.
* If you need a more accurate AABB, compute it using the shape and
* the body transform.
*/
fun getAABB(childIndex: Int): AABB {
return proxies[childIndex].aabb
}
/**
* Dump this fixture to the log file.
*/
fun dump(childIndex: Int) { }
}

1079
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/B2World.kt
View File

File diff suppressed because it is too large Load Diff

209
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/ContactManager.kt
View File

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

302
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/AbstractContact.kt
View File

@ -1,302 +0,0 @@
package ru.dbotthepony.kbox2d.dynamics.contact
import ru.dbotthepony.kbox2d.api.*
import ru.dbotthepony.kbox2d.collision.WorldManifold
import ru.dbotthepony.kbox2d.collision.b2TestOverlap
import ru.dbotthepony.kbox2d.dynamics.B2Body
import ru.dbotthepony.kbox2d.dynamics.B2Fixture
import java.util.*
fun interface ContactFactory {
fun factorize(fixtureA: B2Fixture, childIndexA: Int, fixtureB: B2Fixture, childIndexB: Int): AbstractContact
}
/**
* The class manages contact between two shapes. A contact exists for each overlapping
* AABB in the broad-phase (except if filtered). Therefore a contact object may exist
* that has no contact points.
*/
sealed class AbstractContact(
/**
* Get fixture A in this contact.
*/
val fixtureA: B2Fixture,
/**
* Get the child primitive index for fixture A.
*/
val childIndexA: Int,
/**
* Get fixture B in this contact.
*/
val fixtureB: B2Fixture,
/**
* Get the child primitive index for fixture B.
*/
val childIndexB: Int,
) {
internal var flags: Int = ContactFlags.ENABLED.bitmask
internal var isOnIsland = false
internal var toiFlag = false
/**
* Get the contact manifold. Do not modify the manifold unless you understand the
* internals of Box2D.
*/
var manifold: Manifold = Manifold()
private set
/**
* Get the next contact in the world's contact list.
*/
var next: AbstractContact? = null
internal set
/**
* Get the previous contact in the world's contact list.
*/
var prev: AbstractContact? = null
internal set
internal var isFlaggedForFiltering: Boolean
get() = ContactFlags.FILTER.isit(flags)
set(value) { flags = ContactFlags.FILTER.update(flags, value) }
internal val nodeA: ContactEdge = ContactEdge(contact = this, other = fixtureB.body!!)
internal val nodeB: ContactEdge = ContactEdge(contact = this, other = fixtureA.body!!)
init {
val bodyA = fixtureA.body as B2Body
val bodyB = fixtureB.body as B2Body
nodeA.next = bodyA.contactEdge
nodeB.next = bodyB.contactEdge
bodyA.contactEdge?.prev = nodeA
bodyB.contactEdge?.prev = nodeB
bodyA.contactEdge = nodeA
bodyB.contactEdge = nodeB
}
/**
* Override the default friction mixture. You can call this in b2ContactListener::PreSolve.
* This value persists until set or reset.
* Get the friction.
*/
var friction: Double = b2MixFriction(fixtureA.friction, fixtureB.friction)
/**
* Override the default restitution mixture. You can call this in b2ContactListener::PreSolve.
* The value persists until you set or reset.
*/
var restitution: Double = b2MixRestitution(fixtureA.restitution, fixtureB.restitution)
/**
* Override the default restitution velocity threshold mixture. You can call this in b2ContactListener::PreSolve.
* The value persists until you set or reset.
* Get the restitution threshold.
*/
var restitutionThreshold: Double = b2MixRestitutionThreshold(fixtureA.restitutionThreshold, fixtureB.restitutionThreshold)
/**
* Set the desired tangent speed for a conveyor belt behavior. In meters per second.
* Get the desired tangent speed. In meters per second.
*/
var tangentSpeed: Double = 0.0
internal var toiCount: Int = 0
internal var toi: Double = 0.0
/**
* Evaluate this contact with your own manifold and transforms.
*/
abstract fun evaluate(xfA: Transform, xfB: Transform): Manifold
/**
* Get the world manifold.
*/
val worldManifold: IWorldManifold get() {
val bodyA = checkNotNull(fixtureA.body) { "FixtureA has no body attached" }
val bodyB = checkNotNull(fixtureB.body) { "FixtureB has no body attached" }
val shapeA = fixtureA.shape
val shapeB = fixtureB.shape
return WorldManifold(manifold, bodyA.transform, shapeA.radius, bodyB.transform, shapeB.radius)
}
/**
* Is this contact touching?
*/
var isTouching: Boolean
get() = (flags and ContactFlags.TOUCHING.bitmask) == ContactFlags.TOUCHING.bitmask
internal set(value) { flags = ContactFlags.TOUCHING.update(flags, value) }
/**
* Enable/disable this contact. This can be used inside the pre-solve
* contact listener. The contact is only disabled for the current
* time step (or sub-step in continuous collisions).
*/
var isEnabled: Boolean
get() = (flags and ContactFlags.ENABLED.bitmask) == ContactFlags.ENABLED.bitmask
set(value) { flags = ContactFlags.ENABLED.update(flags, value) }
fun flagForFiltering() {
flags = flags or ContactFlags.FILTER.bitmask
}
/**
* Awakens fixtures this contact belongs to if neither of them are sensors
*/
fun destroy() {
if (manifold.points.isNotEmpty() && !fixtureA.isSensor && !fixtureB.isSensor) {
fixtureA.body!!.isAwake = true
fixtureB.body!!.isAwake = true
}
}
/**
* Update the contact manifold and touching status.
* Note: do not assume the fixture AABBs are overlapping or are valid.
*/
internal fun update(contactListener: IContactListener?) {
// Re-enable this contact.
flags = ContactFlags.ENABLED.or(flags)
val oldManifold = manifold
val touching: Boolean
val wasTouching = isTouching
val sensor = fixtureA.isSensor || fixtureB.isSensor
val bodyA = fixtureA.body as B2Body
val bodyB = fixtureB.body as B2Body
val xfA = bodyA.transform
val xfB = bodyB.transform
// Is this contact a sensor?
if (sensor) {
val shapeA = fixtureA.shape
val shapeB = fixtureB.shape
touching = b2TestOverlap(shapeA, childIndexA, shapeB, childIndexB, xfA, xfB)
// Sensors don't generate manifolds.
manifold = Manifold.EMPTY
} else {
manifold = evaluate(xfA, xfB)
touching = manifold.points.isNotEmpty()
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (mp2 in manifold.points) {
mp2.normalImpulse = 0.0
mp2.tangentImpulse = 0.0
val id2 = mp2.id
for (mp1 in oldManifold.points) {
if (mp1.id.key == id2.key) {
mp2.normalImpulse = mp1.normalImpulse
mp2.tangentImpulse = mp1.tangentImpulse
break
}
}
}
if (touching != wasTouching) {
bodyA.isAwake = true
bodyB.isAwake = true
}
}
this.isTouching = touching
if (!wasTouching && touching) {
contactListener?.beginContact(this)
} else if (wasTouching && !touching) {
contactListener?.endContact(this)
}
if (!sensor && touching) {
contactListener?.preSolve(this, oldManifold)
}
}
/**
* Reset the friction mixture to the default value.
*/
fun resetFriction() {
friction = b2MixFriction(fixtureA.friction, fixtureB.friction)
}
/**
* Reset the restitution to the default value.
*/
fun resetRestitution() {
restitution = b2MixRestitution(fixtureA.restitution, fixtureB.restitution)
}
/**
* Reset the restitution threshold to the default value.
*/
fun resetRestitutionThreshold() {
restitutionThreshold = b2MixRestitutionThreshold(fixtureA.restitutionThreshold, fixtureB.restitutionThreshold)
}
companion object {
private val registry =
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) { EnumMap(IShape.Type::class.java) }[type2] = factory
}
internal fun create(
fixtureA: B2Fixture,
indexA: Int,
fixtureB: B2Fixture,
indexB: Int,
): AbstractContact {
val type1 = fixtureA.type
val type2 = fixtureB.type
return registry[type1]?.get(type2)?.factorize(fixtureA, indexA, fixtureB, indexB)
?: registry[type2]?.get(type1)?.factorize(fixtureB, indexB, fixtureA, indexA)
?: throw IllegalArgumentException("No collision handler for between $type1 and $type2")
}
init {
register(IShape.Type.POLYGON, IShape.Type.POLYGON) { fixtureA: B2Fixture, _: Int, fixtureB: B2Fixture, _: Int ->
return@register PolygonContact(fixtureA, fixtureB)
}
register(IShape.Type.POLYGON, IShape.Type.CIRCLE) { fixtureA: B2Fixture, _: Int, fixtureB: B2Fixture, _: Int ->
return@register PolygonCircleContact(fixtureA, fixtureB)
}
register(IShape.Type.CIRCLE, IShape.Type.CIRCLE) { fixtureA: B2Fixture, _: Int, fixtureB: B2Fixture, _: Int ->
return@register CircleContact(fixtureA, fixtureB)
}
register(IShape.Type.EDGE, IShape.Type.CIRCLE) { fixtureA: B2Fixture, _: Int, fixtureB: B2Fixture, _: Int ->
return@register EdgeCircleContact(fixtureA, fixtureB)
}
register(IShape.Type.EDGE, IShape.Type.POLYGON) { fixtureA: B2Fixture, _: Int, fixtureB: B2Fixture, _: Int ->
return@register EdgePolygonContact(fixtureA, fixtureB)
}
register(IShape.Type.CHAIN, IShape.Type.POLYGON) { fixtureA: B2Fixture, indexA: Int, fixtureB: B2Fixture, indexB: Int ->
return@register ChainPolygonContact(fixtureA, indexA, fixtureB, indexB)
}
register(IShape.Type.CHAIN, IShape.Type.CIRCLE) { fixtureA: B2Fixture, indexA: Int, fixtureB: B2Fixture, indexB: Int ->
return@register ChainCircleContact(fixtureA, indexA, fixtureB, indexB)
}
}
}
}

28
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainCircleContact.kt
View File

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

28
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainPolygonContact.kt
View File

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

22
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/CircleContact.kt
View File

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

23
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgeCircleContact.kt
View File

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

23
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgePolygonContact.kt
View File

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

23
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonCircleContact.kt
View File

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

26
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonContact.kt
View File

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

821
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/ContactSolver.kt
View File

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

480
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/Island.kt
View File

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

204
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/AbstractJoint.kt
View File

@ -1,204 +0,0 @@
package ru.dbotthepony.kbox2d.dynamics.joint
import ru.dbotthepony.kbox2d.api.*
import ru.dbotthepony.kbox2d.dynamics.B2Body
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
import java.util.EnumMap
fun interface JointFactory {
fun factorize(jointDef: IJointDef): AbstractJoint
}
sealed class AbstractJoint(def: IJointDef) : IMovable {
init {
require(def.bodyA != def.bodyB) { "Tried to create join on same body" }
}
internal var isOnIsland = false
protected var index: Int = 0
/**
* Get collide connected.
* Note: modifying the collide connect flag won't work correctly because
* the flag is only checked when fixture AABBs begin to overlap.
*/
val collideConnected: Boolean = def.collideConnected
/**
* Dump this joint to the log file.
*/
open fun dump() { }
/**
* Debug draw this joint
*/
open fun draw(draw: IDebugDraw) { }
/**
* Get the type of the concrete joint.
*/
val type: JointType = def.type
var userData: Any? = def.userData
// KBox2D: In original code, nothing expects bodies to be null
// but certain joints (notably, mouse joint) have no meaningful
// value for second body involved. So, KBox2D CAN handle case where
// joint have only one body, and to avoid null assertions in all places
// possible, bodyA and bodyB getters assert for null by themselves
// However, there is nullable getter for bodies: nullableBodyA and nullableBodyB
protected var _bodyA: B2Body? = def.bodyA as B2Body?
protected var _bodyB: B2Body? = def.bodyB as B2Body?
/**
* Get the first body attached to this joint.
*/
val bodyA: B2Body get() = checkNotNull(_bodyA) { "Body A is not present" }
/**
* Get the second body attached to this joint.
*/
val bodyB: B2Body get() = checkNotNull(_bodyB) { "Body B is not present" }
/**
* Get the anchor point on bodyA in world coordinates.
*/
abstract val anchorA: Vector2d
/**
* Get the anchor point on bodyB in world coordinates.
*/
abstract val anchorB: Vector2d
/**
* Get the reaction force on bodyB at the joint anchor in Newtons.
*/
abstract fun getReactionForce(inv_dt: Double): Vector2d
/**
* Get the reaction torque on bodyB in N*m.
*/
abstract fun getReactionTorque(inv_dt: Double): Double
val hasBodyA: Boolean get() = _bodyA != null
val hasBodyB: Boolean get() = _bodyB != null
val hasTwoBodies: Boolean get() = hasBodyA && hasBodyB
val nullableBodyA get() = _bodyA
val nullableBodyB get() = _bodyB
private var _edgeA: JointEdge? = JointEdge(other = _bodyB, joint = this, next = _bodyA?.jointList)
private var _edgeB: JointEdge? = JointEdge(other = _bodyA, joint = this, next = _bodyB?.jointList)
internal val edgeA: JointEdge get() = checkNotNull(_edgeA) { "Edge A is not present" }
internal val edgeB: JointEdge get() = checkNotNull(_edgeB) { "Edge B is not present" }
var isValid: Boolean = true
private set
init {
// Connect to the bodies' doubly linked lists.
_bodyA?.jointList?.prev = edgeA
_bodyA?.jointList = edgeA
_bodyB?.jointList?.prev = edgeB
_bodyB?.jointList = edgeB
}
/**
* Get the next joint the world joint list.
*
* Kotlin: This is not a list, but, as in C++ impl, a custom
* linked list.
*/
var next: AbstractJoint? = null
internal set
var prev: AbstractJoint? = null
internal set
fun destroy() {
bodyA.world.destroyJoint(this)
}
/**
* Short-cut function to determine if either body is enabled.
*/
val isEnabled: Boolean get() = bodyA.isEnabled || bodyB.isEnabled
/**
* Signals that this joint was destroyed, invalidate stuff
* to fail-fast this object
*/
internal open fun unlink() {
_edgeA = null
_edgeB = null
_bodyA = null
_bodyB = null
isValid = false
}
internal abstract fun initVelocityConstraints(data: B2SolverData)
internal abstract fun solveVelocityConstraints(data: B2SolverData)
internal abstract fun solvePositionConstraints(data: B2SolverData): Boolean
override fun shiftOrigin(newOrigin: Vector2d) {
// no-op
}
companion object {
private val registry = EnumMap<JointType, JointFactory>(JointType::class.java)
internal fun register(jointType: JointType, factory: JointFactory) {
require(registry.put(jointType, factory) == null) { "Re-registered $jointType factory" }
}
internal fun create(jointType: JointType, jointDef: IJointDef): AbstractJoint {
return requireNotNull(registry[jointType]) { "No joint factory registered for type $jointType" }.factorize(jointDef)
}
internal fun create(jointDef: IJointDef): AbstractJoint {
return create(jointDef.type, jointDef)
}
init {
register(JointType.DISTANCE) {
return@register DistanceJoint(it as DistanceJointDef)
}
register(JointType.REVOLUTE) {
return@register RevoluteJoint(it as RevoluteJointDef)
}
register(JointType.PRISMATIC) {
return@register PrismaticJoint(it as PrismaticJointDef)
}
register(JointType.PULLEY) {
return@register PulleyJoint(it as PulleyJointDef)
}
register(JointType.GEAR) {
return@register GearJoint(it as GearJointDef)
}
register(JointType.MOUSE) {
return@register MouseJoint(it as MouseJointDef)
}
register(JointType.WHEEL) {
return@register WheelJoint(it as WheelJointDef)
}
register(JointType.WELD) {
return@register WeldJoint(it as WeldJointDef)
}
register(JointType.FRICTION) {
return@register FrictionJoint(it as FrictionJointDef)
}
register(JointType.MOTOR) {
return@register MotorJoint(it as MotorJointDef)
}
}
}
}

336
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/DistanceJoint.kt
View File

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

211
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/FrictionJoint.kt
View File

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

468
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/GearJoint.kt
View File

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

226
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MotorJoint.kt
View File

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

151
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MouseJoint.kt
View File

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

609
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PrismaticJoint.kt
View File

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

281
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PulleyJoint.kt
View File

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

463
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/RevoluteJoint.kt
View File

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

286
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WeldJoint.kt
View File

@ -1,286 +0,0 @@
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.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
var damping: Double = def.damping
private var bias: Double = 0.0
// Solver shared
val localAnchorA = def.localAnchorA
val localAnchorB = def.localAnchorB
val referenceAngle = def.referenceAngle
private var gamma: Double = 0.0
private var impulse: Vector3d = Vector3d()
// Solver temp
private var indexA: Int = 0
private var indexB: Int = 0
private var rA: Vector2d = Vector2d.ZERO
private var rB: Vector2d = Vector2d.ZERO
private var localCenterA: Vector2d = Vector2d.ZERO
private var localCenterB: Vector2d = Vector2d.ZERO
private var invMassA: Double = 0.0
private var invMassB: Double = 0.0
private var invIA: Double = 0.0
private var invIB: Double = 0.0
private var mass: MutableMatrix3d = MutableMatrix3d.zero()
override fun initVelocityConstraints(data: B2SolverData) {
this.indexA = this.bodyA.islandIndex
this.indexB = this.bodyB.islandIndex
this.localCenterA = this.bodyA.sweep.localCenter
this.localCenterB = this.bodyB.sweep.localCenter
this.invMassA = this.bodyA.invMass
this.invMassB = this.bodyB.invMass
this.invIA = this.bodyA.invI
this.invIB = this.bodyB.invI
val aA = data.positions[this.indexA].a
var vA = data.velocities[this.indexA].v
var wA = data.velocities[this.indexA].w
val aB = data.positions[this.indexB].a
var vB = data.velocities[this.indexB].v
var wB = data.velocities[this.indexB].w
val qA = Rotation(aA)
val qB = Rotation(aB)
this.rA = b2Mul(qA, this.localAnchorA - this.localCenterA)
this.rB = b2Mul(qB, this.localAnchorB - this.localCenterB)
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
val mA = this.invMassA
val mB = this.invMassB
val iA = this.invIA
val iB = this.invIB
val K = MutableMatrix3d()
K.r00 = mA + mB + this.rA.y * this.rA.y * iA + this.rB.y * this.rB.y * iB
K.r01 = -this.rA.y * this.rA.x * iA - this.rB.y * this.rB.x * iB
K.r02 = -this.rA.y * iA - this.rB.y * iB
K.r10 = K.r01
K.r11 = mA + mB + this.rA.x * this.rA.x * iA + this.rB.x * this.rB.x * iB
K.r12 = this.rA.x * iA + this.rB.x * iB
K.r20 = K.r02
K.r21 = K.r12
K.r22 = iA + iB
if (this.stiffness > 0.0) {
this.mass = K.toMatrix2d().inverse?.toMutableMatrix3d() ?: MutableMatrix3d.zero()
var invM = iA + iB
val C = aB - aA - this.referenceAngle
// Damping coefficient
val d = this.damping
// Spring stiffness
val k = this.stiffness
// magic formulas
val h = data.step.dt
this.gamma = h * (d + h * k)
this.gamma = if (this.gamma != 0.0) 1.0 / this.gamma else 0.0
this.bias = C * h * k * this.gamma
invM += this.gamma
this.mass.r22 = if (invM != 0.0) 1.0 / invM else 0.0
} else if (K.r22 == 0.0) {
this.mass = K.toMatrix2d().inverse?.toMutableMatrix3d() ?: MutableMatrix3d.zero()
this.gamma = 0.0
this.bias = 0.0
} else {
this.mass = K.inverse?.toMutableMatrix3d() ?: MutableMatrix3d.zero()
this.gamma = 0.0
this.bias = 0.0
}
if (data.step.warmStarting) {
// Scale impulses to support a variable time step.
this.impulse *= data.step.dtRatio
val P = Vector2d(this.impulse.x, this.impulse.y)
vA -= mA * P
wA -= iA * (b2Cross(this.rA, P) + this.impulse.z)
vB += mB * P
wB += iB * (b2Cross(this.rB, P) + this.impulse.z)
} else {
this.impulse = Vector3d()
}
data.velocities[this.indexA].v = vA
data.velocities[this.indexA].w = wA
data.velocities[this.indexB].v = vB
data.velocities[this.indexB].w = wB
}
override fun solveVelocityConstraints(data: B2SolverData) {
var vA = data.velocities[this.indexA].v
var wA = data.velocities[this.indexA].w
var vB = data.velocities[this.indexB].v
var wB = data.velocities[this.indexB].w
val mA = this.invMassA
val mB = this.invMassB
val iA = this.invIA
val iB = this.invIB
if (this.stiffness > 0.0) {
val Cdot2 = wB - wA
val impulse2 = -this.mass.r22 * (Cdot2 + this.bias + this.gamma * this.impulse.z)
this.impulse += Vector3d(z = impulse2)
wA -= iA * impulse2
wB += iB * impulse2
val Cdot1 = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA)
val impulse1 = -b2Mul22(this.mass, Cdot1)
this.impulse += Vector3d(impulse1.x, impulse1.y)
val P = impulse1
vA -= mA * P
wA -= iA * b2Cross(this.rA, P)
vB += mB * P
wB += iB * b2Cross(this.rB, P)
} else {
val Cdot1 = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA)
val Cdot2 = wB - wA
val Cdot = Vector3d(Cdot1.x, Cdot1.y, Cdot2)
val impulse = -b2Mul(this.mass, Cdot)
this.impulse += impulse
val P = Vector2d(impulse.x, impulse.y)
vA -= mA * P
wA -= iA * (b2Cross(this.rA, P) + impulse.z)
vB += mB * P
wB += iB * (b2Cross(this.rB, P) + impulse.z)
}
data.velocities[this.indexA].v = vA
data.velocities[this.indexA].w = wA
data.velocities[this.indexB].v = vB
data.velocities[this.indexB].w = wB
}
override fun solvePositionConstraints(data: B2SolverData): Boolean {
var cA = data.positions[this.indexA].c
var aA = data.positions[this.indexA].a
var cB = data.positions[this.indexB].c
var aB = data.positions[this.indexB].a
val qA = Rotation(aA)
val qB = Rotation(aB)
val mA = this.invMassA
val mB = this.invMassB
val iA = this.invIA
val iB = this.invIB
val rA = b2Mul(qA, this.localAnchorA - this.localCenterA)
val rB = b2Mul(qB, this.localAnchorB - this.localCenterB)
val positionError: Double
val angularError: Double
val K = MutableMatrix3d()
K.r00 = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB
K.r01 = -rA.y * rA.x * iA - rB.y * rB.x * iB
K.r02 = -rA.y * iA - rB.y * iB
K.r10 = K.r01
K.r11 = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB
K.r12 = rA.x * iA + rB.x * iB
K.r20 = K.r02
K.r21 = K.r12
K.r22 = iA + iB
if (this.stiffness > 0.0f) {
val C1 = cB + rB - cA - rA
positionError = C1.length
angularError = 0.0
val P = -K.solve(C1)
cA -= mA * P
aA -= iA * b2Cross(rA, P)
cB += mB * P
aB += iB * b2Cross(rB, P)
} else {
val C1 = cB + rB - cA - rA
val C2 = aB - aA - this.referenceAngle
positionError = C1.length
angularError = b2Abs(C2)
val C = Vector3d(C1.x, C1.y, C2)
val impulse: Vector3d
if (K.r22 > 0.0) {
impulse = -K.solve(C)
} else {
val impulse2 = -K.solve(C1)
impulse = Vector3d(impulse2.x, impulse2.y)
}
val P = Vector2d(impulse.x, impulse.y)
cA -= mA * P
aA -= iA * (b2Cross(rA, P) + impulse.z)
cB += mB * P
aB += iB * (b2Cross(rB, P) + impulse.z)
}
data.positions[this.indexA].c = cA
data.positions[this.indexA].a = aA
data.positions[this.indexB].c = cB
data.positions[this.indexB].a = aB
return positionError <= b2_linearSlop && angularError <= b2_angularSlop
}
override val anchorA: Vector2d
get() = bodyA.getWorldPoint(localAnchorA)
override val anchorB: Vector2d
get() = bodyB.getWorldPoint(localAnchorB)
override fun getReactionForce(inv_dt: Double): Vector2d {
return Vector2d(impulse.x * inv_dt, impulse.y * inv_dt)
}
override fun getReactionTorque(inv_dt: Double): Double {
return inv_dt * impulse.z
}
}

534
src/kbox2d/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WheelJoint.kt
View File

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

4593
src/kvector/java/ru/dbotthepony/kvector/matrix/generated/MultiplicationsByte.java
View File

File diff suppressed because it is too large Load Diff

4593
src/kvector/java/ru/dbotthepony/kvector/matrix/generated/MultiplicationsDouble.java
View File

File diff suppressed because it is too large Load Diff

4593
src/kvector/java/ru/dbotthepony/kvector/matrix/generated/MultiplicationsFloat.java
View File

File diff suppressed because it is too large Load Diff

4593
src/kvector/java/ru/dbotthepony/kvector/matrix/generated/MultiplicationsInt.java
View File

File diff suppressed because it is too large Load Diff

4593
src/kvector/java/ru/dbotthepony/kvector/matrix/generated/MultiplicationsLong.java
View File

File diff suppressed because it is too large Load Diff

4593
src/kvector/java/ru/dbotthepony/kvector/matrix/generated/MultiplicationsShort.java
View File

File diff suppressed because it is too large Load Diff

214
src/kvector/kotlin/ru/dbotthepony/kvector/api/DoubleMatrix.kt
View File

@ -1,214 +0,0 @@
@file:Suppress("unused")
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
*/
operator fun get(column: Int, row: Int): Double
}
interface IMatrixSetterDouble : IMatrixLike {
/**
* Sets component of this matrix, as double
*/
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
*
* @return new matrix
*/
operator fun times(other: Double): T
/**
* Divides all matrix components by [other], returning new matrix as result
*
* @return new matrix
*/
operator fun div(other: Double): T
/**
* Multiplies this and [other] matrices by general matrix multiplication rules,
* if they are compatible in dimensions, and return new matrix.
*
* @throws IllegalArgumentException if matrices are not applicable for multiplication
* @return new matrix
*/
operator fun times(other: IMatrixGetterDouble): T
/**
* Adds components of both matrices, returning new matrix as result
*
* @throws IllegalArgumentException if matrices are different in dimensions
* @return new matrix
*/
operator fun plus(other: IMatrixGetterDouble): T
/**
* Subtracts components of both matrices, returning new matrix as result
*
* @throws IllegalArgumentException if matrices are different in dimensions
* @return new matrix
*/
operator fun minus(other: IMatrixGetterDouble): T
/**
* 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.
*/
val determinant: Double?
/**
* 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.
*/
val inverse: T?
/**
* This matrix' cofactor matrix.
*
* This operation is computation intense, do cache its result.
*
* If this matrix is not square matrix, null is returned.
*/
val cofactor: T?
/**
* This matrix' adjugate matrix.
*
* This operation is computation intense, do cache its result.
*
* 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 {
/**
* Multiplies all matrix components by [other], writing result into this matrix
*
* @return this matrix
*/
fun timesMut(other: Double): T
/**
* Divides all matrix components by [other], writing result into this matrix
*
* @return this matrix
*/
fun divMut(other: Double): T
/**
* Multiplies this and [other] matrices by general matrix multiplication rules,
* if they are compatible in dimensions, and write result into this matrix.
*
* @throws IllegalArgumentException if [other] is not a square matrix
* @throws IllegalStateException if this matrix is not a square matrix
* @return this matrix
*/
fun timesMut(other: IMatrixGetterDouble): T
/**
* Adds components of both matrices, writing result into this matrix
*
* @throws IllegalArgumentException if matrices are different in dimensions
* @return this matrix
*/
fun plusMut(other: IMatrixGetterDouble): T
/**
* Subtracts components of both matrices, writing result into this matrix
*
* @throws IllegalArgumentException if matrices are different in dimensions
* @return this matrix
*/
fun minusMut(other: IMatrixGetterDouble): T
}

211
src/kvector/kotlin/ru/dbotthepony/kvector/api/FloatMatrix.kt
View File

@ -1,211 +0,0 @@
@file:Suppress("unused")
package ru.dbotthepony.kvector.api
import java.nio.ByteBuffer
import java.nio.FloatBuffer
interface IMatrixGetterFloat : IMatrixLike {
/**
* @return component of this matrix, as float
*/
operator fun get(column: Int, row: Int): Float
}
interface IMatrixSetterFloat : IMatrixLike {
/**
* Sets component of this matrix, as float
*/
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
*
* @return new matrix
*/
operator fun times(other: Float): T
/**
* Divides all matrix components by [other], returning new matrix as result
*
* @return new matrix
*/
operator fun div(other: Float): T
/**
* Multiplies this and [other] matrices by general matrix multiplication rules,
* if they are compatible in dimensions, and return new matrix.
*
* @throws IllegalArgumentException if matrices are not applicable for multiplication
* @return new matrix
*/
operator fun times(other: IMatrixGetterFloat): T
/**
* Adds components of both matrices, returning new matrix as result
*
* @throws IllegalArgumentException if matrices are different in dimensions
* @return new matrix
*/
operator fun plus(other: IMatrixGetterFloat): T
/**
* Subtracts components of both matrices, returning new matrix as result
*
* @throws IllegalArgumentException if matrices are different in dimensions
* @return new matrix
*/
operator fun minus(other: IMatrixGetterFloat): T
/**
* This matrix trace. If matrix is not square matrix, null is returned.
*/
val trace: Float?
/**
* This matrix determinant. If matrix is not square matrix, null is returned.
*/
val determinant: Float?
/**
* This matrix' inverse matrix (this * I = identity).
*
* This operation is computation intense, do cache its result.
*
* If this matrix is not square matrix, or determinant is zero, null is returned.
*/
val inverse: T?
/**
* This matrix' cofactor matrix.
*
* This operation is computation intense, do cache its result.
*
* If this matrix is not square matrix, null is returned.
*/
val cofactor: T?
/**
* This matrix' adjugate matrix.
*
* This operation is computation intense, do cache its result.
*
* 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 {
/**
* Multiplies all matrix components by [other], writing result into this matrix
*
* @return this matrix
*/
fun timesMut(other: Float): T
/**
* Divides all matrix components by [other], writing result into this matrix
*
* @return this matrix
*/
fun divMut(other: Float): T
/**
* Multiplies this and [other] matrices by general matrix multiplication rules,
* if they are compatible in dimensions, and write result into this matrix.
*
* @throws IllegalArgumentException if [other] is not a square matrix
* @throws IllegalStateException if this matrix is not a square matrix
* @return this matrix
*/
fun timesMut(other: IMatrixGetterFloat): T
/**
* Adds components of both matrices, writing result into this matrix
*
* @throws IllegalArgumentException if matrices are different in dimensions
* @return this matrix
*/
fun plusMut(other: IMatrixGetterFloat): T
/**
* Subtracts components of both matrices, writing result into this matrix
*
* @throws IllegalArgumentException if matrices are different in dimensions
* @return this matrix
*/
fun minusMut(other: IMatrixGetterFloat): T
}

448
src/kvector/kotlin/ru/dbotthepony/kvector/api/Matrix.kt
View File

@ -1,448 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.api
/**
* Classes implementing this interface can be manipulated like matrices
*/
interface IMatrixLike {
/**
* Rows this matrix has
*/
val rows: Int
/**
* Columns this matrix has
*/
val columns: Int
/**
* Whenever is this matrix a square matrix
*/
val isSquareMatrix: Boolean get() = rows == columns
/**
* @return Whenever [other] has the same dimensions as this matrix
*/
fun sizeEquals(other: IMatrixLike): Boolean {
return rows == other.rows && columns == other.columns
}
/**
* @throws IllegalArgumentException if [other] is not the same dimensions as this matrix
*/
fun requireSizeEquals(other: IMatrixLike) {
if (!sizeEquals(other)) {
throw IllegalArgumentException("Other matrix (${other.columns}x${other.rows}) is not the same dimensions as this matrix (${columns}x${rows})")
}
}
/**
* @throws IllegalArgumentException if [other] is not the same dimensions as this matrix
*/
fun requireSizeEquals(other: IMatrixLike, lazy: () -> Any) {
if (!sizeEquals(other)) {
throw IllegalArgumentException(lazy.invoke().toString())
}
}
/**
* @throws IllegalStateException if [other] is not the same dimensions as this matrix
*/
fun checkSizeEquals(other: IMatrixLike) {
if (!sizeEquals(other)) {
throw IllegalStateException("Other matrix (${other.columns}x${other.rows}) is not the same dimensions as this matrix (${columns}x${rows})")
}
}
/**
* @throws IllegalStateException if [other] is not the same dimensions as this matrix
*/
fun checkSizeEquals(other: IMatrixLike, lazy: () -> Any) {
if (!sizeEquals(other)) {
throw IllegalStateException(lazy.invoke().toString())
}
}
/**
* @return Whenever [other] has the same amount of rows as this matrix
*/
fun rowsEquals(other: IMatrixLike): Boolean {
return rows == other.rows
}
/**
* @throws IllegalArgumentException if [other] is not the same amount of rows as this matrix
*/
fun requireRowsEquals(other: IMatrixLike) {
if (!rowsEquals(other)) {
throw IllegalArgumentException("Other matrix (${other.rows}) has different amount of rows compared to this matrix (${rows})")
}
}
/**
* @throws IllegalArgumentException if [other] is not the same amount of rows as this matrix
*/
fun requireRowsEquals(other: IMatrixLike, lazy: () -> Any) {
if (!rowsEquals(other)) {
throw IllegalArgumentException(lazy.invoke().toString())
}
}
/**
* @throws IllegalStateException if [other] is not the same amount of rows as this matrix
*/
fun checkRowsEquals(other: IMatrixLike) {
if (!rowsEquals(other)) {
throw IllegalStateException("Other matrix (${other.rows}) has different amount of rows compared to this matrix (${rows})")
}
}
/**
* @throws IllegalStateException if [other] is not the same amount of rows as this matrix
*/
fun checkRowsEquals(other: IMatrixLike, lazy: () -> Any) {
if (!rowsEquals(other)) {
throw IllegalStateException(lazy.invoke().toString())
}
}
/**
* @return Whenever [other] has the same amount of columns as this matrix
*/
fun columnsEquals(other: IMatrixLike): Boolean {
return columns == other.columns
}
/**
* @throws IllegalArgumentException if [other] is not the same amount of columns as this matrix
*/
fun requireColumnsEquals(other: IMatrixLike) {
if (!rowsEquals(other)) {
throw IllegalArgumentException("Other matrix (${other.columns}) has different amount of columns compared to this matrix (${columns})")
}
}
/**
* @throws IllegalArgumentException if [other] is not the same amount of columns as this matrix
*/
fun requireColumnsEquals(other: IMatrixLike, lazy: () -> Any) {
if (!rowsEquals(other)) {
throw IllegalArgumentException(lazy.invoke().toString())
}
}
/**
* @throws IllegalStateException if [other] is not the same amount of columns as this matrix
*/
fun checkColumnsEquals(other: IMatrixLike) {
if (!rowsEquals(other)) {
throw IllegalStateException("Other matrix (${other.columns}) has different amount of columns compared to this matrix (${columns})")
}
}
/**
* @throws IllegalStateException if [other] is not the same amount of columns as this matrix
*/
fun checkColumnsEquals(other: IMatrixLike, lazy: () -> Any) {
if (!rowsEquals(other)) {
throw IllegalStateException(lazy.invoke().toString())
}
}
}
interface IMatrixGetterInt : IMatrixLike {
/**
* @return component of this matrix, as int
*/
operator fun get(column: Int, row: Int): Int
}
interface IMatrixGetterLong : IMatrixLike {
/**
* @return component of this matrix, as long
*/
operator fun get(column: Int, row: Int): Long
}
interface IMatrixGetterShort : IMatrixLike {
/**
* @return component of this matrix, as short
*/
operator fun get(column: Int, row: Int): Short
}
interface IMatrixGetterByte : IMatrixLike {
/**
* @return component of this matrix, as byte
*/
operator fun get(column: Int, row: Int): Byte
}
interface IMatrixSetterInt : IMatrixLike {
/**
* Sets component of this matrix, as int
*/
operator fun set(column: Int, row: Int, value: Int)
}
interface IMatrixSetterLong : IMatrixLike {
/**
* Sets component of this matrix, as long
*/
operator fun set(column: Int, row: Int, value: Long)
}
interface IMatrixSetterShort : IMatrixLike {
/**
* Sets component of this matrix, as short
*/
operator fun set(column: Int, row: Int, value: Short)
}
interface IMatrixSetterByte : IMatrixLike {
/**
* Sets component of this matrix, as byte
*/
operator fun set(column: Int, row: Int, value: Byte)
}
/**
* Root inheritance tree of Matrix classes
*/
interface IMatrix<T : IMatrix<T>> : IMatrixLike {
/**
* Whenever all components of this matrix are finite
*/
val isFinite: Boolean
/**
* Whenever any of components of this matrix are NaN
*/
val isNaN: Boolean
/**
* Whenever is this matrix valid (components are not NaN and are finite)
*/
val isValid: Boolean get() = isFinite && !isNaN
/**
* @throws IllegalArgumentException if matrix is not finite
*/
fun requireIsFinite(lambda: () -> Any) {
if (!isFinite) {
throw IllegalArgumentException(lambda.invoke().toString())
}
}
/**
* @throws IllegalArgumentException if matrix is not finite
*/
fun requireIsFinite() {
if (!isFinite) {
throw IllegalArgumentException("Matrix is not finite")
}
}
/**
* @throws IllegalStateException if matrix is not finite
*/
fun checkIsFinite(lambda: () -> Any) {
if (!isFinite) {
throw IllegalStateException(lambda.invoke().toString())
}
}
/**
* @throws IllegalStateException if matrix is not finite
*/
fun checkIsFinite() {
if (!isFinite) {
throw IllegalStateException("Matrix is not finite")
}
}
/**
* @throws IllegalArgumentException if vector has NaN component
*/
fun requireNotNaN(lambda: () -> Any) {
if (isNaN) {
throw IllegalArgumentException(lambda.invoke().toString())
}
}
/**
* @throws IllegalArgumentException if matrix has NaN component
*/
fun requireNotNaN() {
if (isNaN) {
throw IllegalArgumentException("Matrix has NaN component")
}
}
/**
* @throws IllegalStateException if matrix has NaN component
*/
fun checkNotNaN(lambda: () -> Any) {
if (!isFinite) {
throw IllegalStateException(lambda.invoke().toString())
}
}
/**
* @throws IllegalStateException if matrix has NaN component
*/
fun checkNotNaN() {
if (!isFinite) {
throw IllegalStateException("Matrix has NaN component")
}
}
/**
* @throws IllegalArgumentException if matrix has NaN or infinite component
*/
fun requireIsValid() {
requireIsFinite()
requireNotNaN()
}
/**
* @throws IllegalStateException if matrix has NaN or infinite component
*/
fun checkIsValid() {
requireIsFinite()
requireNotNaN()
}
/**
* @throws IllegalArgumentException if matrix has NaN or infinite component
*/
fun requireIsValid(lambda: () -> Any) {
requireIsFinite(lambda)
requireNotNaN(lambda)
}
/**
* @throws IllegalStateException if matrix has NaN or infinite component
*/
fun checkIsValid(lambda: () -> Any) {
requireIsFinite(lambda)
requireNotNaN(lambda)
}
}
/**
* Marks matrix for being transposable with result of T
*/
interface ITransposable<T> {
/**
* New matrix, which is transposed variant of this matrix
*
* @return new matrix
*/
val transposed: T
}
/**
* Marks matrix for being transposable in place
*/
interface IMutableTransposable<T> {
/**
* Attempts to transpose this matrix, writing the transpose result into itself
*
* @throws IllegalStateException if this matrix is not a square matrix
* @return this matrix
*/
fun transpose(): T
}
/**
* Getter for matrix complement
*/
interface IMatrixComplement<T> {
/**
* Constructs a new matrix, representing complement of this matrix at [column] and [row]
*
* @throws IllegalStateException if this matrix is not applicable for complement operation
* @return new matrix, representing minor, with size - 1 of this matrix
*/
fun complement(column: Int, row: Int): T
}
/**
* Defines common operations with concrete square matrices, such as [translation], [scale]ing, etc
*/
interface ISquareMatrix<T : ISquareMatrix<T, V, F>, V, F> {
/**
* Current translation of this matrix.
*/
val translation: V
/**
* Does raw translation of this matrix by specified [vector],
* returning new matrix as result.
*
* If you scaled and/or rotated this matrix, and want to move it
* with scale and translation, use [translateWithMultiplication].
*
* @return new matrix
*/
fun translate(vector: V): T
/**
* Does translation of this matrix by specified [vector],
* with accounting of it's state, returning new matrix as result.
*
* @return new matrix
*/
fun translateWithMultiplication(vector: V): T
/**
* Multiplies main diagonal of this matrix with [vector],
* returning new matrix as result.
*
* @return new matrix
*/
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, F>, V, F> : ISquareMatrix<T, V, F> {
/**
* Current translation of this matrix.
*
* Writing vector to this property will set raw matrix translation.
*/
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.
*
* @return this matrix
*/
fun translateWithMultiplicationMut(vector: V): T
/**
* Multiplies main diagonal of this matrix with [vector],
* writing result into this matrix.
*
* @return this matrix
*/
fun scaleMut(vector: F): T
}

82
src/kvector/kotlin/ru/dbotthepony/kvector/api/Struct.kt
View File

@ -1,82 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.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
}
interface IStruct2l {
operator fun component1(): Long
operator fun component2(): Long
}
interface IStruct3l : IStruct2l {
operator fun component3(): Long
}
interface IStruct4l : IStruct3l {
operator fun component4(): Long
}
interface IStruct2s {
operator fun component1(): Short
operator fun component2(): Short
}
interface IStruct3s : IStruct2s {
operator fun component3(): Short
}
interface IStruct4s : IStruct3s {
operator fun component4(): Short
}
interface IStruct2b {
operator fun component1(): Byte
operator fun component2(): Byte
}
interface IStruct3b : IStruct2b {
operator fun component3(): Byte
}
interface IStruct4b : IStruct3b {
operator fun component4(): Byte
}

433
src/kvector/kotlin/ru/dbotthepony/kvector/api/Vector.kt
View File

@ -1,433 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.api
import kotlin.math.sqrt
/**
* Root interface of vector inheritance tree, providing all relevant methods
* of operation on vectors of fixed length
*/
interface IVector<T : IVector<T>> {
/**
* Length of this vector, as Double
*/
val length: Double get() = sqrt(lengthSquared)
/**
* Squared length of this vector, as Double
*/
val lengthSquared: Double
/**
* Whenever all components of vector are finite
*
* Keep in mind that in some situations infinite vectors are still making sense
* (e.g. creating [AABB] that cover entire game, or alike, world)
*/
val isFinite: Boolean
/**
* Whenever at least one of components of vector are NaN
*/
val isNaN: Boolean
/**
* Whenever is this vector valid (components are not NaN and are finite)
*
* Keep in mind that in some situations infinite vectors are still making sense
* (e.g. creating [AABB] that cover entire game, or alike, world)
*/
val isValid: Boolean get() = isFinite && !isNaN
/**
* Adds components of both vectors, returning new vector as result
*
* @return new vector
*/
operator fun plus(other: T): T
/**
* Subtracts components of both vectors, returning new vector as result
*
* @return new vector
*/
operator fun minus(other: T): T
/**
* Multiplies components of both vectors, returning new vector as result
*
* @return new vector
*/
operator fun times(other: T): T
/**
* Divides components of both vectors, returning new vector as result
*
* @throws ArithmeticException if vector work with whole numbers, and
* one of [other] components end up zero
*
* @return new vector
*/
operator fun div(other: T): T
/**
* New vector, with every component being absolute
*/
val absoluteValue: T
/**
* Takes all components of both vectors, and choose the smallest of each,
* and construct a new vector from them
*
* @return new vector
*/
fun coerceAtMost(maximal: T): T
/**
* Takes all components of both vectors, and choose the biggest of each,
* and construct a new vector from them
*
* @return new vector
*/
fun coerceAtLeast(minimal: T): T
/**
* Takes all components of this vector, and clamp them in between [minimal] and [maximal] corresponding
* components and construct a new vector from them
*
* @return new vector
*/
fun clamp(minimal: T, maximal: T): T
/**
* Constructs a new vector with all components negated
*
* @return new vector
*/
operator fun unaryMinus(): T
/**
* Returns square rooted distance bewteen this and [other]
*
* @return distance between vectors (as points) as [Double]
*/
fun distance(other: T): Double {
return sqrt(distanceSquared(other))
}
/**
* Returns squared distance between this and [other]
*
* @return distance between vectors (as points) as [Double]
*/
fun distanceSquared(other: T): Double
}
/**
* Define all relatable methods on vectors which components are fractional
*/
interface IFractionalVector<T : IFractionalVector<T>> {
/**
* New (directional) vector, with length of 1
*
* If vector length is zero, a zero vector is returned
*/
val normalized: T
}
/**
* Define all relatable mutating methods on vectors which components are fractional
*/
interface IMutableFractionalVector<T : IMutableFractionalVector<T>> {
/**
* Normalizes this vector (by setting all of its components) to have 1 unit length
* (turning it into directional or unit vector),
* returning old length as [Double]
*
* @return old length as [Double]
*/
fun normalize(): Double
}
/**
* Define all relatable methods on vectors which components are whole numbers
*/
interface IWholeVector<T : IWholeVector<T>> {
/**
* Returns squared distance between this and [other], but as whole number
*
* @return distance between vectors (as points) as [Long]
*/
fun wholeDistanceSquared(other: T): Long
}
/**
* Define all relatable mutating methods on vectors which components are whole numbers
*/
interface IMutableWholeVector<T : IMutableWholeVector<T>> {
}
/**
* Define mutable vector x vector operations
*/
interface IMutableVector<T : IMutableVector<T, V>, V> {
/**
* Adds components of both vectors, setting result to this vector
*
* @return this vector
*/
fun plusMut(other: V): T
/**
* Subtracts components of both vectors, setting result to this vector
*
* @return this vector
*/
fun minusMut(other: V): T
/**
* Multiplies components of both vectors, setting result to this vector
*
* @return this vector
*/
fun timesMut(other: V): T
/**
* Divides components of both vectors, setting result to this vector
*
* @throws ArithmeticException if vector work with whole numbers, and
* one of [other] components end up zero
*
* @return this vector
*/
fun divMut(other: V): T
}
/**
* Define mutable scalar x vector operations using [Double]s
*/
interface IMutableDoubleVector<T : IMutableDoubleVector<T>> {
/**
* Multiplies components of vector by [other], setting result to this vector
*
* @return this vector
*/
fun timesMut(other: Double): T
/**
* Divides components of vector by [other], setting result to this vector
*
* @return this vector
*/
fun divMut(other: Double): T
}
/**
* Define scalar x vector operations using [Double]s
*/
interface IDoubleVector<T : IDoubleVector<T>> {
/**
* Multiplies [other] by all components, returning new vector as result
*
* @return new vector
*/
operator fun times(other: Double): T
/**
* Divides components by [other], returning new vector as result
*
* @return new vector
*/
operator fun div(other: Double): T
}
/**
* Define mutable scalar x vector operations using [Float]s
*/
interface IMutableFloatVector<T : IMutableFloatVector<T>> {
/**
* Multiplies components of vector by [other], setting result to this vector
*
* @return this vector
*/
fun timesMut(other: Float): T
/**
* Divides components of vector by [other], setting result to this vector
*
* @return this vector
*/
fun divMut(other: Float): T
}
/**
* Define scalar x vector operations using [Float]s
*/
interface IFloatVector<T : IFloatVector<T>> {
/**
* Multiplies [other] by all components, returning new vector as result
*
* @return new vector
*/
operator fun times(other: Float): T
/**
* Divides components by [other], returning new vector as result
*
* @return new vector
*/
operator fun div(other: Float): T
}
/**
* Define mutable scalar x vector operations using [Int]s
*/
interface IMutableIntVector<T : IMutableIntVector<T>> {
/**
* Multiplies components of vector by [other], setting result to this vector
*
* @return this vector
*/
fun timesMut(other: Int): T
/**
* Divides components of vector by [other], setting result to this vector
*
* @return this vector
*/
fun divMut(other: Int): T
}
/**
* Define scalar x vector operations using [Int]s
*/
interface IIntVector<T : IIntVector<T>> {
/**
* Multiplies [other] by all components, returning new vector as result
*
* @return new vector
*/
operator fun times(other: Int): T
/**
* Divides components by [other], returning new vector as result
*
* @return new vector
*/
operator fun div(other: Int): T
}
/**
* Root class of vector inheritance tree, providing some concrete methods
* for vectors of fixed length
*/
abstract class AbstractVector<T : AbstractVector<T>> : IMatrixLike, IVector<T> {
final override val columns: Int = 1
/**
* @throws IllegalArgumentException if vector is not finite
*/
inline fun requireIsFinite(lambda: () -> Any) {
if (!isFinite) {
throw IllegalArgumentException(lambda.invoke().toString())
}
}
/**
* @throws IllegalArgumentException if vector is not finite
*/
inline fun requireIsFinite() {
if (!isFinite) {
throw IllegalArgumentException("Vector is not finite")
}
}
/**
* @throws IllegalStateException if vector is not finite
*/
inline fun checkIsFinite(lambda: () -> Any) {
if (!isFinite) {
throw IllegalStateException(lambda.invoke().toString())
}
}
/**
* @throws IllegalStateException if vector is not finite
*/
inline fun checkIsFinite() {
if (!isFinite) {
throw IllegalStateException("Vector is not finite")
}
}
/**
* @throws IllegalArgumentException if vector has NaN component
*/
inline fun requireNotNaN(lambda: () -> Any) {
if (isNaN) {
throw IllegalArgumentException(lambda.invoke().toString())
}
}
/**
* @throws IllegalArgumentException if vector has NaN component
*/
inline fun requireNotNaN() {
if (isNaN) {
throw IllegalArgumentException("Vector has NaN component")
}
}
/**
* @throws IllegalStateException if vector has NaN component
*/
inline fun checkNotNaN(lambda: () -> Any) {
if (!isFinite) {
throw IllegalStateException(lambda.invoke().toString())
}
}
/**
* @throws IllegalStateException if vector has NaN component
*/
inline fun checkNotNaN() {
if (!isFinite) {
throw IllegalStateException("Vector has NaN component")
}
}
/**
* @throws IllegalArgumentException if vector has NaN or infinite component
*/
inline fun requireIsValid() {
requireIsFinite()
requireNotNaN()
}
/**
* @throws IllegalStateException if vector has NaN or infinite component
*/
inline fun checkIsValid() {
requireIsFinite()
requireNotNaN()
}
/**
* @throws IllegalArgumentException if vector has NaN or infinite component
*/
inline fun requireIsValid(lambda: () -> Any) {
requireIsFinite(lambda)
requireNotNaN(lambda)
}
/**
* @throws IllegalStateException if vector has NaN or infinite component
*/
inline fun checkIsValid(lambda: () -> Any) {
requireIsFinite(lambda)
requireNotNaN(lambda)
}
}

607
src/kvector/kotlin/ru/dbotthepony/kvector/api/concrete/DoubleMatrix.kt
View File

@ -1,607 +0,0 @@
package ru.dbotthepony.kvector.api.concrete
import ru.dbotthepony.kvector.api.IDoubleMatrix
import ru.dbotthepony.kvector.api.IMatrix
import ru.dbotthepony.kvector.api.ISquareMatrix
import ru.dbotthepony.kvector.api.ITransposable
import ru.dbotthepony.kvector.matrix.*
import ru.dbotthepony.kvector.matrix.ndouble.*
import ru.dbotthepony.kvector.matrix.nfloat.*
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
import ru.dbotthepony.kvector.vector.ndouble.Vector3d
import ru.dbotthepony.kvector.vector.ndouble.Vector4d
/**
* [Matrix2d] and [MutableMatrix2d] implement this
*/
interface IMatrix2d<T : IMatrix2d<T>> : IMatrix<T>, IDoubleMatrix<T>, ITransposable<T> {
val c00: Double
val c10: Double
val c01: Double
val c11: Double
val r00: Double
val r01: Double
val r10: Double
val r11: Double
val a11: Double
val a21: Double
val a12: Double
val a22: Double
val b11: Double
val b21: Double
val b12: Double
val b22: Double
val c0: Vector2d
val c1: Vector2d
val r0: Vector2d
val r1: Vector2d
/**
* Creates a copy of this matrix as [Matrix4d],
* setting last two rows and columns to 0
*/
fun toMatrix4d(): Matrix4d {
return Matrix4d(
c00, c01, 0.0, 0.0,
c10, c11, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [Matrix3d],
* setting last row and column to 0
*/
fun toMatrix3d(): Matrix3d {
return Matrix3d(
c00, c01, 0.0,
c10, c11, 0.0,
0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [Matrix2d]
*/
fun toMatrix2d(): Matrix2d {
return Matrix2d(
c00, c01,
c10, c11,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4d],
* setting last two columns and rows to 0
*/
fun toMutableMatrix4d(): MutableMatrix4d {
return MutableMatrix4d(
c00, c01, 0.0, 0.0,
c10, c11, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3d],
* setting last column and row to 0
*/
fun toMutableMatrix3d(): MutableMatrix3d {
return MutableMatrix3d(
c00, c01, 0.0,
c10, c11, 0.0,
0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2d]
*/
fun toMutableMatrix2d(): MutableMatrix2d {
return MutableMatrix2d(
c00, c01,
c10, c11,
)
}
/**
* Creates a copy of this matrix as [Matrix4f],
* casing all [Double]s to [Float]s,
* setting last two columns and rows to 0
*/
fun toMatrix4f(): Matrix4f {
return Matrix4f(
c00.toFloat(), c01.toFloat(), 0f, 0f,
c10.toFloat(), c11.toFloat(), 0f, 0f,
0f, 0f, 0f, 0f,
0f, 0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [Matrix3f],
* casing all [Double]s to [Float]s,
* setting last column and row to 0
*/
fun toMatrix3f(): Matrix3f {
return Matrix3f(
c00.toFloat(), c01.toFloat(), 0f,
c10.toFloat(), c11.toFloat(), 0f,
0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [Matrix2f],
* truncating last row and column
*/
fun toMatrix2f(): Matrix2f {
return Matrix2f(
c00.toFloat(), c01.toFloat(),
c10.toFloat(), c11.toFloat(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4f],
* setting last two columns and rows to 0,
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix4f(): MutableMatrix4f {
return MutableMatrix4f(
c00.toFloat(), c01.toFloat(), 0f, 0f,
c10.toFloat(), c11.toFloat(), 0f, 0f,
0f, 0f, 0f, 0f,
0f, 0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3f],
* setting last column and row to 0,
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix3f(): MutableMatrix3f {
return MutableMatrix3f(
c00.toFloat(), c01.toFloat(), 0f,
c10.toFloat(), c11.toFloat(), 0f,
0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2f],
* truncating last row and column,
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix2f(): MutableMatrix2f {
return MutableMatrix2f(
c00.toFloat(), c01.toFloat(),
c10.toFloat(), c11.toFloat(),
)
}
}
/**
* [Matrix3d] and [MutableMatrix3d] implement this
*/
interface IMatrix3d<T : IMatrix3d<T>> : IMatrix<T>, IDoubleMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector2d, Vector3d> {
val c00: Double
val c10: Double
val c20: Double
val c01: Double
val c11: Double
val c21: Double
val c02: Double
val c12: Double
val c22: Double
val r00: Double
val r01: Double
val r02: Double
val r10: Double
val r11: Double
val r12: Double
val r20: Double
val r21: Double
val r22: Double
val a11: Double
val a21: Double
val a31: Double
val a12: Double
val a22: Double
val a32: Double
val a13: Double
val a23: Double
val a33: Double
val b11: Double
val b21: Double
val b31: Double
val b12: Double
val b22: Double
val b32: Double
val b13: Double
val b23: Double
val b33: Double
val c0: Vector3d
val c1: Vector3d
val c2: Vector3d
val r0: Vector3d
val r1: Vector3d
val r2: Vector3d
override val translation: Vector2d get() = Vector2d(r02, r12)
/**
* Creates a copy of this matrix as [Matrix4d],
* setting last row and column to 0
*/
fun toMatrix4d(): Matrix4d {
return Matrix4d(
c00, c01, c02, 0.0,
c10, c11, c12, 0.0,
c20, c21, c22, 0.0,
0.0, 0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [Matrix3d],
*/
fun toMatrix3d(): Matrix3d {
return Matrix3d(
c00, c01, c02,
c10, c11, c12,
c20, c21, c22,
)
}
/**
* Creates a copy of this matrix as [Matrix2d],
* truncating last row and column
*/
fun toMatrix2d(): Matrix2d {
return Matrix2d(
c00, c01,
c10, c11,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4d],
* setting last column and row to 0
*/
fun toMutableMatrix4d(): MutableMatrix4d {
return MutableMatrix4d(
c00, c01, c02, 0.0,
c10, c11, c12, 0.0,
c20, c21, c22, 0.0,
0.0, 0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3d]
*/
fun toMutableMatrix3d(): MutableMatrix3d {
return MutableMatrix3d(
c00, c01, c02,
c10, c11, c12,
c20, c21, c22,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2d],
* truncating last row and column
*/
fun toMutableMatrix2d(): MutableMatrix2d {
return MutableMatrix2d(
c00, c01,
c10, c11,
)
}
/**
* Creates a copy of this matrix as [Matrix4f],
* casing all [Double]s to [Float]s,
* setting last column and row to 0
*/
fun toMatrix4f(): Matrix4f {
return Matrix4f(
c00.toFloat(), c01.toFloat(), c02.toFloat(), 0f,
c10.toFloat(), c11.toFloat(), c12.toFloat(), 0f,
c20.toFloat(), c21.toFloat(), c22.toFloat(), 0f,
0f, 0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [Matrix3f],
* casing all [Double]s to [Float]s
*/
fun toMatrix3f(): Matrix3f {
return Matrix3f(
c00.toFloat(), c01.toFloat(), c02.toFloat(),
c10.toFloat(), c11.toFloat(), c12.toFloat(),
c20.toFloat(), c21.toFloat(), c22.toFloat(),
)
}
/**
* Creates a copy of this matrix as [Matrix2f],
* truncating last row and column,
* casing all [Double]s to [Float]s
*/
fun toMatrix2f(): Matrix2f {
return Matrix2f(
c00.toFloat(), c01.toFloat(),
c10.toFloat(), c11.toFloat(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4f],
* setting last column and row to 0,
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix4f(): MutableMatrix4f {
return MutableMatrix4f(
c00.toFloat(), c01.toFloat(), c02.toFloat(), 0f,
c10.toFloat(), c11.toFloat(), c12.toFloat(), 0f,
c20.toFloat(), c21.toFloat(), c22.toFloat(), 0f,
0f, 0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3f],
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix3f(): MutableMatrix3f {
return MutableMatrix3f(
c00.toFloat(), c01.toFloat(), c02.toFloat(),
c10.toFloat(), c11.toFloat(), c12.toFloat(),
c20.toFloat(), c21.toFloat(), c22.toFloat(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2f],
* truncating last row and column,
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix2f(): MutableMatrix2f {
return MutableMatrix2f(
c00.toFloat(), c01.toFloat(),
c10.toFloat(), c11.toFloat(),
)
}
}
/**
* [Matrix4d] and [MutableMatrix4d] implement this
*/
interface IMatrix4d<T : IMatrix4d<T>> : IMatrix<T>, IDoubleMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector3d, Vector4d> {
val c00: Double
val c10: Double
val c20: Double
val c30: Double
val c01: Double
val c11: Double
val c21: Double
val c31: Double
val c02: Double
val c12: Double
val c22: Double
val c32: Double
val c03: Double
val c13: Double
val c23: Double
val c33: Double
val r00: Double
val r01: Double
val r02: Double
val r03: Double
val r10: Double
val r11: Double
val r12: Double
val r13: Double
val r20: Double
val r21: Double
val r22: Double
val r23: Double
val r30: Double
val r31: Double
val r32: Double
val r33: Double
val a11: Double
val a21: Double
val a31: Double
val a41: Double
val a12: Double
val a22: Double
val a32: Double
val a42: Double
val a13: Double
val a23: Double
val a33: Double
val a43: Double
val a14: Double
val a24: Double
val a34: Double
val a44: Double
val b11: Double
val b21: Double
val b31: Double
val b41: Double
val b12: Double
val b22: Double
val b32: Double
val b42: Double
val b13: Double
val b23: Double
val b33: Double
val b43: Double
val b14: Double
val b24: Double
val b34: Double
val b44: Double
val c0: Vector4d
val c1: Vector4d
val c2: Vector4d
val c3: Vector4d
val r0: Vector4d
val r1: Vector4d
val r2: Vector4d
val r3: Vector4d
override val translation: Vector3d get() = Vector3d(r03, r13, r23)
/**
* Creates a copy of this matrix as [Matrix4d]
*/
fun toMatrix4d(): Matrix4d {
return Matrix4d(
c00, c01, c02, c03,
c10, c11, c12, c13,
c20, c21, c22, c23,
c30, c31, c32, c33,
)
}
/**
* Creates a copy of this matrix as [Matrix3d],
* truncating last row and column
*/
fun toMatrix3d(): Matrix3d {
return Matrix3d(
c00, c01, c02,
c10, c11, c12,
c20, c21, c22,
)
}
/**
* Creates a copy of this matrix as [Matrix2d],
* truncating two last rows and columns
*/
fun toMatrix2d(): Matrix2d {
return Matrix2d(
c00, c01,
c10, c11,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4d]
*/
fun toMutableMatrix4d(): MutableMatrix4d {
return MutableMatrix4d(
c00, c01, c02, c03,
c10, c11, c12, c13,
c20, c21, c22, c23,
c30, c31, c32, c33,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3d],
* truncating last row and column
*/
fun toMutableMatrix3d(): MutableMatrix3d {
return MutableMatrix3d(
c00, c01, c02,
c10, c11, c12,
c20, c21, c22,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2d],
* truncating two last rows and columns
*/
fun toMutableMatrix2d(): MutableMatrix2d {
return MutableMatrix2d(
c00, c01,
c10, c11,
)
}
/**
* Creates a copy of this matrix as [Matrix4f],
* casing all [Double]s to [Float]s
*/
fun toMatrix4f(): Matrix4f {
return Matrix4f(
c00.toFloat(), c01.toFloat(), c02.toFloat(), c03.toFloat(),
c10.toFloat(), c11.toFloat(), c12.toFloat(), c13.toFloat(),
c20.toFloat(), c21.toFloat(), c22.toFloat(), c23.toFloat(),
c30.toFloat(), c31.toFloat(), c32.toFloat(), c33.toFloat(),
)
}
/**
* Creates a copy of this matrix as [Matrix3f],
* truncating last row and column,
* casing all [Double]s to [Float]s
*/
fun toMatrix3f(): Matrix3f {
return Matrix3f(
c00.toFloat(), c01.toFloat(), c02.toFloat(),
c10.toFloat(), c11.toFloat(), c12.toFloat(),
c20.toFloat(), c21.toFloat(), c22.toFloat(),
)
}
/**
* Creates a copy of this matrix as [Matrix2f],
* truncating two last rows and columns,
* casing all [Double]s to [Float]s
*/
fun toMatrix2f(): Matrix2f {
return Matrix2f(
c00.toFloat(), c01.toFloat(),
c10.toFloat(), c11.toFloat(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4f],
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix4f(): MutableMatrix4f {
return MutableMatrix4f(
c00.toFloat(), c01.toFloat(), c02.toFloat(), c03.toFloat(),
c10.toFloat(), c11.toFloat(), c12.toFloat(), c13.toFloat(),
c20.toFloat(), c21.toFloat(), c22.toFloat(), c23.toFloat(),
c30.toFloat(), c31.toFloat(), c32.toFloat(), c33.toFloat(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3f],
* truncating last row and column,
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix3f(): MutableMatrix3f {
return MutableMatrix3f(
c00.toFloat(), c01.toFloat(), c02.toFloat(),
c10.toFloat(), c11.toFloat(), c12.toFloat(),
c20.toFloat(), c21.toFloat(), c22.toFloat(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2f],
* truncating two last rows and columns,
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix2f(): MutableMatrix2f {
return MutableMatrix2f(
c00.toFloat(), c01.toFloat(),
c10.toFloat(), c11.toFloat(),
)
}
}

611
src/kvector/kotlin/ru/dbotthepony/kvector/api/concrete/FloatMatrix.kt
View File

@ -1,611 +0,0 @@
@file:Suppress("unused")
package ru.dbotthepony.kvector.api.concrete
import ru.dbotthepony.kvector.api.IFloatMatrix
import ru.dbotthepony.kvector.api.IMatrix
import ru.dbotthepony.kvector.api.ISquareMatrix
import ru.dbotthepony.kvector.api.ITransposable
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
*/
interface IMatrix2f<T : IMatrix2f<T>> : IMatrix<T>, IFloatMatrix<T>, ITransposable<T> {
val c00: Float
val c10: Float
val c01: Float
val c11: Float
val r00: Float
val r01: Float
val r10: Float
val r11: Float
val a11: Float
val a21: Float
val a12: Float
val a22: Float
val b11: Float
val b21: Float
val b12: Float
val b22: Float
val c0: Vector2f
val c1: Vector2f
val r0: Vector2f
val r1: Vector2f
/**
* Creates a copy of this matrix as [Matrix4d],
* setting last two rows and columns to 0,
* casting all [Float]s to [Double]s
*/
fun toMatrix4d(): Matrix4d {
return Matrix4d(
c00.toDouble(), c01.toDouble(), 0.0, 0.0,
c10.toDouble(), c11.toDouble(), 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [Matrix3d],
* setting last row and column to 0,
* casting all [Float]s to [Double]s
*/
fun toMatrix3d(): Matrix3d {
return Matrix3d(
c00.toDouble(), c01.toDouble(), 0.0,
c10.toDouble(), c11.toDouble(), 0.0,
0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [Matrix2d],
* casting all [Float]s to [Double]s
*/
fun toMatrix2d(): Matrix2d {
return Matrix2d(
c00.toDouble(), c01.toDouble(),
c10.toDouble(), c11.toDouble(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4d],
* setting last two columns and rows to 0,
* casting all [Float]s to [Double]s
*/
fun toMutableMatrix4d(): MutableMatrix4d {
return MutableMatrix4d(
c00.toDouble(), c01.toDouble(), 0.0, 0.0,
c10.toDouble(), c11.toDouble(), 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3d],
* setting last column and row to 0,
* casting all [Float]s to [Double]s
*/
fun toMutableMatrix3d(): MutableMatrix3d {
return MutableMatrix3d(
c00.toDouble(), c01.toDouble(), 0.0,
c10.toDouble(), c11.toDouble(), 0.0,
0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2d],
* casting all [Float]s to [Double]s
*/
fun toMutableMatrix2d(): MutableMatrix2d {
return MutableMatrix2d(
c00.toDouble(), c01.toDouble(),
c10.toDouble(), c11.toDouble(),
)
}
/**
* Creates a copy of this matrix as [Matrix4f],
* setting last two columns and rows to 0
*/
fun toMatrix4f(): Matrix4f {
return Matrix4f(
c00, c01, 0f, 0f,
c10, c11, 0f, 0f,
0f, 0f, 0f, 0f,
0f, 0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [Matrix3f],
* setting last column and row to 0
*/
fun toMatrix3f(): Matrix3f {
return Matrix3f(
c00, c01, 0f,
c10, c11, 0f,
0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [Matrix2f],
* truncating last row and column
*/
fun toMatrix2f(): Matrix2f {
return Matrix2f(
c00, c01,
c10, c11,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4f],
* setting last two columns and rows to 0,
*/
fun toMutableMatrix4f(): MutableMatrix4f {
return MutableMatrix4f(
c00, c01, 0f, 0f,
c10, c11, 0f, 0f,
0f, 0f, 0f, 0f,
0f, 0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3f],
* setting last column and row to 0,
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix3f(): MutableMatrix3f {
return MutableMatrix3f(
c00, c01, 0f,
c10, c11, 0f,
0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2f],
* truncating last row and column,
* casing all [Double]s to [Float]s
*/
fun toMutableMatrix2f(): MutableMatrix2f {
return MutableMatrix2f(
c00, c01,
c10, c11,
)
}
}
/**
* [Matrix3f] and [MutableMatrix3f] implement this
*/
interface IMatrix3f<T : IMatrix3f<T>> : IMatrix<T>, IFloatMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector2f, Vector3f> {
val c00: Float
val c10: Float
val c20: Float
val c01: Float
val c11: Float
val c21: Float
val c02: Float
val c12: Float
val c22: Float
val r00: Float
val r01: Float
val r02: Float
val r10: Float
val r11: Float
val r12: Float
val r20: Float
val r21: Float
val r22: Float
val a11: Float
val a21: Float
val a31: Float
val a12: Float
val a22: Float
val a32: Float
val a13: Float
val a23: Float
val a33: Float
val b11: Float
val b21: Float
val b31: Float
val b12: Float
val b22: Float
val b32: Float
val b13: Float
val b23: Float
val b33: Float
val c0: Vector3f
val c1: Vector3f
val c2: Vector3f
val r0: Vector3f
val r1: Vector3f
val r2: Vector3f
override val translation: Vector2f get() = Vector2f(r02, r12)
/**
* Creates a copy of this matrix as [Matrix4d],
* setting last row and column to 0,
* casting all [Float]s to [Double]s
*/
fun toMatrix4d(): Matrix4d {
return Matrix4d(
c00.toDouble(), c01.toDouble(), c02.toDouble(), 0.0,
c10.toDouble(), c11.toDouble(), c12.toDouble(), 0.0,
c20.toDouble(), c21.toDouble(), c22.toDouble(), 0.0,
0.0, 0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [Matrix3d],
* casting all [Float]s to [Double]s
*/
fun toMatrix3d(): Matrix3d {
return Matrix3d(
c00.toDouble(), c01.toDouble(), c02.toDouble(),
c10.toDouble(), c11.toDouble(), c12.toDouble(),
c20.toDouble(), c21.toDouble(), c22.toDouble(),
)
}
/**
* Creates a copy of this matrix as [Matrix2d],
* truncating last row and column,
* casting all [Float]s to [Double]s
*/
fun toMatrix2d(): Matrix2d {
return Matrix2d(
c00.toDouble(), c01.toDouble(),
c10.toDouble(), c11.toDouble(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4d],
* setting last column and row to 0,
* casting all [Float]s to [Double]s
*/
fun toMutableMatrix4d(): MutableMatrix4d {
return MutableMatrix4d(
c00.toDouble(), c01.toDouble(), c02.toDouble(), 0.0,
c10.toDouble(), c11.toDouble(), c12.toDouble(), 0.0,
c20.toDouble(), c21.toDouble(), c22.toDouble(), 0.0,
0.0, 0.0, 0.0, 0.0,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3d],
* casting all [Float]s to [Double]s
*/
fun toMutableMatrix3d(): MutableMatrix3d {
return MutableMatrix3d(
c00.toDouble(), c01.toDouble(), c02.toDouble(),
c10.toDouble(), c11.toDouble(), c12.toDouble(),
c20.toDouble(), c21.toDouble(), c22.toDouble(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2d],
* truncating last row and column,
* casting all [Float]s to [Double]s
*/
fun toMutableMatrix2d(): MutableMatrix2d {
return MutableMatrix2d(
c00.toDouble(), c01.toDouble(),
c10.toDouble(), c11.toDouble(),
)
}
/**
* Creates a copy of this matrix as [Matrix4f],
* setting last column and row to 0
*/
fun toMatrix4f(): Matrix4f {
return Matrix4f(
c00, c01, c02, 0f,
c10, c11, c12, 0f,
c20, c21, c22, 0f,
0f, 0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [Matrix3f]
*/
fun toMatrix3f(): Matrix3f {
return Matrix3f(
c00, c01, c02,
c10, c11, c12,
c20, c21, c22,
)
}
/**
* Creates a copy of this matrix as [Matrix2f],
* truncating last row and column
*/
fun toMatrix2f(): Matrix2f {
return Matrix2f(
c00, c01,
c10, c11,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4f],
* setting last column and row to 0
*/
fun toMutableMatrix4f(): MutableMatrix4f {
return MutableMatrix4f(
c00, c01, c02, 0f,
c10, c11, c12, 0f,
c20, c21, c22, 0f,
0f, 0f, 0f, 0f,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3f]
*/
fun toMutableMatrix3f(): MutableMatrix3f {
return MutableMatrix3f(
c00, c01, c02,
c10, c11, c12,
c20, c21, c22,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2f],
* truncating last row and column
*/
fun toMutableMatrix2f(): MutableMatrix2f {
return MutableMatrix2f(
c00, c01,
c10, c11,
)
}
}
/**
* [Matrix4f] and [MutableMatrix4f] implement this
*/
interface IMatrix4f<T : IMatrix4f<T>> : IMatrix<T>, IFloatMatrix<T>, ITransposable<T>, ISquareMatrix<T, Vector3f, Vector4f> {
val c00: Float
val c10: Float
val c20: Float
val c30: Float
val c01: Float
val c11: Float
val c21: Float
val c31: Float
val c02: Float
val c12: Float
val c22: Float
val c32: Float
val c03: Float
val c13: Float
val c23: Float
val c33: Float
val r00: Float
val r01: Float
val r02: Float
val r03: Float
val r10: Float
val r11: Float
val r12: Float
val r13: Float
val r20: Float
val r21: Float
val r22: Float
val r23: Float
val r30: Float
val r31: Float
val r32: Float
val r33: Float
val a11: Float
val a21: Float
val a31: Float
val a41: Float
val a12: Float
val a22: Float
val a32: Float
val a42: Float
val a13: Float
val a23: Float
val a33: Float
val a43: Float
val a14: Float
val a24: Float
val a34: Float
val a44: Float
val b11: Float
val b21: Float
val b31: Float
val b41: Float
val b12: Float
val b22: Float
val b32: Float
val b42: Float
val b13: Float
val b23: Float
val b33: Float
val b43: Float
val b14: Float
val b24: Float
val b34: Float
val b44: Float
val c0: Vector4f
val c1: Vector4f
val c2: Vector4f
val c3: Vector4f
val r0: Vector4f
val r1: Vector4f
val r2: Vector4f
val r3: Vector4f
override val translation: Vector3f get() = Vector3f(r03, r13, r23)
/**
* Creates a copy of this matrix as [Matrix4d],
* casting all [Float]s to [Double]s
*/
fun toMatrix4d(): Matrix4d {
return Matrix4d(
c00.toDouble(), c01.toDouble(), c02.toDouble(), c03.toDouble(),
c10.toDouble(), c11.toDouble(), c12.toDouble(), c13.toDouble(),
c20.toDouble(), c21.toDouble(), c22.toDouble(), c23.toDouble(),
c30.toDouble(), c31.toDouble(), c32.toDouble(), c33.toDouble(),
)
}
/**
* Creates a copy of this matrix as [Matrix3d],
* truncating last row and column,
* casting all [Float]s to [Double]s
*/
fun toMatrix3d(): Matrix3d {
return Matrix3d(
c00.toDouble(), c01.toDouble(), c02.toDouble(),
c10.toDouble(), c11.toDouble(), c12.toDouble(),
c20.toDouble(), c21.toDouble(), c22.toDouble(),
)
}
/**
* Creates a copy of this matrix as [Matrix2d],
* truncating two last rows and columns,
* casting all [Float]s to [Double]s
*/
fun toMatrix2d(): Matrix2d {
return Matrix2d(
c00.toDouble(), c01.toDouble(),
c10.toDouble(), c11.toDouble(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4d],
* casting all [Float]s to [Double]s
*/
fun toMutableMatrix4d(): MutableMatrix4d {
return MutableMatrix4d(
c00.toDouble(), c01.toDouble(), c02.toDouble(), c03.toDouble(),
c10.toDouble(), c11.toDouble(), c12.toDouble(), c13.toDouble(),
c20.toDouble(), c21.toDouble(), c22.toDouble(), c23.toDouble(),
c30.toDouble(), c31.toDouble(), c32.toDouble(), c33.toDouble(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3d],
* truncating last row and column
*/
fun toMutableMatrix3d(): MutableMatrix3d {
return MutableMatrix3d(
c00.toDouble(), c01.toDouble(), c02.toDouble(),
c10.toDouble(), c11.toDouble(), c12.toDouble(),
c20.toDouble(), c21.toDouble(), c22.toDouble(),
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2d],
* truncating two last rows and columns
*/
fun toMutableMatrix2d(): MutableMatrix2d {
return MutableMatrix2d(
c00.toDouble(), c01.toDouble(),
c10.toDouble(), c11.toDouble(),
)
}
/**
* Creates a copy of this matrix as [Matrix4f]
*/
fun toMatrix4f(): Matrix4f {
return Matrix4f(
c00, c01, c02, c03,
c10, c11, c12, c13,
c20, c21, c22, c23,
c30, c31, c32, c33,
)
}
/**
* Creates a copy of this matrix as [Matrix3f],
* truncating last row and column
*/
fun toMatrix3f(): Matrix3f {
return Matrix3f(
c00, c01, c02,
c10, c11, c12,
c20, c21, c22,
)
}
/**
* Creates a copy of this matrix as [Matrix2f],
* truncating two last rows and columns
*/
fun toMatrix2f(): Matrix2f {
return Matrix2f(
c00, c01,
c10, c11,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix4f]
*/
fun toMutableMatrix4f(): MutableMatrix4f {
return MutableMatrix4f(
c00, c01, c02, c03,
c10, c11, c12, c13,
c20, c21, c22, c23,
c30, c31, c32, c33,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix3f],
* truncating last row and column
*/
fun toMutableMatrix3f(): MutableMatrix3f {
return MutableMatrix3f(
c00, c01, c02,
c10, c11, c12,
c20, c21, c22,
)
}
/**
* Creates a copy of this matrix as [MutableMatrix2f],
* truncating two last rows and columns
*/
fun toMutableMatrix2f(): MutableMatrix2f {
return MutableMatrix2f(
c00, c01,
c10, c11,
)
}
}

170
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/Matrix4fStack.kt
View File

@ -1,170 +0,0 @@
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
}
}

710
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/functions.kt
View File

@ -1,710 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.matrix
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.matrix.generated.*
import ru.dbotthepony.kvector.matrix.generated.matrixConcreteDeterminant
import ru.dbotthepony.kvector.narray.*
import kotlin.math.pow
/**
* Multiplies matrices as long as they are compatible, of any size.
* Attempts to use concrete implementation first.
*
* @throws IllegalArgumentException if matrices are incompatible
*
* @return new [Double2Dimensional] buffer with multiplication result
*/
fun multiplyMatrix(matrix1: IMatrixGetterDouble, matrix2: IMatrixGetterDouble): Double2Dimensional {
require(matrix1.columns == matrix2.rows) { "Matrices are incompatible for multiplication (Matrix 1 has ${matrix1.columns} columns, Matrix 2 has ${matrix2.rows} rows)" }
val multiplied = MultiplicationsDouble.multiplyMatrix(matrix1, matrix2)
if (multiplied != null) {
return multiplied
}
val resultRows = matrix1.rows
val resultColumns = matrix2.columns
val vectorized = matrix1.columns
val output = Double2Dimensional(resultColumns, resultRows)
for (row in 0 until resultRows) {
for (column in 0 until resultColumns) {
var sum = 0.0
for (rowColumn in 0 until vectorized) {
sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
}
output[column, row] = sum
}
}
return output
}
/**
* Multiplies matrices as long as they are compatible, of any size.
* Attempts to use concrete implementation first.
*
* @throws IllegalArgumentException if matrices are incompatible
*
* @return new [Float2Dimensional] buffer with multiplication result
*/
fun multiplyMatrix(matrix1: IMatrixGetterFloat, matrix2: IMatrixGetterFloat): Float2Dimensional {
require(matrix1.columns == matrix2.rows) { "Matrices are incompatible for multiplication (Matrix 1 has ${matrix1.columns} columns, Matrix 2 has ${matrix2.rows} rows)" }
val multiplied = MultiplicationsFloat.multiplyMatrix(matrix1, matrix2)
if (multiplied != null) {
return multiplied
}
val resultRows = matrix1.rows
val resultColumns = matrix2.columns
val vectorized = matrix1.columns
val output = Float2Dimensional(resultColumns, resultRows)
for (row in 0 until resultRows) {
for (column in 0 until resultColumns) {
var sum = 0f
for (rowColumn in 0 until vectorized) {
sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
}
output[column, row] = sum
}
}
return output
}
/**
* Multiplies matrices as long as they are compatible, of any size.
* Attempts to use concrete implementation first.
*
* @throws IllegalArgumentException if matrices are incompatible
*
* @return new [Int2Dimensional] buffer with multiplication result
*/
fun multiplyMatrix(matrix1: IMatrixGetterInt, matrix2: IMatrixGetterInt): Int2Dimensional {
require(matrix1.columns == matrix2.rows) { "Matrices are incompatible for multiplication (Matrix 1 has ${matrix1.columns} columns, Matrix 2 has ${matrix2.rows} rows)" }
val multiplied = MultiplicationsInt.multiplyMatrix(matrix1, matrix2)
if (multiplied != null) {
return multiplied
}
val resultRows = matrix1.rows
val resultColumns = matrix2.columns
val vectorized = matrix1.columns
val output = Int2Dimensional(resultColumns, resultRows)
for (row in 0 until resultRows) {
for (column in 0 until resultColumns) {
var sum = 0
for (rowColumn in 0 until vectorized) {
sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
}
output[column, row] = sum
}
}
return output
}
/**
* Multiplies matrices as long as they are compatible, of any size.
* Attempts to use concrete implementation first.
*
* @throws IllegalArgumentException if matrices are incompatible
*
* @return new [Long2Dimensional] buffer with multiplication result
*/
fun multiplyMatrix(matrix1: IMatrixGetterLong, matrix2: IMatrixGetterLong): Long2Dimensional {
require(matrix1.columns == matrix2.rows) { "Matrices are incompatible for multiplication (Matrix 1 has ${matrix1.columns} columns, Matrix 2 has ${matrix2.rows} rows)" }
val multiplied = MultiplicationsLong.multiplyMatrix(matrix1, matrix2)
if (multiplied != null) {
return multiplied
}
val resultRows = matrix1.rows
val resultColumns = matrix2.columns
val vectorized = matrix1.columns
val output = Long2Dimensional(resultColumns, resultRows)
for (row in 0 until resultRows) {
for (column in 0 until resultColumns) {
var sum = 0L
for (rowColumn in 0 until vectorized) {
sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
}
output[column, row] = sum
}
}
return output
}
/**
* Multiplies matrices as long as they are compatible, of any size.
* Attempts to use concrete implementation first.
*
* @throws IllegalArgumentException if matrices are incompatible
*
* @return new [Short2Dimensional] buffer with multiplication result
*/
fun multiplyMatrix(matrix1: IMatrixGetterShort, matrix2: IMatrixGetterShort): Short2Dimensional {
require(matrix1.columns == matrix2.rows) { "Matrices are incompatible for multiplication (Matrix 1 has ${matrix1.columns} columns, Matrix 2 has ${matrix2.rows} rows)" }
val multiplied = MultiplicationsShort.multiplyMatrix(matrix1, matrix2)
if (multiplied != null) {
return multiplied
}
val resultRows = matrix1.rows
val resultColumns = matrix2.columns
val vectorized = matrix1.columns
val output = Short2Dimensional(resultColumns, resultRows)
for (row in 0 until resultRows) {
for (column in 0 until resultColumns) {
var sum = 0
for (rowColumn in 0 until vectorized) {
sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
}
output[column, row] = sum.toShort()
}
}
return output
}
/**
* Multiplies matrices as long as they are compatible, of any size.
* Attempts to use concrete implementation first.
*
* @throws IllegalArgumentException if matrices are incompatible
*
* @return new [Byte2Dimensional] buffer with multiplication result
*/
fun multiplyMatrix(matrix1: IMatrixGetterByte, matrix2: IMatrixGetterByte): Byte2Dimensional {
require(matrix1.columns == matrix2.rows) { "Matrices are incompatible for multiplication (Matrix 1 has ${matrix1.columns} columns, Matrix 2 has ${matrix2.rows} rows)" }
val multiplied = MultiplicationsByte.multiplyMatrix(matrix1, matrix2)
if (multiplied != null) {
return multiplied
}
val resultRows = matrix1.rows
val resultColumns = matrix2.columns
val vectorized = matrix1.columns
val output = Byte2Dimensional(resultColumns, resultRows)
for (row in 0 until resultRows) {
for (column in 0 until resultColumns) {
var sum = 0L
for (rowColumn in 0 until vectorized) {
sum += matrix1[rowColumn, row] * matrix2[column, rowColumn]
}
output[column, row] = sum.toByte()
}
}
return output
}
/**
* Transposes structure representing matrix of any size.
*/
fun transposeMatrix(matrix: IMatrixGetterDouble): Double2Dimensional {
val output = Double2Dimensional(matrix.rows, matrix.columns)
for (column in 0 until matrix.columns) {
for (row in 0 until matrix.rows) {
output[row, column] = matrix[column, row]
}
}
return output
}
/**
* Transposes structure representing matrix of any size.
*/
fun transposeMatrix(matrix: IMatrixGetterFloat): Float2Dimensional {
val output = Float2Dimensional(matrix.rows, matrix.columns)
for (column in 0 until matrix.columns) {
for (row in 0 until matrix.rows) {
output[row, column] = matrix[column, row]
}
}
return output
}
private fun load(matrix: IMatrixGetterDouble, buffers: Array<Double2Dimensional>): Double2Dimensional {
val buffer = buffers[matrix.columns - 1]
for (column in 0 until matrix.columns) {
for (row in 0 until matrix.rows) {
buffer[column, row] = matrix[column, row]
}
}
return buffer
}
private fun load(matrix: IMatrixGetterFloat, buffers: Array<Float2Dimensional>): Float2Dimensional {
val buffer = buffers[matrix.columns - 1]
for (column in 0 until matrix.columns) {
for (row in 0 until matrix.rows) {
buffer[column, row] = matrix[column, row]
}
}
return buffer
}
private fun load(matrix: IMatrixGetterInt, buffers: Array<Int2Dimensional>): Int2Dimensional {
val buffer = buffers[matrix.columns - 1]
for (column in 0 until matrix.columns) {
for (row in 0 until matrix.rows) {
buffer[column, row] = matrix[column, row]
}
}
return buffer
}
private fun load(matrix: IMatrixGetterLong, buffers: Array<Long2Dimensional>): Long2Dimensional {
val buffer = buffers[matrix.columns - 1]
for (column in 0 until matrix.columns) {
for (row in 0 until matrix.rows) {
buffer[column, row] = matrix[column, row]
}
}
return buffer
}
/**
* Constructs new complement matrix from [input] representing square matrix,
* by taking out [column] and [row], and load result into [buffer].
*
* @throws IllegalArgumentException if [column] or [row] is out of bounds,
* or if [input] does not represent a square matrix
*
* @return [buffer]
*/
fun complementMatrix(input: IMatrixGetterDouble, column: Int, row: Int, buffer: Double2Dimensional): Double2Dimensional {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
require(column < input.columns) { "$column < ${input.columns}" }
require(row < input.rows) { "$row < ${input.rows}" }
var iRow = 0
var iColumn = 0
for (gColumn in 0 until input.columns) {
if (gColumn == column)
continue
for (gRow in 0 until input.rows) {
if (gRow == row)
continue
buffer[iColumn, iRow++] = input[gColumn, gRow]
}
iColumn++
iRow = 0
}
return buffer
}
/**
* Constructs new complement matrix from [input] representing square matrix,
* by taking out [column] and [row], and load result into [buffer].
*
* @throws IllegalArgumentException if [column] or [row] is out of bounds,
* or if [input] does not represent a square matrix
*
* @return [buffer]
*/
fun complementMatrix(input: IMatrixGetterFloat, column: Int, row: Int, buffer: Float2Dimensional): Float2Dimensional {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
require(column < input.columns) { "$column < ${input.columns}" }
require(row < input.rows) { "$row < ${input.rows}" }
var iRow = 0
var iColumn = 0
for (gColumn in 0 until input.columns) {
if (gColumn == column)
continue
for (gRow in 0 until input.rows) {
if (gRow == row)
continue
buffer[iColumn, iRow++] = input[gColumn, gRow]
}
iColumn++
iRow = 0
}
return buffer
}
/**
* Constructs new complement matrix from [input] representing square matrix,
* by taking out [column] and [row], and load result into [buffer].
*
* @throws IllegalArgumentException if [column] or [row] is out of bounds,
* or if [input] does not represent a square matrix
*
* @return [buffer]
*/
fun complementMatrix(input: IMatrixGetterInt, column: Int, row: Int, buffer: Int2Dimensional): Int2Dimensional {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
require(column < input.columns) { "$column < ${input.columns}" }
require(row < input.rows) { "$row < ${input.rows}" }
var iRow = 0
var iColumn = 0
for (gColumn in 0 until input.columns) {
if (gColumn == column)
continue
for (gRow in 0 until input.rows) {
if (gRow == row)
continue
buffer[iColumn, iRow++] = input[gColumn, gRow]
}
iColumn++
iRow = 0
}
return buffer
}
/**
* Constructs new complement matrix from [input] representing square matrix,
* by taking out [column] and [row], and load result into [buffer].
*
* @throws IllegalArgumentException if [column] or [row] is out of bounds,
* or if [input] does not represent a square matrix
*
* @return [buffer]
*/
fun complementMatrix(input: IMatrixGetterLong, column: Int, row: Int, buffer: Long2Dimensional): Long2Dimensional {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
require(column < input.columns) { "$column < ${input.columns}" }
require(row < input.rows) { "$row < ${input.rows}" }
var iRow = 0
var iColumn = 0
for (gColumn in 0 until input.columns) {
if (gColumn == column)
continue
for (gRow in 0 until input.rows) {
if (gRow == row)
continue
buffer[iColumn, iRow++] = input[gColumn, gRow]
}
iColumn++
iRow = 0
}
return buffer
}
/**
* Computes cofactor matrix of specified square matrix [input],
* returning result as [Double2Dimensional]
*
* This operation requires **O(n!)** determinant calculations of matrix,
* so do cache result of this function in performance critical context,
* because it is calculated analytically.
*
* @throws IllegalArgumentException if provided [input] is not a square matrix
* @return 2d array containing result matrix
*/
fun cofactorMatrix(input: IMatrixGetterDouble): Double2Dimensional {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
if (input.rows == 1) {
return Double2Dimensional(1, 1).also { it[0, 0] = input[0, 0] }
} else if (input.rows == 2) {
return Double2Dimensional(2, 2).also {
it[0, 0] = input[1, 1]
it[1, 0] = -input[1, 0]
it[0, 1] = -input[0, 1]
it[1, 1] = input[0, 0]
}
}
val result = Double2Dimensional(input.columns, input.rows)
val buffers = Array(input.rows) { Double2Dimensional(it + 1, it + 1) }
for (column in 0 until input.columns) {
for (row in 0 until input.rows) {
val minor = matrixDeterminant(complementMatrix(input, column, row, buffers[input.columns - 2]), input.columns - 1, buffers)
val cofactor = (-1.0).pow(row + column) * minor
result[column, row] = cofactor
}
}
return result
}
/**
* Computes cofactor matrix of specified square matrix [input],
* returning result as [Float2Dimensional]
*
* This operation requires **O(n!)** determinant calculations of matrix,
* so do cache result of this function in performance critical context,
* because it is calculated analytically.
*
* @throws IllegalArgumentException if provided [input] is not a square matrix
* @return 2d array containing result matrix
*/
fun cofactorMatrix(input: IMatrixGetterFloat): Float2Dimensional {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
if (input.rows == 1) {
return Float2Dimensional(1, 1).also { it[0, 0] = input[0, 0] }
} else if (input.rows == 2) {
return Float2Dimensional(2, 2).also {
it[0, 0] = input[1, 1]
it[1, 0] = -input[1, 0]
it[0, 1] = -input[0, 1]
it[1, 1] = input[0, 0]
}
}
val result = Float2Dimensional(input.columns, input.rows)
val buffers = Array(input.rows) { Float2Dimensional(it + 1, it + 1) }
for (column in 0 until input.columns) {
for (row in 0 until input.rows) {
val minor = matrixDeterminant(complementMatrix(input, column, row, buffers[input.columns - 2]), input.columns - 1, buffers)
val cofactor = (-1f).pow(row + column) * minor
result[column, row] = cofactor
}
}
return result
}
/**
* Computes adjugate matrix of specified square matrix [input],
* returning result as [Double2Dimensional],
*
* [https://en.wikipedia.org/wiki/Adjugate_matrix]
*
* This operation requires n * n determinant calculations of matrix,
* so do cache result of this function in performance critical context.
*
* @throws IllegalArgumentException if provided [input] is not a square matrix
* @return 2d array containing result matrix
*/
fun adjugateMatrix(input: IMatrixGetterDouble): Double2Dimensional {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
return transposeMatrix(cofactorMatrix(input))
}
/**
* Computes adjugate matrix of specified square matrix [input],
* returning result as [Float2Dimensional],
*
* [https://en.wikipedia.org/wiki/Adjugate_matrix]
*
* This operation requires n * n determinant calculations of matrix,
* so do cache result of this function in performance critical context.
*
* @throws IllegalArgumentException if provided [input] is not a square matrix
* @return 2d array containing result matrix
*/
fun adjugateMatrix(input: IMatrixGetterFloat): Float2Dimensional {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
return transposeMatrix(cofactorMatrix(input))
}
/**
* Computes inverse matrix of specified square matrix [input],
* returning result as [Double2Dimensional],
*
* This operation requires **O(n! + 1)** determinant calculations of original matrix,
* so do cache result of this function in performance critical context,
* because it is calculated analytically.
*
* If determinant is zero, null is returned.
*
* @throws IllegalArgumentException if provided [input] is not a square matrix
* @return 2d array containing result matrix, or null if determinant is zero.
*/
fun inverseMatrix(input: IMatrixGetterDouble): Double2Dimensional? {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
var determinant = matrixDeterminant(input)
if (determinant == 0.0)
return null
determinant = 1.0 / determinant
val adjugate = adjugateMatrix(input)
for (column in 0 until input.columns) {
for (row in 0 until input.rows) {
adjugate[column, row] *= determinant
}
}
return adjugate
}
/**
* Computes inverse matrix of specified square matrix [input],
* returning result as [Float2Dimensional],
*
* This operation requires **O(n! + 1)** determinant calculations of original matrix,
* so do cache result of this function in performance critical context,
* because it is calculated analytically.
*
* If determinant is zero, null is returned.
*
* @throws IllegalArgumentException if provided [input] is not a square matrix
* @return 2d array containing result matrix, or null if determinant is zero.
*/
fun inverseMatrix(input: IMatrixGetterFloat): Float2Dimensional? {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
var determinant = matrixDeterminant(input)
if (determinant == 0f)
return null
determinant = 1f / determinant
val adjugate = adjugateMatrix(input)
for (column in 0 until input.columns) {
for (row in 0 until input.rows) {
adjugate[column, row] *= determinant
}
}
return adjugate
}
private fun matrixDeterminant(input: Double2Dimensional, size: Int, buffers: Array<Double2Dimensional>): Double {
if (input.rows == 1)
return input[0, 0]
val concrete = matrixConcreteDeterminant(input)
if (concrete != null) {
return concrete
}
var result = 0.0
for (column in 0 until size) {
val minor = matrixDeterminant(complementMatrix(input, column, 0, buffers[input.columns - 2]), size - 1, buffers)
val cofactor = (-1.0).pow(2 + column) * minor
result += input[column, 0] * cofactor
}
return result
}
/**
* Recursive determinant finder of specified structure representing square matrix.
*
* @throws IllegalArgumentException if [input] structure is not square matrix
*/
fun matrixDeterminant(input: IMatrixGetterDouble): Double {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
val concrete = matrixConcreteDeterminant(input)
if (concrete != null) {
return concrete
}
val buffers = Array(input.rows) { Double2Dimensional(it + 1, it + 1) }
return matrixDeterminant(load(input, buffers), input.columns, buffers)
}
private fun matrixDeterminant(input: Float2Dimensional, size: Int, buffers: Array<Float2Dimensional>): Float {
val concrete = matrixConcreteDeterminant(input)
if (concrete != null) {
return concrete
}
var result = 0f
for (column in 0 until size) {
val minor = matrixDeterminant(complementMatrix(input, column, 0, buffers[input.columns - 2]), size - 1, buffers)
val cofactor = (-1f).pow(2 + column) * minor
result += input[column, 0] * cofactor
}
return result
}
/**
* Recursive determinant finder of specified structure representing square matrix.
*
* @throws IllegalArgumentException if [input] structure is not square matrix
*/
fun matrixDeterminant(input: IMatrixGetterFloat): Float {
require(input.rows == input.columns) { "Provided matrix is not square matrix (${input.columns} columns, ${input.rows} rows)" }
val concrete = matrixConcreteDeterminant(input)
if (concrete != null) {
return concrete
}
val buffers = Array(input.rows) { Float2Dimensional(it + 1, it + 1) }
return matrixDeterminant(load(input, buffers), input.columns, buffers).toFloat()
}

94
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/generated/DeterminantsByte.kt
View File

@ -1,94 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.matrix.generated
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.narray.*
// Some metaprogramming
private fun determinant2(matrix: IMatrixGetterByte): Byte {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
return (c00*c11 - c01*c10).toByte()
}
private fun determinant3(matrix: IMatrixGetterByte): Byte {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
return (c00*c11*c22 - c00*c12*c21 - c01*c10*c22 + c01*c12*c20 + c02*c10*c21 - c02*c11*c20).toByte()
}
private fun determinant4(matrix: IMatrixGetterByte): Byte {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
return (c00*c11*c22*c33 - c00*c11*c23*c32 - c00*c12*c21*c33 + c00*c12*c23*c31 + c00*c13*c21*c32 - c00*c13*c22*c31 - c01*c10*c22*c33 + c01*c10*c23*c32 + c01*c12*c20*c33 - c01*c12*c23*c30 - c01*c13*c20*c32 + c01*c13*c22*c30 + c02*c10*c21*c33 - c02*c10*c23*c31 - c02*c11*c20*c33 + c02*c11*c23*c30 + c02*c13*c20*c31 - c02*c13*c21*c30 - c03*c10*c21*c32 + c03*c10*c22*c31 + c03*c11*c20*c32 - c03*c11*c22*c30 - c03*c12*c20*c31 + c03*c12*c21*c30).toByte()
}
private fun determinant5(matrix: IMatrixGetterByte): Byte {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c40 = matrix[4, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c41 = matrix[4, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c42 = matrix[4, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
val c43 = matrix[4, 3]
val c04 = matrix[0, 4]
val c14 = matrix[1, 4]
val c24 = matrix[2, 4]
val c34 = matrix[3, 4]
val c44 = matrix[4, 4]
return (c00*c11*c22*c33*c44 - c00*c11*c22*c34*c43 - c00*c11*c23*c32*c44 + c00*c11*c23*c34*c42 + c00*c11*c24*c32*c43 - c00*c11*c24*c33*c42 - c00*c12*c21*c33*c44 + c00*c12*c21*c34*c43 + c00*c12*c23*c31*c44 - c00*c12*c23*c34*c41 - c00*c12*c24*c31*c43 + c00*c12*c24*c33*c41 + c00*c13*c21*c32*c44 - c00*c13*c21*c34*c42 - c00*c13*c22*c31*c44 + c00*c13*c22*c34*c41 + c00*c13*c24*c31*c42 - c00*c13*c24*c32*c41 - c00*c14*c21*c32*c43 + c00*c14*c21*c33*c42 + c00*c14*c22*c31*c43 - c00*c14*c22*c33*c41 - c00*c14*c23*c31*c42 + c00*c14*c23*c32*c41 - c01*c10*c22*c33*c44 + c01*c10*c22*c34*c43 + c01*c10*c23*c32*c44 - c01*c10*c23*c34*c42 - c01*c10*c24*c32*c43 + c01*c10*c24*c33*c42 + c01*c12*c20*c33*c44 - c01*c12*c20*c34*c43 - c01*c12*c23*c30*c44 + c01*c12*c23*c34*c40 + c01*c12*c24*c30*c43 - c01*c12*c24*c33*c40 - c01*c13*c20*c32*c44 + c01*c13*c20*c34*c42 + c01*c13*c22*c30*c44 - c01*c13*c22*c34*c40 - c01*c13*c24*c30*c42 + c01*c13*c24*c32*c40 + c01*c14*c20*c32*c43 - c01*c14*c20*c33*c42 - c01*c14*c22*c30*c43 + c01*c14*c22*c33*c40 + c01*c14*c23*c30*c42 - c01*c14*c23*c32*c40 + c02*c10*c21*c33*c44 - c02*c10*c21*c34*c43 - c02*c10*c23*c31*c44 + c02*c10*c23*c34*c41 + c02*c10*c24*c31*c43 - c02*c10*c24*c33*c41 - c02*c11*c20*c33*c44 + c02*c11*c20*c34*c43 + c02*c11*c23*c30*c44 - c02*c11*c23*c34*c40 - c02*c11*c24*c30*c43 + c02*c11*c24*c33*c40 + c02*c13*c20*c31*c44 - c02*c13*c20*c34*c41 - c02*c13*c21*c30*c44 + c02*c13*c21*c34*c40 + c02*c13*c24*c30*c41 - c02*c13*c24*c31*c40 - c02*c14*c20*c31*c43 + c02*c14*c20*c33*c41 + c02*c14*c21*c30*c43 - c02*c14*c21*c33*c40 - c02*c14*c23*c30*c41 + c02*c14*c23*c31*c40 - c03*c10*c21*c32*c44 + c03*c10*c21*c34*c42 + c03*c10*c22*c31*c44 - c03*c10*c22*c34*c41 - c03*c10*c24*c31*c42 + c03*c10*c24*c32*c41 + c03*c11*c20*c32*c44 - c03*c11*c20*c34*c42 - c03*c11*c22*c30*c44 + c03*c11*c22*c34*c40 + c03*c11*c24*c30*c42 - c03*c11*c24*c32*c40 - c03*c12*c20*c31*c44 + c03*c12*c20*c34*c41 + c03*c12*c21*c30*c44 - c03*c12*c21*c34*c40 - c03*c12*c24*c30*c41 + c03*c12*c24*c31*c40 + c03*c14*c20*c31*c42 - c03*c14*c20*c32*c41 - c03*c14*c21*c30*c42 + c03*c14*c21*c32*c40 + c03*c14*c22*c30*c41 - c03*c14*c22*c31*c40 + c04*c10*c21*c32*c43 - c04*c10*c21*c33*c42 - c04*c10*c22*c31*c43 + c04*c10*c22*c33*c41 + c04*c10*c23*c31*c42 - c04*c10*c23*c32*c41 - c04*c11*c20*c32*c43 + c04*c11*c20*c33*c42 + c04*c11*c22*c30*c43 - c04*c11*c22*c33*c40 - c04*c11*c23*c30*c42 + c04*c11*c23*c32*c40 + c04*c12*c20*c31*c43 - c04*c12*c20*c33*c41 - c04*c12*c21*c30*c43 + c04*c12*c21*c33*c40 + c04*c12*c23*c30*c41 - c04*c12*c23*c31*c40 - c04*c13*c20*c31*c42 + c04*c13*c20*c32*c41 + c04*c13*c21*c30*c42 - c04*c13*c21*c32*c40 - c04*c13*c22*c30*c41 + c04*c13*c22*c31*c40).toByte()
}
/**
* Automatically generated concrete matrix determinant finder. If no mapping exist, this function returns null
*/
fun matrixConcreteDeterminant(matrix: IMatrixGetterByte): Byte? {
require(matrix.columns == matrix.rows) { "Provided matrix (${matrix.columns}x${matrix.rows}) is not a square matrix" }
return when (matrix.columns) {
2 -> determinant2(matrix)
3 -> determinant3(matrix)
4 -> determinant4(matrix)
5 -> determinant5(matrix)
else -> null
}
}

94
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/generated/DeterminantsDouble.kt
View File

@ -1,94 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.matrix.generated
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.narray.*
// Some metaprogramming
private fun determinant2(matrix: IMatrixGetterDouble): Double {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
return c00*c11 - c01*c10
}
private fun determinant3(matrix: IMatrixGetterDouble): Double {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
return c00*c11*c22 - c00*c12*c21 - c01*c10*c22 + c01*c12*c20 + c02*c10*c21 - c02*c11*c20
}
private fun determinant4(matrix: IMatrixGetterDouble): Double {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
return c00*c11*c22*c33 - c00*c11*c23*c32 - c00*c12*c21*c33 + c00*c12*c23*c31 + c00*c13*c21*c32 - c00*c13*c22*c31 - c01*c10*c22*c33 + c01*c10*c23*c32 + c01*c12*c20*c33 - c01*c12*c23*c30 - c01*c13*c20*c32 + c01*c13*c22*c30 + c02*c10*c21*c33 - c02*c10*c23*c31 - c02*c11*c20*c33 + c02*c11*c23*c30 + c02*c13*c20*c31 - c02*c13*c21*c30 - c03*c10*c21*c32 + c03*c10*c22*c31 + c03*c11*c20*c32 - c03*c11*c22*c30 - c03*c12*c20*c31 + c03*c12*c21*c30
}
private fun determinant5(matrix: IMatrixGetterDouble): Double {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c40 = matrix[4, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c41 = matrix[4, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c42 = matrix[4, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
val c43 = matrix[4, 3]
val c04 = matrix[0, 4]
val c14 = matrix[1, 4]
val c24 = matrix[2, 4]
val c34 = matrix[3, 4]
val c44 = matrix[4, 4]
return c00*c11*c22*c33*c44 - c00*c11*c22*c34*c43 - c00*c11*c23*c32*c44 + c00*c11*c23*c34*c42 + c00*c11*c24*c32*c43 - c00*c11*c24*c33*c42 - c00*c12*c21*c33*c44 + c00*c12*c21*c34*c43 + c00*c12*c23*c31*c44 - c00*c12*c23*c34*c41 - c00*c12*c24*c31*c43 + c00*c12*c24*c33*c41 + c00*c13*c21*c32*c44 - c00*c13*c21*c34*c42 - c00*c13*c22*c31*c44 + c00*c13*c22*c34*c41 + c00*c13*c24*c31*c42 - c00*c13*c24*c32*c41 - c00*c14*c21*c32*c43 + c00*c14*c21*c33*c42 + c00*c14*c22*c31*c43 - c00*c14*c22*c33*c41 - c00*c14*c23*c31*c42 + c00*c14*c23*c32*c41 - c01*c10*c22*c33*c44 + c01*c10*c22*c34*c43 + c01*c10*c23*c32*c44 - c01*c10*c23*c34*c42 - c01*c10*c24*c32*c43 + c01*c10*c24*c33*c42 + c01*c12*c20*c33*c44 - c01*c12*c20*c34*c43 - c01*c12*c23*c30*c44 + c01*c12*c23*c34*c40 + c01*c12*c24*c30*c43 - c01*c12*c24*c33*c40 - c01*c13*c20*c32*c44 + c01*c13*c20*c34*c42 + c01*c13*c22*c30*c44 - c01*c13*c22*c34*c40 - c01*c13*c24*c30*c42 + c01*c13*c24*c32*c40 + c01*c14*c20*c32*c43 - c01*c14*c20*c33*c42 - c01*c14*c22*c30*c43 + c01*c14*c22*c33*c40 + c01*c14*c23*c30*c42 - c01*c14*c23*c32*c40 + c02*c10*c21*c33*c44 - c02*c10*c21*c34*c43 - c02*c10*c23*c31*c44 + c02*c10*c23*c34*c41 + c02*c10*c24*c31*c43 - c02*c10*c24*c33*c41 - c02*c11*c20*c33*c44 + c02*c11*c20*c34*c43 + c02*c11*c23*c30*c44 - c02*c11*c23*c34*c40 - c02*c11*c24*c30*c43 + c02*c11*c24*c33*c40 + c02*c13*c20*c31*c44 - c02*c13*c20*c34*c41 - c02*c13*c21*c30*c44 + c02*c13*c21*c34*c40 + c02*c13*c24*c30*c41 - c02*c13*c24*c31*c40 - c02*c14*c20*c31*c43 + c02*c14*c20*c33*c41 + c02*c14*c21*c30*c43 - c02*c14*c21*c33*c40 - c02*c14*c23*c30*c41 + c02*c14*c23*c31*c40 - c03*c10*c21*c32*c44 + c03*c10*c21*c34*c42 + c03*c10*c22*c31*c44 - c03*c10*c22*c34*c41 - c03*c10*c24*c31*c42 + c03*c10*c24*c32*c41 + c03*c11*c20*c32*c44 - c03*c11*c20*c34*c42 - c03*c11*c22*c30*c44 + c03*c11*c22*c34*c40 + c03*c11*c24*c30*c42 - c03*c11*c24*c32*c40 - c03*c12*c20*c31*c44 + c03*c12*c20*c34*c41 + c03*c12*c21*c30*c44 - c03*c12*c21*c34*c40 - c03*c12*c24*c30*c41 + c03*c12*c24*c31*c40 + c03*c14*c20*c31*c42 - c03*c14*c20*c32*c41 - c03*c14*c21*c30*c42 + c03*c14*c21*c32*c40 + c03*c14*c22*c30*c41 - c03*c14*c22*c31*c40 + c04*c10*c21*c32*c43 - c04*c10*c21*c33*c42 - c04*c10*c22*c31*c43 + c04*c10*c22*c33*c41 + c04*c10*c23*c31*c42 - c04*c10*c23*c32*c41 - c04*c11*c20*c32*c43 + c04*c11*c20*c33*c42 + c04*c11*c22*c30*c43 - c04*c11*c22*c33*c40 - c04*c11*c23*c30*c42 + c04*c11*c23*c32*c40 + c04*c12*c20*c31*c43 - c04*c12*c20*c33*c41 - c04*c12*c21*c30*c43 + c04*c12*c21*c33*c40 + c04*c12*c23*c30*c41 - c04*c12*c23*c31*c40 - c04*c13*c20*c31*c42 + c04*c13*c20*c32*c41 + c04*c13*c21*c30*c42 - c04*c13*c21*c32*c40 - c04*c13*c22*c30*c41 + c04*c13*c22*c31*c40
}
/**
* Automatically generated concrete matrix determinant finder. If no mapping exist, this function returns null
*/
fun matrixConcreteDeterminant(matrix: IMatrixGetterDouble): Double? {
require(matrix.columns == matrix.rows) { "Provided matrix (${matrix.columns}x${matrix.rows}) is not a square matrix" }
return when (matrix.columns) {
2 -> determinant2(matrix)
3 -> determinant3(matrix)
4 -> determinant4(matrix)
5 -> determinant5(matrix)
else -> null
}
}

94
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/generated/DeterminantsFloat.kt
View File

@ -1,94 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.matrix.generated
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.narray.*
// Some metaprogramming
private fun determinant2(matrix: IMatrixGetterFloat): Float {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
return c00*c11 - c01*c10
}
private fun determinant3(matrix: IMatrixGetterFloat): Float {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
return c00*c11*c22 - c00*c12*c21 - c01*c10*c22 + c01*c12*c20 + c02*c10*c21 - c02*c11*c20
}
private fun determinant4(matrix: IMatrixGetterFloat): Float {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
return c00*c11*c22*c33 - c00*c11*c23*c32 - c00*c12*c21*c33 + c00*c12*c23*c31 + c00*c13*c21*c32 - c00*c13*c22*c31 - c01*c10*c22*c33 + c01*c10*c23*c32 + c01*c12*c20*c33 - c01*c12*c23*c30 - c01*c13*c20*c32 + c01*c13*c22*c30 + c02*c10*c21*c33 - c02*c10*c23*c31 - c02*c11*c20*c33 + c02*c11*c23*c30 + c02*c13*c20*c31 - c02*c13*c21*c30 - c03*c10*c21*c32 + c03*c10*c22*c31 + c03*c11*c20*c32 - c03*c11*c22*c30 - c03*c12*c20*c31 + c03*c12*c21*c30
}
private fun determinant5(matrix: IMatrixGetterFloat): Float {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c40 = matrix[4, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c41 = matrix[4, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c42 = matrix[4, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
val c43 = matrix[4, 3]
val c04 = matrix[0, 4]
val c14 = matrix[1, 4]
val c24 = matrix[2, 4]
val c34 = matrix[3, 4]
val c44 = matrix[4, 4]
return c00*c11*c22*c33*c44 - c00*c11*c22*c34*c43 - c00*c11*c23*c32*c44 + c00*c11*c23*c34*c42 + c00*c11*c24*c32*c43 - c00*c11*c24*c33*c42 - c00*c12*c21*c33*c44 + c00*c12*c21*c34*c43 + c00*c12*c23*c31*c44 - c00*c12*c23*c34*c41 - c00*c12*c24*c31*c43 + c00*c12*c24*c33*c41 + c00*c13*c21*c32*c44 - c00*c13*c21*c34*c42 - c00*c13*c22*c31*c44 + c00*c13*c22*c34*c41 + c00*c13*c24*c31*c42 - c00*c13*c24*c32*c41 - c00*c14*c21*c32*c43 + c00*c14*c21*c33*c42 + c00*c14*c22*c31*c43 - c00*c14*c22*c33*c41 - c00*c14*c23*c31*c42 + c00*c14*c23*c32*c41 - c01*c10*c22*c33*c44 + c01*c10*c22*c34*c43 + c01*c10*c23*c32*c44 - c01*c10*c23*c34*c42 - c01*c10*c24*c32*c43 + c01*c10*c24*c33*c42 + c01*c12*c20*c33*c44 - c01*c12*c20*c34*c43 - c01*c12*c23*c30*c44 + c01*c12*c23*c34*c40 + c01*c12*c24*c30*c43 - c01*c12*c24*c33*c40 - c01*c13*c20*c32*c44 + c01*c13*c20*c34*c42 + c01*c13*c22*c30*c44 - c01*c13*c22*c34*c40 - c01*c13*c24*c30*c42 + c01*c13*c24*c32*c40 + c01*c14*c20*c32*c43 - c01*c14*c20*c33*c42 - c01*c14*c22*c30*c43 + c01*c14*c22*c33*c40 + c01*c14*c23*c30*c42 - c01*c14*c23*c32*c40 + c02*c10*c21*c33*c44 - c02*c10*c21*c34*c43 - c02*c10*c23*c31*c44 + c02*c10*c23*c34*c41 + c02*c10*c24*c31*c43 - c02*c10*c24*c33*c41 - c02*c11*c20*c33*c44 + c02*c11*c20*c34*c43 + c02*c11*c23*c30*c44 - c02*c11*c23*c34*c40 - c02*c11*c24*c30*c43 + c02*c11*c24*c33*c40 + c02*c13*c20*c31*c44 - c02*c13*c20*c34*c41 - c02*c13*c21*c30*c44 + c02*c13*c21*c34*c40 + c02*c13*c24*c30*c41 - c02*c13*c24*c31*c40 - c02*c14*c20*c31*c43 + c02*c14*c20*c33*c41 + c02*c14*c21*c30*c43 - c02*c14*c21*c33*c40 - c02*c14*c23*c30*c41 + c02*c14*c23*c31*c40 - c03*c10*c21*c32*c44 + c03*c10*c21*c34*c42 + c03*c10*c22*c31*c44 - c03*c10*c22*c34*c41 - c03*c10*c24*c31*c42 + c03*c10*c24*c32*c41 + c03*c11*c20*c32*c44 - c03*c11*c20*c34*c42 - c03*c11*c22*c30*c44 + c03*c11*c22*c34*c40 + c03*c11*c24*c30*c42 - c03*c11*c24*c32*c40 - c03*c12*c20*c31*c44 + c03*c12*c20*c34*c41 + c03*c12*c21*c30*c44 - c03*c12*c21*c34*c40 - c03*c12*c24*c30*c41 + c03*c12*c24*c31*c40 + c03*c14*c20*c31*c42 - c03*c14*c20*c32*c41 - c03*c14*c21*c30*c42 + c03*c14*c21*c32*c40 + c03*c14*c22*c30*c41 - c03*c14*c22*c31*c40 + c04*c10*c21*c32*c43 - c04*c10*c21*c33*c42 - c04*c10*c22*c31*c43 + c04*c10*c22*c33*c41 + c04*c10*c23*c31*c42 - c04*c10*c23*c32*c41 - c04*c11*c20*c32*c43 + c04*c11*c20*c33*c42 + c04*c11*c22*c30*c43 - c04*c11*c22*c33*c40 - c04*c11*c23*c30*c42 + c04*c11*c23*c32*c40 + c04*c12*c20*c31*c43 - c04*c12*c20*c33*c41 - c04*c12*c21*c30*c43 + c04*c12*c21*c33*c40 + c04*c12*c23*c30*c41 - c04*c12*c23*c31*c40 - c04*c13*c20*c31*c42 + c04*c13*c20*c32*c41 + c04*c13*c21*c30*c42 - c04*c13*c21*c32*c40 - c04*c13*c22*c30*c41 + c04*c13*c22*c31*c40
}
/**
* Automatically generated concrete matrix determinant finder. If no mapping exist, this function returns null
*/
fun matrixConcreteDeterminant(matrix: IMatrixGetterFloat): Float? {
require(matrix.columns == matrix.rows) { "Provided matrix (${matrix.columns}x${matrix.rows}) is not a square matrix" }
return when (matrix.columns) {
2 -> determinant2(matrix)
3 -> determinant3(matrix)
4 -> determinant4(matrix)
5 -> determinant5(matrix)
else -> null
}
}

94
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/generated/DeterminantsInt.kt
View File

@ -1,94 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.matrix.generated
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.narray.*
// Some metaprogramming
private fun determinant2(matrix: IMatrixGetterInt): Int {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
return c00*c11 - c01*c10
}
private fun determinant3(matrix: IMatrixGetterInt): Int {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
return c00*c11*c22 - c00*c12*c21 - c01*c10*c22 + c01*c12*c20 + c02*c10*c21 - c02*c11*c20
}
private fun determinant4(matrix: IMatrixGetterInt): Int {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
return c00*c11*c22*c33 - c00*c11*c23*c32 - c00*c12*c21*c33 + c00*c12*c23*c31 + c00*c13*c21*c32 - c00*c13*c22*c31 - c01*c10*c22*c33 + c01*c10*c23*c32 + c01*c12*c20*c33 - c01*c12*c23*c30 - c01*c13*c20*c32 + c01*c13*c22*c30 + c02*c10*c21*c33 - c02*c10*c23*c31 - c02*c11*c20*c33 + c02*c11*c23*c30 + c02*c13*c20*c31 - c02*c13*c21*c30 - c03*c10*c21*c32 + c03*c10*c22*c31 + c03*c11*c20*c32 - c03*c11*c22*c30 - c03*c12*c20*c31 + c03*c12*c21*c30
}
private fun determinant5(matrix: IMatrixGetterInt): Int {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c40 = matrix[4, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c41 = matrix[4, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c42 = matrix[4, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
val c43 = matrix[4, 3]
val c04 = matrix[0, 4]
val c14 = matrix[1, 4]
val c24 = matrix[2, 4]
val c34 = matrix[3, 4]
val c44 = matrix[4, 4]
return c00*c11*c22*c33*c44 - c00*c11*c22*c34*c43 - c00*c11*c23*c32*c44 + c00*c11*c23*c34*c42 + c00*c11*c24*c32*c43 - c00*c11*c24*c33*c42 - c00*c12*c21*c33*c44 + c00*c12*c21*c34*c43 + c00*c12*c23*c31*c44 - c00*c12*c23*c34*c41 - c00*c12*c24*c31*c43 + c00*c12*c24*c33*c41 + c00*c13*c21*c32*c44 - c00*c13*c21*c34*c42 - c00*c13*c22*c31*c44 + c00*c13*c22*c34*c41 + c00*c13*c24*c31*c42 - c00*c13*c24*c32*c41 - c00*c14*c21*c32*c43 + c00*c14*c21*c33*c42 + c00*c14*c22*c31*c43 - c00*c14*c22*c33*c41 - c00*c14*c23*c31*c42 + c00*c14*c23*c32*c41 - c01*c10*c22*c33*c44 + c01*c10*c22*c34*c43 + c01*c10*c23*c32*c44 - c01*c10*c23*c34*c42 - c01*c10*c24*c32*c43 + c01*c10*c24*c33*c42 + c01*c12*c20*c33*c44 - c01*c12*c20*c34*c43 - c01*c12*c23*c30*c44 + c01*c12*c23*c34*c40 + c01*c12*c24*c30*c43 - c01*c12*c24*c33*c40 - c01*c13*c20*c32*c44 + c01*c13*c20*c34*c42 + c01*c13*c22*c30*c44 - c01*c13*c22*c34*c40 - c01*c13*c24*c30*c42 + c01*c13*c24*c32*c40 + c01*c14*c20*c32*c43 - c01*c14*c20*c33*c42 - c01*c14*c22*c30*c43 + c01*c14*c22*c33*c40 + c01*c14*c23*c30*c42 - c01*c14*c23*c32*c40 + c02*c10*c21*c33*c44 - c02*c10*c21*c34*c43 - c02*c10*c23*c31*c44 + c02*c10*c23*c34*c41 + c02*c10*c24*c31*c43 - c02*c10*c24*c33*c41 - c02*c11*c20*c33*c44 + c02*c11*c20*c34*c43 + c02*c11*c23*c30*c44 - c02*c11*c23*c34*c40 - c02*c11*c24*c30*c43 + c02*c11*c24*c33*c40 + c02*c13*c20*c31*c44 - c02*c13*c20*c34*c41 - c02*c13*c21*c30*c44 + c02*c13*c21*c34*c40 + c02*c13*c24*c30*c41 - c02*c13*c24*c31*c40 - c02*c14*c20*c31*c43 + c02*c14*c20*c33*c41 + c02*c14*c21*c30*c43 - c02*c14*c21*c33*c40 - c02*c14*c23*c30*c41 + c02*c14*c23*c31*c40 - c03*c10*c21*c32*c44 + c03*c10*c21*c34*c42 + c03*c10*c22*c31*c44 - c03*c10*c22*c34*c41 - c03*c10*c24*c31*c42 + c03*c10*c24*c32*c41 + c03*c11*c20*c32*c44 - c03*c11*c20*c34*c42 - c03*c11*c22*c30*c44 + c03*c11*c22*c34*c40 + c03*c11*c24*c30*c42 - c03*c11*c24*c32*c40 - c03*c12*c20*c31*c44 + c03*c12*c20*c34*c41 + c03*c12*c21*c30*c44 - c03*c12*c21*c34*c40 - c03*c12*c24*c30*c41 + c03*c12*c24*c31*c40 + c03*c14*c20*c31*c42 - c03*c14*c20*c32*c41 - c03*c14*c21*c30*c42 + c03*c14*c21*c32*c40 + c03*c14*c22*c30*c41 - c03*c14*c22*c31*c40 + c04*c10*c21*c32*c43 - c04*c10*c21*c33*c42 - c04*c10*c22*c31*c43 + c04*c10*c22*c33*c41 + c04*c10*c23*c31*c42 - c04*c10*c23*c32*c41 - c04*c11*c20*c32*c43 + c04*c11*c20*c33*c42 + c04*c11*c22*c30*c43 - c04*c11*c22*c33*c40 - c04*c11*c23*c30*c42 + c04*c11*c23*c32*c40 + c04*c12*c20*c31*c43 - c04*c12*c20*c33*c41 - c04*c12*c21*c30*c43 + c04*c12*c21*c33*c40 + c04*c12*c23*c30*c41 - c04*c12*c23*c31*c40 - c04*c13*c20*c31*c42 + c04*c13*c20*c32*c41 + c04*c13*c21*c30*c42 - c04*c13*c21*c32*c40 - c04*c13*c22*c30*c41 + c04*c13*c22*c31*c40
}
/**
* Automatically generated concrete matrix determinant finder. If no mapping exist, this function returns null
*/
fun matrixConcreteDeterminant(matrix: IMatrixGetterInt): Int? {
require(matrix.columns == matrix.rows) { "Provided matrix (${matrix.columns}x${matrix.rows}) is not a square matrix" }
return when (matrix.columns) {
2 -> determinant2(matrix)
3 -> determinant3(matrix)
4 -> determinant4(matrix)
5 -> determinant5(matrix)
else -> null
}
}

94
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/generated/DeterminantsLong.kt
View File

@ -1,94 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.matrix.generated
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.narray.*
// Some metaprogramming
private fun determinant2(matrix: IMatrixGetterLong): Long {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
return c00*c11 - c01*c10
}
private fun determinant3(matrix: IMatrixGetterLong): Long {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
return c00*c11*c22 - c00*c12*c21 - c01*c10*c22 + c01*c12*c20 + c02*c10*c21 - c02*c11*c20
}
private fun determinant4(matrix: IMatrixGetterLong): Long {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
return c00*c11*c22*c33 - c00*c11*c23*c32 - c00*c12*c21*c33 + c00*c12*c23*c31 + c00*c13*c21*c32 - c00*c13*c22*c31 - c01*c10*c22*c33 + c01*c10*c23*c32 + c01*c12*c20*c33 - c01*c12*c23*c30 - c01*c13*c20*c32 + c01*c13*c22*c30 + c02*c10*c21*c33 - c02*c10*c23*c31 - c02*c11*c20*c33 + c02*c11*c23*c30 + c02*c13*c20*c31 - c02*c13*c21*c30 - c03*c10*c21*c32 + c03*c10*c22*c31 + c03*c11*c20*c32 - c03*c11*c22*c30 - c03*c12*c20*c31 + c03*c12*c21*c30
}
private fun determinant5(matrix: IMatrixGetterLong): Long {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c40 = matrix[4, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c41 = matrix[4, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c42 = matrix[4, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
val c43 = matrix[4, 3]
val c04 = matrix[0, 4]
val c14 = matrix[1, 4]
val c24 = matrix[2, 4]
val c34 = matrix[3, 4]
val c44 = matrix[4, 4]
return c00*c11*c22*c33*c44 - c00*c11*c22*c34*c43 - c00*c11*c23*c32*c44 + c00*c11*c23*c34*c42 + c00*c11*c24*c32*c43 - c00*c11*c24*c33*c42 - c00*c12*c21*c33*c44 + c00*c12*c21*c34*c43 + c00*c12*c23*c31*c44 - c00*c12*c23*c34*c41 - c00*c12*c24*c31*c43 + c00*c12*c24*c33*c41 + c00*c13*c21*c32*c44 - c00*c13*c21*c34*c42 - c00*c13*c22*c31*c44 + c00*c13*c22*c34*c41 + c00*c13*c24*c31*c42 - c00*c13*c24*c32*c41 - c00*c14*c21*c32*c43 + c00*c14*c21*c33*c42 + c00*c14*c22*c31*c43 - c00*c14*c22*c33*c41 - c00*c14*c23*c31*c42 + c00*c14*c23*c32*c41 - c01*c10*c22*c33*c44 + c01*c10*c22*c34*c43 + c01*c10*c23*c32*c44 - c01*c10*c23*c34*c42 - c01*c10*c24*c32*c43 + c01*c10*c24*c33*c42 + c01*c12*c20*c33*c44 - c01*c12*c20*c34*c43 - c01*c12*c23*c30*c44 + c01*c12*c23*c34*c40 + c01*c12*c24*c30*c43 - c01*c12*c24*c33*c40 - c01*c13*c20*c32*c44 + c01*c13*c20*c34*c42 + c01*c13*c22*c30*c44 - c01*c13*c22*c34*c40 - c01*c13*c24*c30*c42 + c01*c13*c24*c32*c40 + c01*c14*c20*c32*c43 - c01*c14*c20*c33*c42 - c01*c14*c22*c30*c43 + c01*c14*c22*c33*c40 + c01*c14*c23*c30*c42 - c01*c14*c23*c32*c40 + c02*c10*c21*c33*c44 - c02*c10*c21*c34*c43 - c02*c10*c23*c31*c44 + c02*c10*c23*c34*c41 + c02*c10*c24*c31*c43 - c02*c10*c24*c33*c41 - c02*c11*c20*c33*c44 + c02*c11*c20*c34*c43 + c02*c11*c23*c30*c44 - c02*c11*c23*c34*c40 - c02*c11*c24*c30*c43 + c02*c11*c24*c33*c40 + c02*c13*c20*c31*c44 - c02*c13*c20*c34*c41 - c02*c13*c21*c30*c44 + c02*c13*c21*c34*c40 + c02*c13*c24*c30*c41 - c02*c13*c24*c31*c40 - c02*c14*c20*c31*c43 + c02*c14*c20*c33*c41 + c02*c14*c21*c30*c43 - c02*c14*c21*c33*c40 - c02*c14*c23*c30*c41 + c02*c14*c23*c31*c40 - c03*c10*c21*c32*c44 + c03*c10*c21*c34*c42 + c03*c10*c22*c31*c44 - c03*c10*c22*c34*c41 - c03*c10*c24*c31*c42 + c03*c10*c24*c32*c41 + c03*c11*c20*c32*c44 - c03*c11*c20*c34*c42 - c03*c11*c22*c30*c44 + c03*c11*c22*c34*c40 + c03*c11*c24*c30*c42 - c03*c11*c24*c32*c40 - c03*c12*c20*c31*c44 + c03*c12*c20*c34*c41 + c03*c12*c21*c30*c44 - c03*c12*c21*c34*c40 - c03*c12*c24*c30*c41 + c03*c12*c24*c31*c40 + c03*c14*c20*c31*c42 - c03*c14*c20*c32*c41 - c03*c14*c21*c30*c42 + c03*c14*c21*c32*c40 + c03*c14*c22*c30*c41 - c03*c14*c22*c31*c40 + c04*c10*c21*c32*c43 - c04*c10*c21*c33*c42 - c04*c10*c22*c31*c43 + c04*c10*c22*c33*c41 + c04*c10*c23*c31*c42 - c04*c10*c23*c32*c41 - c04*c11*c20*c32*c43 + c04*c11*c20*c33*c42 + c04*c11*c22*c30*c43 - c04*c11*c22*c33*c40 - c04*c11*c23*c30*c42 + c04*c11*c23*c32*c40 + c04*c12*c20*c31*c43 - c04*c12*c20*c33*c41 - c04*c12*c21*c30*c43 + c04*c12*c21*c33*c40 + c04*c12*c23*c30*c41 - c04*c12*c23*c31*c40 - c04*c13*c20*c31*c42 + c04*c13*c20*c32*c41 + c04*c13*c21*c30*c42 - c04*c13*c21*c32*c40 - c04*c13*c22*c30*c41 + c04*c13*c22*c31*c40
}
/**
* Automatically generated concrete matrix determinant finder. If no mapping exist, this function returns null
*/
fun matrixConcreteDeterminant(matrix: IMatrixGetterLong): Long? {
require(matrix.columns == matrix.rows) { "Provided matrix (${matrix.columns}x${matrix.rows}) is not a square matrix" }
return when (matrix.columns) {
2 -> determinant2(matrix)
3 -> determinant3(matrix)
4 -> determinant4(matrix)
5 -> determinant5(matrix)
else -> null
}
}

94
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/generated/DeterminantsShort.kt
View File

@ -1,94 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.matrix.generated
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.narray.*
// Some metaprogramming
private fun determinant2(matrix: IMatrixGetterShort): Short {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
return (c00*c11 - c01*c10).toShort()
}
private fun determinant3(matrix: IMatrixGetterShort): Short {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
return (c00*c11*c22 - c00*c12*c21 - c01*c10*c22 + c01*c12*c20 + c02*c10*c21 - c02*c11*c20).toShort()
}
private fun determinant4(matrix: IMatrixGetterShort): Short {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
return (c00*c11*c22*c33 - c00*c11*c23*c32 - c00*c12*c21*c33 + c00*c12*c23*c31 + c00*c13*c21*c32 - c00*c13*c22*c31 - c01*c10*c22*c33 + c01*c10*c23*c32 + c01*c12*c20*c33 - c01*c12*c23*c30 - c01*c13*c20*c32 + c01*c13*c22*c30 + c02*c10*c21*c33 - c02*c10*c23*c31 - c02*c11*c20*c33 + c02*c11*c23*c30 + c02*c13*c20*c31 - c02*c13*c21*c30 - c03*c10*c21*c32 + c03*c10*c22*c31 + c03*c11*c20*c32 - c03*c11*c22*c30 - c03*c12*c20*c31 + c03*c12*c21*c30).toShort()
}
private fun determinant5(matrix: IMatrixGetterShort): Short {
val c00 = matrix[0, 0]
val c10 = matrix[1, 0]
val c20 = matrix[2, 0]
val c30 = matrix[3, 0]
val c40 = matrix[4, 0]
val c01 = matrix[0, 1]
val c11 = matrix[1, 1]
val c21 = matrix[2, 1]
val c31 = matrix[3, 1]
val c41 = matrix[4, 1]
val c02 = matrix[0, 2]
val c12 = matrix[1, 2]
val c22 = matrix[2, 2]
val c32 = matrix[3, 2]
val c42 = matrix[4, 2]
val c03 = matrix[0, 3]
val c13 = matrix[1, 3]
val c23 = matrix[2, 3]
val c33 = matrix[3, 3]
val c43 = matrix[4, 3]
val c04 = matrix[0, 4]
val c14 = matrix[1, 4]
val c24 = matrix[2, 4]
val c34 = matrix[3, 4]
val c44 = matrix[4, 4]
return (c00*c11*c22*c33*c44 - c00*c11*c22*c34*c43 - c00*c11*c23*c32*c44 + c00*c11*c23*c34*c42 + c00*c11*c24*c32*c43 - c00*c11*c24*c33*c42 - c00*c12*c21*c33*c44 + c00*c12*c21*c34*c43 + c00*c12*c23*c31*c44 - c00*c12*c23*c34*c41 - c00*c12*c24*c31*c43 + c00*c12*c24*c33*c41 + c00*c13*c21*c32*c44 - c00*c13*c21*c34*c42 - c00*c13*c22*c31*c44 + c00*c13*c22*c34*c41 + c00*c13*c24*c31*c42 - c00*c13*c24*c32*c41 - c00*c14*c21*c32*c43 + c00*c14*c21*c33*c42 + c00*c14*c22*c31*c43 - c00*c14*c22*c33*c41 - c00*c14*c23*c31*c42 + c00*c14*c23*c32*c41 - c01*c10*c22*c33*c44 + c01*c10*c22*c34*c43 + c01*c10*c23*c32*c44 - c01*c10*c23*c34*c42 - c01*c10*c24*c32*c43 + c01*c10*c24*c33*c42 + c01*c12*c20*c33*c44 - c01*c12*c20*c34*c43 - c01*c12*c23*c30*c44 + c01*c12*c23*c34*c40 + c01*c12*c24*c30*c43 - c01*c12*c24*c33*c40 - c01*c13*c20*c32*c44 + c01*c13*c20*c34*c42 + c01*c13*c22*c30*c44 - c01*c13*c22*c34*c40 - c01*c13*c24*c30*c42 + c01*c13*c24*c32*c40 + c01*c14*c20*c32*c43 - c01*c14*c20*c33*c42 - c01*c14*c22*c30*c43 + c01*c14*c22*c33*c40 + c01*c14*c23*c30*c42 - c01*c14*c23*c32*c40 + c02*c10*c21*c33*c44 - c02*c10*c21*c34*c43 - c02*c10*c23*c31*c44 + c02*c10*c23*c34*c41 + c02*c10*c24*c31*c43 - c02*c10*c24*c33*c41 - c02*c11*c20*c33*c44 + c02*c11*c20*c34*c43 + c02*c11*c23*c30*c44 - c02*c11*c23*c34*c40 - c02*c11*c24*c30*c43 + c02*c11*c24*c33*c40 + c02*c13*c20*c31*c44 - c02*c13*c20*c34*c41 - c02*c13*c21*c30*c44 + c02*c13*c21*c34*c40 + c02*c13*c24*c30*c41 - c02*c13*c24*c31*c40 - c02*c14*c20*c31*c43 + c02*c14*c20*c33*c41 + c02*c14*c21*c30*c43 - c02*c14*c21*c33*c40 - c02*c14*c23*c30*c41 + c02*c14*c23*c31*c40 - c03*c10*c21*c32*c44 + c03*c10*c21*c34*c42 + c03*c10*c22*c31*c44 - c03*c10*c22*c34*c41 - c03*c10*c24*c31*c42 + c03*c10*c24*c32*c41 + c03*c11*c20*c32*c44 - c03*c11*c20*c34*c42 - c03*c11*c22*c30*c44 + c03*c11*c22*c34*c40 + c03*c11*c24*c30*c42 - c03*c11*c24*c32*c40 - c03*c12*c20*c31*c44 + c03*c12*c20*c34*c41 + c03*c12*c21*c30*c44 - c03*c12*c21*c34*c40 - c03*c12*c24*c30*c41 + c03*c12*c24*c31*c40 + c03*c14*c20*c31*c42 - c03*c14*c20*c32*c41 - c03*c14*c21*c30*c42 + c03*c14*c21*c32*c40 + c03*c14*c22*c30*c41 - c03*c14*c22*c31*c40 + c04*c10*c21*c32*c43 - c04*c10*c21*c33*c42 - c04*c10*c22*c31*c43 + c04*c10*c22*c33*c41 + c04*c10*c23*c31*c42 - c04*c10*c23*c32*c41 - c04*c11*c20*c32*c43 + c04*c11*c20*c33*c42 + c04*c11*c22*c30*c43 - c04*c11*c22*c33*c40 - c04*c11*c23*c30*c42 + c04*c11*c23*c32*c40 + c04*c12*c20*c31*c43 - c04*c12*c20*c33*c41 - c04*c12*c21*c30*c43 + c04*c12*c21*c33*c40 + c04*c12*c23*c30*c41 - c04*c12*c23*c31*c40 - c04*c13*c20*c31*c42 + c04*c13*c20*c32*c41 + c04*c13*c21*c30*c42 - c04*c13*c21*c32*c40 - c04*c13*c22*c30*c41 + c04*c13*c22*c31*c40).toShort()
}
/**
* Automatically generated concrete matrix determinant finder. If no mapping exist, this function returns null
*/
fun matrixConcreteDeterminant(matrix: IMatrixGetterShort): Short? {
require(matrix.columns == matrix.rows) { "Provided matrix (${matrix.columns}x${matrix.rows}) is not a square matrix" }
return when (matrix.columns) {
2 -> determinant2(matrix)
3 -> determinant3(matrix)
4 -> determinant4(matrix)
5 -> determinant5(matrix)
else -> null
}
}

228
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/ndouble/Matrix2d.kt
View File

@ -1,228 +0,0 @@
@file:Suppress("unused")
package ru.dbotthepony.kvector.matrix.ndouble
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.api.concrete.IMatrix2d
import ru.dbotthepony.kvector.narray.Double2Dimensional
import ru.dbotthepony.kvector.vector.ndouble.MutableVector2d
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
/**
* Represents immutable concrete matrix with 2x2 dimensions, storing values as [Double]s.
*
* See [Matrix4d] for documentation, with reflection of this class.
*
* Vectorized access use [Vector2d], generic interface is represented by [IMatrix2d].
*
* It does *not* implement [ISquareMatrix] since it doesn't make much sense.
*/
class Matrix2d : AbstractMatrixVd<Matrix2d>, IMatrix2d<Matrix2d> {
constructor(
c00: Double = 1.0, c01: Double = 0.0,
c10: Double = 0.0, c11: Double = 1.0,
) : super(2, 2, Double2Dimensional(2, 2).also {
it[0, 0] = c00
it[1, 0] = c10
it[0, 1] = c01
it[1, 1] = c11
})
private constructor(memory: Double2Dimensional) : super(2, 2, memory)
override val flexible: Boolean = false
override fun factorize(memory: Double2Dimensional): Matrix2d {
requireSizeEquals(memory)
return Matrix2d(memory)
}
override val c00: Double get() = memory[0, 0]
override val c10: Double get() = memory[1, 0]
override val c01: Double get() = memory[0, 1]
override val c11: Double get() = memory[1, 1]
override val r00 by this::c00
override val r01 by this::c10
override val r10 by this::c01
override val r11 by this::c11
override val a11 by this::c00
override val a21 by this::c01
override val a12 by this::c10
override val a22 by this::c11
override val b11 by this::c00
override val b21 by this::c10
override val b12 by this::c01
override val b22 by this::c11
override val c0: Vector2d get() = Vector2d(c00, c01)
override val c1: Vector2d get() = Vector2d(c10, c11)
override val r0: Vector2d get() = Vector2d(r00, r01)
override val r1: Vector2d get() = Vector2d(r10, r11)
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Double = 1.0, r01: Double = 0.0,
r10: Double = 0.0, r11: Double = 1.0,
): Matrix2d {
return Matrix2d(
c00 = r00, c10 = r01,
c01 = r10, c11 = r11,
)
}
/**
* Initializes new zero matrix.
*
* Since [Matrix2d] is immutable, it is encouraged you use [ZERO] property instead.
*/
fun zero(): Matrix2d {
return Matrix2d(
c00 = 0.0, c10 = 0.0,
c01 = 0.0, c11 = 0.0,
)
}
/**
* All values of this matrix are zero
*/
@JvmField val ZERO = zero()
/**
* Main diagonal of this matrix is 1s
*/
@JvmField val IDENTITY = Matrix2d()
}
}
/**
* Represents mutable concrete matrix with 2x2 dimensions, storing values as [Double]s.
*
* See [MutableMatrix4d] for documentation, with reflection of this class.
*
* Vectorized access use [MutableVector2d], generic interface is represented by [IMatrix2d].
*
* It does *not* implement [ISquareMatrix] since it doesn't make much sense.
*/
class MutableMatrix2d : AbstractMutableMatrixVd<MutableMatrix2d>, IMatrix2d<MutableMatrix2d> {
constructor(
c00: Double = 1.0, c01: Double = 0.0,
c10: Double = 0.0, c11: Double = 1.0,
) : super(2, 2, Double2Dimensional(2, 2).also {
it[0, 0] = c00
it[1, 0] = c10
it[0, 1] = c01
it[1, 1] = c11
})
private constructor(memory: Double2Dimensional) : super(2, 2, memory)
override val flexible: Boolean = false
override fun factorize(memory: Double2Dimensional): MutableMatrix2d {
requireSizeEquals(memory)
return MutableMatrix2d(memory)
}
override var c00: Double
get() = memory[0, 0]
set(value) { memory[0, 0] = value }
override var c10: Double
get() = memory[1, 0]
set(value) { memory[1, 0] = value }
override var c01: Double
get() = memory[0, 1]
set(value) { memory[0, 1] = value }
override var c11: Double
get() = memory[1, 1]
set(value) { memory[1, 1] = value }
override var r00 by this::c00
override var r01 by this::c10
override var r10 by this::c01
override var r11 by this::c11
override var a11 by this::c00
override var a21 by this::c01
override var a12 by this::c10
override var a22 by this::c11
override var b11 by this::c00
override var b21 by this::c10
override var b12 by this::c01
override var b22 by this::c11
override var c0: Vector2d
get() = object : MutableVector2d(c00, c01) {
override var x by this@MutableMatrix2d::c00
override var y by this@MutableMatrix2d::c01
}
set(value) {
c00 = value.x
c01 = value.y
}
override var c1: Vector2d
get() = object : MutableVector2d(c10, c11) {
override var x by this@MutableMatrix2d::c10
override var y by this@MutableMatrix2d::c11
}
set(value) {
c10 = value.x
c11 = value.y
}
override var r0: Vector2d
get() = object : MutableVector2d(r00, r01) {
override var x by this@MutableMatrix2d::r00
override var y by this@MutableMatrix2d::r01
}
set(value) {
r00 = value.x
r01 = value.y
}
override var r1: Vector2d
get() = object : MutableVector2d(r10, r11) {
override var x by this@MutableMatrix2d::r10
override var y by this@MutableMatrix2d::r11
}
set(value) {
r10 = value.x
r11 = value.y
}
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Double = 1.0, r01: Double = 0.0,
r10: Double = 0.0, r11: Double = 1.0,
): MutableMatrix2d {
return MutableMatrix2d(
c00 = r00, c10 = r01,
c01 = r10, c11 = r11,
)
}
/**
* Initializes new zero matrix.
*/
fun zero(): MutableMatrix2d {
return MutableMatrix2d(
c00 = 0.0, c10 = 0.0,
c01 = 0.0, c11 = 0.0,
)
}
}
}

400
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/ndouble/Matrix3d.kt
View File

@ -1,400 +0,0 @@
@file:Suppress("unused")
package ru.dbotthepony.kvector.matrix.ndouble
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.api.concrete.IMatrix3d
import ru.dbotthepony.kvector.narray.Double2Dimensional
import ru.dbotthepony.kvector.vector.ndouble.*
/**
* Represents immutable concrete matrix with 3x3 dimensions, storing values as [Double]s.
*
* See [Matrix4d] for documentation, with reflection of this class.
*
* Vectorized access use [Vector3d], generic interface is represented by [IMatrix3d].
*/
class Matrix3d : AbstractMatrixVd<Matrix3d>, IMatrix3d<Matrix3d> {
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,
c20: Double = 0.0, c21: Double = 0.0, c22: Double = 1.0,
) : super(3, 3, Double2Dimensional(3, 3).also {
it[0, 0] = c00
it[1, 0] = c10
it[2, 0] = c20
it[0, 1] = c01
it[1, 1] = c11
it[2, 1] = c21
it[0, 2] = c02
it[1, 2] = c12
it[2, 2] = c22
})
private constructor(memory: Double2Dimensional) : super(3, 3, memory)
override val flexible: Boolean = false
override fun factorize(memory: Double2Dimensional): Matrix3d {
requireSizeEquals(memory)
return Matrix3d(memory)
}
override val c00: Double get() = memory[0, 0]
override val c10: Double get() = memory[1, 0]
override val c20: Double get() = memory[2, 0]
override val c01: Double get() = memory[0, 1]
override val c11: Double get() = memory[1, 1]
override val c21: Double get() = memory[2, 1]
override val c02: Double get() = memory[0, 2]
override val c12: Double get() = memory[1, 2]
override val c22: Double get() = memory[2, 2]
override val r00 by this::c00
override val r01 by this::c10
override val r02 by this::c20
override val r10 by this::c01
override val r11 by this::c11
override val r12 by this::c21
override val r20 by this::c02
override val r21 by this::c12
override val r22 by this::c22
override val a11 by this::c00
override val a21 by this::c01
override val a31 by this::c02
override val a12 by this::c10
override val a22 by this::c11
override val a32 by this::c12
override val a13 by this::c20
override val a23 by this::c21
override val a33 by this::c22
override val b11 by this::c00
override val b21 by this::c10
override val b31 by this::c20
override val b12 by this::c01
override val b22 by this::c11
override val b32 by this::c21
override val b13 by this::c02
override val b23 by this::c12
override val b33 by this::c22
override val c0: Vector3d get() = Vector3d(c00, c01, c02)
override val c1: Vector3d get() = Vector3d(c10, c11, c12)
override val c2: Vector3d get() = Vector3d(c20, c21, c22)
override val r0: Vector3d get() = Vector3d(r00, r01, r02)
override val r1: Vector3d get() = Vector3d(r10, r11, r12)
override val r2: Vector3d get() = Vector3d(r20, r21, r22)
override fun translate(vector: Vector2d): Matrix3d {
return rm(
r00, r01, r02 + vector.x,
r10, r11, r12 + vector.y,
r20, r21, r22,
)
}
override fun translateWithMultiplication(vector: Vector2d): Matrix3d {
return rm(
r00, r01, r02 + vector.x * r00 + vector.y * r01,
r10, r11, r12 + vector.x * r10 + vector.y * r11,
r20, r21, r22,
)
}
override fun scale(vector: Vector3d): Matrix3d {
return rm(
r00 * vector.x, r01, r02,
r10, r11 * vector.y, r12,
r20, r21, r22 * vector.z
)
}
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Double = 1.0, r01: Double = 0.0, r02: Double = 0.0,
r10: Double = 0.0, r11: Double = 1.0, r12: Double = 0.0,
r20: Double = 0.0, r21: Double = 0.0, r22: Double = 1.0,
): Matrix3d {
return Matrix3d(
c00 = r00, c10 = r01, c20 = r02,
c01 = r10, c11 = r11, c21 = r12,
c02 = r20, c12 = r21, c22 = r22,
)
}
/**
* Initializes new zero matrix.
*
* Since [Matrix3d] is immutable, it is encouraged you use [ZERO] property instead.
*/
fun zero(): Matrix3d {
return Matrix3d(
c00 = 0.0, c10 = 0.0, c20 = 0.0,
c01 = 0.0, c11 = 0.0, c21 = 0.0,
c02 = 0.0, c12 = 0.0, c22 = 0.0,
)
}
/**
* All values of this matrix are zero
*/
@JvmField val ZERO = zero()
/**
* Main diagonal of this matrix is 1s
*/
@JvmField val IDENTITY = Matrix3d()
}
}
/**
* Represents mutable concrete matrix with 3x3 dimensions, storing values as [Double]s.
*
* See [MutableMatrix4d] for documentation, with reflection of this class.
*
* Vectorized access use [MutableVector3d], generic interface is represented by [IMatrix3d].
*/
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,
c20: Double = 0.0, c21: Double = 0.0, c22: Double = 1.0,
) : super(3, 3, Double2Dimensional(3, 3).also {
it[0, 0] = c00
it[1, 0] = c10
it[2, 0] = c20
it[0, 1] = c01
it[1, 1] = c11
it[2, 1] = c21
it[0, 2] = c02
it[1, 2] = c12
it[2, 2] = c22
})
private constructor(memory: Double2Dimensional) : super(3, 3, memory)
override val flexible: Boolean = false
override fun factorize(memory: Double2Dimensional): MutableMatrix3d {
requireSizeEquals(memory)
return MutableMatrix3d(memory)
}
override fun translateWithMultiplicationMut(vector: Vector2d): MutableMatrix3d {
r02 += vector.x * r00 + vector.y * r01
r12 += vector.x * r10 + vector.y * r11
return this
}
override fun scaleMut(vector: Vector3d): MutableMatrix3d {
r00 *= vector.x
r11 *= vector.y
r22 *= vector.z
return this
}
override var c00: Double
get() = memory[0, 0]
set(value) { memory[0, 0] = value }
override var c10: Double
get() = memory[1, 0]
set(value) { memory[1, 0] = value }
override var c20: Double
get() = memory[2, 0]
set(value) { memory[2, 0] = value }
override var c01: Double
get() = memory[0, 1]
set(value) { memory[0, 1] = value }
override var c11: Double
get() = memory[1, 1]
set(value) { memory[1, 1] = value }
override var c21: Double
get() = memory[2, 1]
set(value) { memory[2, 1] = value }
override var c02: Double
get() = memory[0, 2]
set(value) { memory[0, 2] = value }
override var c12: Double
get() = memory[1, 2]
set(value) { memory[1, 2] = value }
override var c22: Double
get() = memory[2, 2]
set(value) { memory[2, 2] = value }
override var r00 by this::c00
override var r01 by this::c10
override var r02 by this::c20
override var r10 by this::c01
override var r11 by this::c11
override var r12 by this::c21
override var r20 by this::c02
override var r21 by this::c12
override var r22 by this::c22
override var a11 by this::c00
override var a21 by this::c01
override var a31 by this::c02
override var a12 by this::c10
override var a22 by this::c11
override var a32 by this::c12
override var a13 by this::c20
override var a23 by this::c21
override var a33 by this::c22
override var b11 by this::c00
override var b21 by this::c10
override var b31 by this::c20
override var b12 by this::c01
override var b22 by this::c11
override var b32 by this::c21
override var b13 by this::c02
override var b23 by this::c12
override var b33 by this::c22
override var translation: Vector2d
get() = super.translation
set(value) {
r02 = value.x
r12 = value.y
}
override var c0: Vector3d
get() = object : MutableVector3d(c00, c01, c02) {
override var x by this@MutableMatrix3d::c00
override var y by this@MutableMatrix3d::c01
override var z by this@MutableMatrix3d::c02
}
set(value) {
c00 = value.x
c01 = value.y
c02 = value.z
}
override var c1: Vector3d
get() = object : MutableVector3d(c10, c11, c12) {
override var x by this@MutableMatrix3d::c10
override var y by this@MutableMatrix3d::c11
override var z by this@MutableMatrix3d::c12
}
set(value) {
c10 = value.x
c11 = value.y
c12 = value.z
}
override var c2: Vector3d
get() = object : MutableVector3d(c20, c21, c22) {
override var x by this@MutableMatrix3d::c20
override var y by this@MutableMatrix3d::c21
override var z by this@MutableMatrix3d::c22
}
set(value) {
c20 = value.x
c21 = value.y
c22 = value.z
}
override var r0: Vector3d
get() = object : MutableVector3d(r00, r01, r02) {
override var x by this@MutableMatrix3d::r00
override var y by this@MutableMatrix3d::r01
override var z by this@MutableMatrix3d::r02
}
set(value) {
r00 = value.x
r01 = value.y
r02 = value.z
}
override var r1: Vector3d
get() = object : MutableVector3d(r10, r11, r12) {
override var x by this@MutableMatrix3d::r10
override var y by this@MutableMatrix3d::r11
override var z by this@MutableMatrix3d::r12
}
set(value) {
r10 = value.x
r11 = value.y
r12 = value.z
}
override var r2: Vector3d
get() = object : MutableVector3d(r20, r21, r22) {
override var x by this@MutableMatrix3d::r20
override var y by this@MutableMatrix3d::r21
override var z by this@MutableMatrix3d::r22
}
set(value) {
r20 = value.x
r21 = value.y
r22 = value.z
}
override fun translate(vector: Vector2d): MutableMatrix3d {
return rm(
r00, r01, r02 + vector.x,
r10, r11, r12 + vector.y,
r20, r21, r22,
)
}
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,
r10, r11, r12 + vector.x * r10 + vector.y * r11,
r20, r21, r22,
)
}
override fun scale(vector: Vector3d): MutableMatrix3d {
return rm(
r00 * vector.x, r01, r02,
r10, r11 * vector.y, r12,
r20, r21, r22 * vector.z
)
}
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Double = 1.0, r01: Double = 0.0, r02: Double = 0.0,
r10: Double = 0.0, r11: Double = 1.0, r12: Double = 0.0,
r20: Double = 0.0, r21: Double = 0.0, r22: Double = 1.0,
): MutableMatrix3d {
return MutableMatrix3d(
c00 = r00, c10 = r01, c20 = r02,
c01 = r10, c11 = r11, c21 = r12,
c02 = r20, c12 = r21, c22 = r22,
)
}
/**
* Initializes new zero matrix.
*/
fun zero(): MutableMatrix3d {
return MutableMatrix3d(
c00 = 0.0, c10 = 0.0, c20 = 0.0,
c01 = 0.0, c11 = 0.0, c21 = 0.0,
c02 = 0.0, c12 = 0.0, c22 = 0.0,
)
}
}
}

577
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/ndouble/Matrix4d.kt
View File

@ -1,577 +0,0 @@
@file:Suppress("unused")
package ru.dbotthepony.kvector.matrix.ndouble
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.api.concrete.IMatrix4d
import ru.dbotthepony.kvector.matrix.ndouble.Matrix4d.Companion.ZERO
import ru.dbotthepony.kvector.matrix.ndouble.Matrix4d.Companion.rm
import ru.dbotthepony.kvector.matrix.ndouble.MutableMatrix4d.Companion.rm
import ru.dbotthepony.kvector.narray.Double2Dimensional
import ru.dbotthepony.kvector.vector.ndouble.MutableVector4d
import ru.dbotthepony.kvector.vector.ndouble.Vector3d
import ru.dbotthepony.kvector.vector.ndouble.Vector4d
/**
* Represents immutable concrete matrix with 4x4 dimensions, storing values as [Double]s.
*
* Primary constructor accept values in *Column-Major* order, that is, first row of values
* is first column in matrix. For *Row-Major* constructor, you should use [rm]
* companion object method.
*
* By default, primary constructor initialize identity matrix, e.g. main diagonal set to 1s.
*
* Matrices have multiple sets of properties to access values stored, for convenience of use:
* - rXY - access row X and column Y
* - cXY - access column X and row Y
* - aXY - access row X and column Y, but indices start from 1
* - bXY - access column X and row Y, but indices start from 1
* - cX - access column X as [Vector4d]
* - rX - access row X as [Vector4d]
*
* **If you don't care about mutability of matrix your method want to accept, use [IMatrix4d]**,
* because [MutableMatrix4d] is not a subtype of [Matrix4d], but they both are subtype of [IMatrix4d].
*/
class Matrix4d : AbstractMatrixVd<Matrix4d>, IMatrix4d<Matrix4d> {
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,
c20: Double = 0.0, c21: Double = 0.0, c22: Double = 1.0, c23: Double = 0.0,
c30: Double = 0.0, c31: Double = 0.0, c32: Double = 0.0, c33: Double = 1.0,
) : super(4, 4, Double2Dimensional(4, 4).also {
it[0, 0] = c00
it[1, 0] = c10
it[2, 0] = c20
it[3, 0] = c30
it[0, 1] = c01
it[1, 1] = c11
it[2, 1] = c21
it[3, 1] = c31
it[0, 2] = c02
it[1, 2] = c12
it[2, 2] = c22
it[3, 2] = c32
it[0, 3] = c03
it[1, 3] = c13
it[2, 3] = c23
it[3, 3] = c33
})
private constructor(memory: Double2Dimensional) : super(4, 4, memory)
override val flexible: Boolean = false
override val c00: Double get() = memory[0, 0]
override val c10: Double get() = memory[1, 0]
override val c20: Double get() = memory[2, 0]
override val c30: Double get() = memory[3, 0]
override val c01: Double get() = memory[0, 1]
override val c11: Double get() = memory[1, 1]
override val c21: Double get() = memory[2, 1]
override val c31: Double get() = memory[3, 1]
override val c02: Double get() = memory[0, 2]
override val c12: Double get() = memory[1, 2]
override val c22: Double get() = memory[2, 2]
override val c32: Double get() = memory[3, 2]
override val c03: Double get() = memory[0, 3]
override val c13: Double get() = memory[1, 3]
override val c23: Double get() = memory[2, 3]
override val c33: Double get() = memory[3, 3]
override val r00 by this::c00
override val r01 by this::c10
override val r02 by this::c20
override val r03 by this::c30
override val r10 by this::c01
override val r11 by this::c11
override val r12 by this::c21
override val r13 by this::c31
override val r20 by this::c02
override val r21 by this::c12
override val r22 by this::c22
override val r23 by this::c32
override val r30 by this::c03
override val r31 by this::c13
override val r32 by this::c23
override val r33 by this::c33
override val a11 by this::c00
override val a21 by this::c01
override val a31 by this::c02
override val a41 by this::c03
override val a12 by this::c10
override val a22 by this::c11
override val a32 by this::c12
override val a42 by this::c13
override val a13 by this::c20
override val a23 by this::c21
override val a33 by this::c22
override val a43 by this::c23
override val a14 by this::c30
override val a24 by this::c31
override val a34 by this::c32
override val a44 by this::c33
override val b11 by this::c00
override val b21 by this::c10
override val b31 by this::c20
override val b41 by this::c30
override val b12 by this::c01
override val b22 by this::c11
override val b32 by this::c21
override val b42 by this::c31
override val b13 by this::c02
override val b23 by this::c12
override val b33 by this::c22
override val b43 by this::c32
override val b14 by this::c03
override val b24 by this::c13
override val b34 by this::c23
override val b44 by this::c33
override val c0: Vector4d get() = Vector4d(c00, c01, c02, c03)
override val c1: Vector4d get() = Vector4d(c10, c11, c12, c13)
override val c2: Vector4d get() = Vector4d(c20, c21, c22, c23)
override val c3: Vector4d get() = Vector4d(c30, c31, c32, c33)
override val r0: Vector4d get() = Vector4d(r00, r01, r02, r03)
override val r1: Vector4d get() = Vector4d(r10, r11, r12, r13)
override val r2: Vector4d get() = Vector4d(r20, r21, r22, r23)
override val r3: Vector4d get() = Vector4d(r30, r31, r32, r33)
override fun factorize(memory: Double2Dimensional): Matrix4d {
requireSizeEquals(memory)
return Matrix4d(memory)
}
override fun translate(vector: Vector3d): Matrix4d {
return rm(
r00, r01, r02, r03 + vector.x,
r10, r11, r12, r13 + vector.y,
r20, r21, r22, r23 + vector.z,
r30, r31, r32, r33,
)
}
override fun translateWithMultiplication(vector: Vector3d): Matrix4d {
return rm(
r00, r01, r02, r03 + vector.x * r00 + vector.y * r01 + vector.z * r02,
r10, r11, r12, r13 + vector.x * r10 + vector.y * r11 + vector.z * r12,
r20, r21, r22, r23 + vector.x * r20 + vector.y * r21 + vector.z * r22,
r30, r31, r32, r33,
)
}
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 * vector.w,
)
}
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Double = 1.0, r01: Double = 0.0, r02: Double = 0.0, r03: Double = 0.0,
r10: Double = 0.0, r11: Double = 1.0, r12: Double = 0.0, r13: Double = 0.0,
r20: Double = 0.0, r21: Double = 0.0, r22: Double = 1.0, r23: Double = 0.0,
r30: Double = 0.0, r31: Double = 0.0, r32: Double = 0.0, r33: Double = 1.0,
): Matrix4d {
return Matrix4d(
c00 = r00, c10 = r01, c20 = r02, c30 = r03,
c01 = r10, c11 = r11, c21 = r12, c31 = r13,
c02 = r20, c12 = r21, c22 = r22, c32 = r23,
c03 = r30, c13 = r31, c23 = r32, c33 = r33,
)
}
/**
* Initializes new zero matrix.
*
* Since [Matrix4d] is immutable, it is encouraged you use [ZERO] property instead.
*/
fun zero(): Matrix4d {
return Matrix4d(
c00 = 0.0, c10 = 0.0, c20 = 0.0, c30 = 0.0,
c01 = 0.0, c11 = 0.0, c21 = 0.0, c31 = 0.0,
c02 = 0.0, c12 = 0.0, c22 = 0.0, c32 = 0.0,
c03 = 0.0, c13 = 0.0, c23 = 0.0, c33 = 0.0,
)
}
/**
* All values of this matrix are zero
*/
@JvmField val ZERO = zero()
/**
* Main diagonal of this matrix is 1s
*/
@JvmField val IDENTITY = Matrix4d()
}
}
/**
* Represents mutable concrete matrix with 4x4 dimensions, storing values as [Double]s
*
* Primary constructor accept values in *Column-Major* order, that is, first row of values
* is first column in matrix. For *Row-Major* constructor, you should use [rm]
* companion object method.
*
* Same notes about properties of [Matrix4d] apply.
*
* **This class is not a subclass of [Matrix4d]**, but a subclass of [IMatrix4d].
*
* Differences:
* - All properties are mutable
* - Vectorized access return [MutableVector4d], which actually reflect columns/rows of this matrix.
* That is, any changes done to matrix will be instantly reflected by vector, and any changes to vector
* will be reflected in matrix.
* - 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, 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,
c20: Double = 0.0, c21: Double = 0.0, c22: Double = 1.0, c23: Double = 0.0,
c30: Double = 0.0, c31: Double = 0.0, c32: Double = 0.0, c33: Double = 1.0,
) : super(4, 4, Double2Dimensional(4, 4).also {
it[0, 0] = c00
it[1, 0] = c10
it[2, 0] = c20
it[3, 0] = c30
it[0, 1] = c01
it[1, 1] = c11
it[2, 1] = c21
it[3, 1] = c31
it[0, 2] = c02
it[1, 2] = c12
it[2, 2] = c22
it[3, 2] = c32
it[0, 3] = c03
it[1, 3] = c13
it[2, 3] = c23
it[3, 3] = c33
})
private constructor(memory: Double2Dimensional) : super(4, 4, memory)
override val flexible: Boolean = false
override fun factorize(memory: Double2Dimensional): MutableMatrix4d {
requireSizeEquals(memory)
return MutableMatrix4d(memory)
}
override fun translate(vector: Vector3d): MutableMatrix4d {
return rm(
r00, r01, r02, r03 + vector.x,
r10, r11, r12, r13 + vector.y,
r20, r21, r22, r23 + vector.z,
r30, r31, r32, r33,
)
}
override fun translateWithMultiplication(vector: Vector3d): MutableMatrix4d {
return rm(
r00, r01, r02, r03 + vector.x * r00 + vector.y * r01 + vector.z * r02,
r10, r11, r12, r13 + vector.x * r10 + vector.y * r11 + vector.z * r12,
r20, r21, r22, r23 + vector.x * r20 + vector.y * r21 + vector.z * r22,
r30, r31, r32, r33,
)
}
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 * vector.w,
)
}
override fun translateWithMultiplicationMut(vector: Vector3d): MutableMatrix4d {
r03 += vector.x * r00 + vector.y * r01 + vector.z * r02
r13 += vector.x * r10 + vector.y * r11 + vector.z * r12
r23 += vector.x * r20 + vector.y * r21 + vector.z * r22
return this
}
override fun scaleMut(vector: Vector4d): MutableMatrix4d {
c00 *= vector.x
c11 *= vector.y
c22 *= vector.z
c33 *= vector.w
return this
}
override var translation: Vector3d
get() = super.translation
set(value) {
r03 = value.x
r13 = value.y
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 }
override var c10: Double
get() = memory[1, 0]
set(value) { memory[1, 0] = value }
override var c20: Double
get() = memory[2, 0]
set(value) { memory[2, 0] = value }
override var c30: Double
get() = memory[3, 0]
set(value) { memory[3, 0] = value }
override var c01: Double
get() = memory[0, 1]
set(value) { memory[0, 1] = value }
override var c11: Double
get() = memory[1, 1]
set(value) { memory[1, 1] = value }
override var c21: Double
get() = memory[2, 1]
set(value) { memory[2, 1] = value }
override var c31: Double
get() = memory[3, 1]
set(value) { memory[3, 1] = value }
override var c02: Double
get() = memory[0, 2]
set(value) { memory[0, 2] = value }
override var c12: Double
get() = memory[1, 2]
set(value) { memory[1, 2] = value }
override var c22: Double
get() = memory[2, 2]
set(value) { memory[2, 2] = value }
override var c32: Double
get() = memory[3, 2]
set(value) { memory[3, 2] = value }
override var c03: Double
get() = memory[0, 3]
set(value) { memory[0, 3] = value }
override var c13: Double
get() = memory[1, 3]
set(value) { memory[1, 3] = value }
override var c23: Double
get() = memory[2, 3]
set(value) { memory[2, 3] = value }
override var c33: Double
get() = memory[3, 3]
set(value) { memory[3, 3] = value }
override var r00 by this::c00
override var r01 by this::c10
override var r02 by this::c20
override var r03 by this::c30
override var r10 by this::c01
override var r11 by this::c11
override var r12 by this::c21
override var r13 by this::c31
override var r20 by this::c02
override var r21 by this::c12
override var r22 by this::c22
override var r23 by this::c32
override var r30 by this::c03
override var r31 by this::c13
override var r32 by this::c23
override var r33 by this::c33
override var a11 by this::c00
override var a21 by this::c01
override var a31 by this::c02
override var a41 by this::c03
override var a12 by this::c10
override var a22 by this::c11
override var a32 by this::c12
override var a42 by this::c13
override var a13 by this::c20
override var a23 by this::c21
override var a33 by this::c22
override var a43 by this::c23
override var a14 by this::c30
override var a24 by this::c31
override var a34 by this::c32
override var a44 by this::c33
override var b11 by this::c00
override var b21 by this::c01
override var b31 by this::c02
override var b41 by this::c03
override var b12 by this::c10
override var b22 by this::c11
override var b32 by this::c12
override var b42 by this::c13
override var b13 by this::c20
override var b23 by this::c21
override var b33 by this::c22
override var b43 by this::c23
override var b14 by this::c30
override var b24 by this::c31
override var b34 by this::c32
override var b44 by this::c33
override var c0: Vector4d
get() = object : MutableVector4d(c00, c01, c02, c03) {
override var x by this@MutableMatrix4d::c00
override var y by this@MutableMatrix4d::c01
override var z by this@MutableMatrix4d::c02
override var w by this@MutableMatrix4d::c03
}
set(value) {
c00 = value.x
c01 = value.y
c02 = value.z
c03 = value.w
}
override var c1: Vector4d
get() = object : MutableVector4d(c10, c11, c12, c13) {
override var x by this@MutableMatrix4d::c10
override var y by this@MutableMatrix4d::c11
override var z by this@MutableMatrix4d::c12
override var w by this@MutableMatrix4d::c13
}
set(value) {
c10 = value.x
c11 = value.y
c12 = value.z
c13 = value.w
}
override var c2: Vector4d
get() = object : MutableVector4d(c20, c21, c22, c23) {
override var x by this@MutableMatrix4d::c20
override var y by this@MutableMatrix4d::c21
override var z by this@MutableMatrix4d::c22
override var w by this@MutableMatrix4d::c23
}
set(value) {
c20 = value.x
c21 = value.y
c22 = value.z
c23 = value.w
}
override var c3: Vector4d
get() = object : MutableVector4d(c30, c31, c32, c33) {
override var x by this@MutableMatrix4d::c30
override var y by this@MutableMatrix4d::c31
override var z by this@MutableMatrix4d::c32
override var w by this@MutableMatrix4d::c33
}
set(value) {
c30 = value.x
c31 = value.y
c32 = value.z
c33 = value.w
}
override var r0: Vector4d
get() = object : MutableVector4d(r00, r01, r02, r03) {
override var x by this@MutableMatrix4d::r00
override var y by this@MutableMatrix4d::r01
override var z by this@MutableMatrix4d::r02
override var w by this@MutableMatrix4d::r03
}
set(value) {
r00 = value.x
r01 = value.y
r02 = value.z
r03 = value.w
}
override var r1: Vector4d
get() = object : MutableVector4d(r10, r11, r12, r13) {
override var x by this@MutableMatrix4d::r10
override var y by this@MutableMatrix4d::r11
override var z by this@MutableMatrix4d::r12
override var w by this@MutableMatrix4d::r13
}
set(value) {
r10 = value.x
r11 = value.y
r12 = value.z
r13 = value.w
}
override var r2: Vector4d
get() = object : MutableVector4d(r20, r21, r22, r23) {
override var x by this@MutableMatrix4d::r20
override var y by this@MutableMatrix4d::r21
override var z by this@MutableMatrix4d::r22
override var w by this@MutableMatrix4d::r23
}
set(value) {
r20 = value.x
r21 = value.y
r22 = value.z
r23 = value.w
}
override var r3: Vector4d
get() = object : MutableVector4d(r30, r31, r32, r33) {
override var x by this@MutableMatrix4d::r30
override var y by this@MutableMatrix4d::r31
override var z by this@MutableMatrix4d::r32
override var w by this@MutableMatrix4d::r33
}
set(value) {
r30 = value.x
r31 = value.y
r32 = value.z
r33 = value.w
}
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Double = 1.0, r01: Double = 0.0, r02: Double = 0.0, r03: Double = 0.0,
r10: Double = 0.0, r11: Double = 1.0, r12: Double = 0.0, r13: Double = 0.0,
r20: Double = 0.0, r21: Double = 0.0, r22: Double = 1.0, r23: Double = 0.0,
r30: Double = 0.0, r31: Double = 0.0, r32: Double = 0.0, r33: Double = 1.0,
): MutableMatrix4d {
return MutableMatrix4d(
c00 = r00, c10 = r01, c20 = r02, c30 = r03,
c01 = r10, c11 = r11, c21 = r12, c31 = r13,
c02 = r20, c12 = r21, c22 = r22, c32 = r23,
c03 = r30, c13 = r31, c23 = r32, c33 = r33,
)
}
/**
* Initializes new mutable zero matrix.
*/
fun zero(): MutableMatrix4d {
return MutableMatrix4d(
c00 = 0.0, c10 = 0.0, c20 = 0.0, c30 = 0.0,
c01 = 0.0, c11 = 0.0, c21 = 0.0, c31 = 0.0,
c02 = 0.0, c12 = 0.0, c22 = 0.0, c32 = 0.0,
c03 = 0.0, c13 = 0.0, c23 = 0.0, c33 = 0.0,
)
}
}
}

418
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/ndouble/MatrixVd.kt
View File

@ -1,418 +0,0 @@
@file:Suppress("unchecked_cast", "unused")
package ru.dbotthepony.kvector.matrix.ndouble
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.matrix.*
import ru.dbotthepony.kvector.matrix.generated.MultiplicationsDouble
import ru.dbotthepony.kvector.narray.Double2Dimensional
private fun checkRows(value: Int): Int {
require(value > 0) { "Invalid amount of rows $value" }
return value
}
private fun checkColumns(value: Int): Int {
require(value > 0) { "Invalid amount of columns $value" }
return value
}
/**
* Abstract class with only one protected abstract method: [factorize].
*
* This class is meant to be only internally overridden (to implement interfaces), but if you really need to, you can
* extend this class.
*/
abstract class AbstractMatrixVd<T : AbstractMatrixVd<T>> protected constructor(
final override val columns: Int,
final override val rows: Int,
@JvmField protected val memory: Double2Dimensional
) : IMatrix<T>, IDoubleMatrix<T>, ITransposable<T> {
/**
* To avoid middle-object creation when deriving from [AbstractMatrixVd] ([MatrixVd]), this method
* allows to factory produce new instances of final class
*/
protected abstract fun factorize(memory: Double2Dimensional): T
protected open val flexible = true
final override fun get(column: Int, row: Int): Double {
return memory[column, row]
}
override fun plus(other: IMatrixGetterDouble): T {
requireSizeEquals(other)
val result = Double2Dimensional(columns, rows)
for (column in 0 until columns) {
for (row in 0 until rows) {
result[column, row] = memory[column, row] + other[column, row]
}
}
return factorize(result)
}
override fun minus(other: IMatrixGetterDouble): T {
requireSizeEquals(other)
val result = Double2Dimensional(columns, rows)
for (column in 0 until columns) {
for (row in 0 until rows) {
result[column, row] = memory[column, row] - other[column, row]
}
}
return factorize(result)
}
override fun times(other: IMatrixGetterDouble): T {
require(flexible || sizeEquals(other)) { "${this::class.qualifiedName} is not a flexible" }
val result = multiplyMatrix(this, other)
return factorize(result)
}
override fun times(other: Double): T {
val result = Double2Dimensional(columns, rows)
for (column in 0 until columns) {
for (row in 0 until rows) {
result[column, row] = memory[column, row] * other
}
}
return factorize(result)
}
override fun div(other: Double): T {
val result = Double2Dimensional(columns, rows)
for (column in 0 until columns) {
for (row in 0 until rows) {
result[column, row] = memory[column, row] / other
}
}
return factorize(result)
}
override val transposed: T
get() {
val result = transposeMatrix(this)
return factorize(result)
}
override val trace: Double?
get() {
if (!isSquareMatrix)
return null
var calc = 0.0
for (i in 0 until rows) {
calc += memory[columns, rows]
}
return calc
}
override val determinant: Double?
get() {
if (!isSquareMatrix)
return null
return matrixDeterminant(this)
}
override val isFinite: Boolean
get() {
for (column in 0 until columns) {
for (row in 0 until rows) {
if (!memory[column, row].isFinite()) {
return false
}
}
}
return true
}
override val isNaN: Boolean
get() {
for (column in 0 until columns) {
for (row in 0 until rows) {
if (memory[column, row].isNaN()) {
return true
}
}
}
return false
}
override val inverse: T?
get() {
if (!isSquareMatrix)
return null
val result = inverseMatrix(memory) ?: return null
return factorize(result)
}
override val cofactor: T?
get() {
if (!isSquareMatrix)
return null
return factorize(cofactorMatrix(memory))
}
override val adjugate: T?
get() {
if (!isSquareMatrix)
return null
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()
}
}
/**
* Abstract class inheriting [AbstractMatrixVd], implementing mutating methods.
*
* This class is meant to be only internally overridden (to implement interfaces), but if you really need to, you can
* extend this class.
*/
abstract class AbstractMutableMatrixVd<T : AbstractMutableMatrixVd<T>> protected constructor(
columns: Int,
rows: Int,
memory: Double2Dimensional
) : AbstractMatrixVd<T>(columns, rows, memory), IMutableDoubleMatrix<T>, IMutableTransposable<T> {
final override fun set(column: Int, row: Int, value: Double) {
memory[column, row] = value
}
override fun timesMut(other: Double): T {
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] *= other
}
}
return this as T
}
override fun divMut(other: Double): T {
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] /= other
}
}
return this as T
}
override fun timesMut(other: IMatrixGetterDouble): T {
requireSizeEquals(other)
if (MultiplicationsDouble.multiplyMatrix(this, other, this) == null) {
val result = multiplyMatrix(this, other)
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] = result[column, row]
}
}
}
return this as T
}
override fun plusMut(other: IMatrixGetterDouble): T {
requireSizeEquals(other)
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] += other[column, row]
}
}
return this as T
}
override fun minusMut(other: IMatrixGetterDouble): T {
requireSizeEquals(other)
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] -= other[column, row]
}
}
return this as T
}
override fun transpose(): T {
check(isSquareMatrix) { "This matrix is not a square matrix (${columns}x${rows})" }
val result = transposeMatrix(this)
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] = result[column, row]
}
}
return this as T
}
}
/**
* Represents immutable matrix with arbitrary dimensions, storing values as [Double]s,
* backed by [Double2Dimensional].
*
* Matrix is initialized to zeros upon creation.
*/
open class MatrixVd protected constructor(
columns: Int,
rows: Int,
memory: Double2Dimensional
) : AbstractMatrixVd<MatrixVd>(columns, rows, memory) {
constructor(columns: Int, rows: Int) : this(checkColumns(columns), checkRows(rows), Double2Dimensional(checkColumns(columns), checkRows(rows)))
override fun factorize(memory: Double2Dimensional): MatrixVd {
return MatrixVd(memory.columns, memory.rows, memory)
}
companion object {
/**
* Copies specified matrix into new [MatrixVd]
*/
fun copy(input: IMatrixGetterDouble): MatrixVd {
val memory = Double2Dimensional(input.columns, input.rows)
for (column in 0 until input.columns) {
for (row in 0 until input.rows) {
memory[column, row] = input[column, row]
}
}
return MatrixVd(input.columns, input.rows, memory)
}
}
}
/**
* Represents mutable matrix with arbitrary dimensions, storing values as [Double]s,
* backed by [Double2Dimensional].
*
* Matrix is initialized to zeros upon creation.
*/
open class MutableMatrixVd protected constructor(
columns: Int,
rows: Int,
memory: Double2Dimensional
) : AbstractMutableMatrixVd<MutableMatrixVd>(columns, rows, memory) {
constructor(columns: Int, rows: Int) : this(checkColumns(columns), checkRows(rows), Double2Dimensional(checkColumns(columns), checkRows(rows)))
override fun factorize(memory: Double2Dimensional): MutableMatrixVd {
return MutableMatrixVd(memory.columns, memory.rows, memory)
}
companion object {
/**
* Copies specified matrix into new [MutableMatrixVd]
*/
fun copy(input: IMatrixGetterDouble): MutableMatrixVd {
val memory = Double2Dimensional(input.columns, input.rows)
for (column in 0 until input.columns) {
for (row in 0 until input.rows) {
memory[column, row] = input[column, row]
}
}
return MutableMatrixVd(input.columns, input.rows, memory)
}
}
}

226
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/nfloat/Matrix2f.kt
View File

@ -1,226 +0,0 @@
@file:Suppress("unused")
package ru.dbotthepony.kvector.matrix.nfloat
import ru.dbotthepony.kvector.api.concrete.IMatrix2f
import ru.dbotthepony.kvector.narray.Float2Dimensional
import ru.dbotthepony.kvector.vector.nfloat.Vector2f
import ru.dbotthepony.kvector.matrix.ndouble.Matrix4d
import ru.dbotthepony.kvector.matrix.ndouble.MutableMatrix4d
import ru.dbotthepony.kvector.vector.nfloat.MutableVector2f
/**
* Represents immutable concrete matrix with 2x2 dimensions, storing values as [Float]s.
*
* See [Matrix4d] for documentation, with reflection of this class.
*
* Vectorized access use [Vector2f], generic interface is represented by [IMatrix2f].
*/
class Matrix2f : AbstractMatrixVf<Matrix2f>, IMatrix2f<Matrix2f> {
constructor(
c00: Float = 1f, c01: Float = 0f,
c10: Float = 0f, c11: Float = 1f,
) : super(2, 2, Float2Dimensional(2, 2).also {
it[0, 0] = c00
it[1, 0] = c10
it[0, 1] = c01
it[1, 1] = c11
})
private constructor(memory: Float2Dimensional) : super(2, 2, memory)
override val flexible: Boolean = false
override fun factorize(memory: Float2Dimensional): Matrix2f {
requireSizeEquals(memory)
return Matrix2f(memory)
}
override val c00: Float get() = memory[0, 0]
override val c10: Float get() = memory[1, 0]
override val c01: Float get() = memory[0, 1]
override val c11: Float get() = memory[1, 1]
override val r00 by this::c00
override val r01 by this::c10
override val r10 by this::c01
override val r11 by this::c11
override val a11 by this::c00
override val a21 by this::c01
override val a12 by this::c10
override val a22 by this::c11
override val b11 by this::c00
override val b21 by this::c10
override val b12 by this::c01
override val b22 by this::c11
override val c0: Vector2f get() = Vector2f(c00, c01)
override val c1: Vector2f get() = Vector2f(c10, c11)
override val r0: Vector2f get() = Vector2f(r00, r01)
override val r1: Vector2f get() = Vector2f(r10, r11)
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Float = 1f, r01: Float = 0f,
r10: Float = 0f, r11: Float = 1f,
): Matrix2f {
return Matrix2f(
c00 = r00, c10 = r01,
c01 = r10, c11 = r11,
)
}
/**
* Initializes new zero matrix.
*
* Since [Matrix2f] is immutable, it is encouraged you use [ZERO] property instead.
*/
fun zero(): Matrix2f {
return Matrix2f(
c00 = 0f, c10 = 0f,
c01 = 0f, c11 = 0f,
)
}
/**
* All values of this matrix are zero
*/
@JvmField val ZERO = zero()
/**
* Main diagonal of this matrix is 1s
*/
@JvmField val IDENTITY = Matrix2f()
}
}
/**
* Represents mutable concrete matrix with 2x2 dimensions, storing values as [Double]s.
*
* See [MutableMatrix4d] for documentation, with reflection of this class.
*
* Vectorized access use [MutableVector2f], generic interface is represented by [IMatrix2f].
*/
class MutableMatrix2f : AbstractMutableMatrixVf<MutableMatrix2f>, IMatrix2f<MutableMatrix2f> {
constructor(
c00: Float = 1f, c01: Float = 0f,
c10: Float = 0f, c11: Float = 1f,
) : super(2, 2, Float2Dimensional(2, 2).also {
it[0, 0] = c00
it[1, 0] = c10
it[0, 1] = c01
it[1, 1] = c11
})
private constructor(memory: Float2Dimensional) : super(2, 2, memory)
override val flexible: Boolean = false
override fun factorize(memory: Float2Dimensional): MutableMatrix2f {
requireSizeEquals(memory)
return MutableMatrix2f(memory)
}
override var c00: Float
get() = memory[0, 0]
set(value) { memory[0, 0] = value }
override var c10: Float
get() = memory[1, 0]
set(value) { memory[1, 0] = value }
override var c01: Float
get() = memory[0, 1]
set(value) { memory[0, 1] = value }
override var c11: Float
get() = memory[1, 1]
set(value) { memory[1, 1] = value }
override var r00 by this::c00
override var r01 by this::c10
override var r10 by this::c01
override var r11 by this::c11
override var a11 by this::c00
override var a21 by this::c01
override var a12 by this::c10
override var a22 by this::c11
override var b11 by this::c00
override var b21 by this::c10
override var b12 by this::c01
override var b22 by this::c11
override var c0: Vector2f
get() = object : MutableVector2f(c00, c01) {
override var x by this@MutableMatrix2f::c00
override var y by this@MutableMatrix2f::c01
}
set(value) {
c00 = value.x
c01 = value.y
}
override var c1: Vector2f
get() = object : MutableVector2f(c10, c11) {
override var x by this@MutableMatrix2f::c10
override var y by this@MutableMatrix2f::c11
}
set(value) {
c10 = value.x
c11 = value.y
}
override var r0: Vector2f
get() = object : MutableVector2f(r00, r01) {
override var x by this@MutableMatrix2f::r00
override var y by this@MutableMatrix2f::r01
}
set(value) {
r00 = value.x
r01 = value.y
}
override var r1: Vector2f
get() = object : MutableVector2f(r10, r11) {
override var x by this@MutableMatrix2f::r10
override var y by this@MutableMatrix2f::r11
}
set(value) {
r10 = value.x
r11 = value.y
}
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Float = 1f, r01: Float = 0f,
r10: Float = 0f, r11: Float = 1f,
): MutableMatrix2f {
return MutableMatrix2f(
c00 = r00, c10 = r01,
c01 = r10, c11 = r11,
)
}
/**
* Initializes new zero matrix.
*/
fun zero(): MutableMatrix2f {
return MutableMatrix2f(
c00 = 0f, c10 = 0f,
c01 = 0f, c11 = 0f,
)
}
}
}

401
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/nfloat/Matrix3f.kt
View File

@ -1,401 +0,0 @@
@file:Suppress("unused")
package ru.dbotthepony.kvector.matrix.nfloat
import ru.dbotthepony.kvector.api.IMutableSquareMatrix
import ru.dbotthepony.kvector.api.concrete.IMatrix3f
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.*
/**
* Represents immutable concrete matrix with 3x3 dimensions, storing values as [Float]s.
*
* See [Matrix4d] for documentation, with reflection of this class.
*
* Vectorized access use [Vector3f], generic interface is represented by [IMatrix3f].
*/
class Matrix3f : AbstractMatrixVf<Matrix3f>, IMatrix3f<Matrix3f> {
constructor(
c00: Float = 1f, c01: Float = 0f, c02: Float = 0f,
c10: Float = 0f, c11: Float = 1f, c12: Float = 0f,
c20: Float = 0f, c21: Float = 0f, c22: Float = 1f,
) : super(3, 3, Float2Dimensional(3, 3).also {
it[0, 0] = c00
it[1, 0] = c10
it[2, 0] = c20
it[0, 1] = c01
it[1, 1] = c11
it[2, 1] = c21
it[0, 2] = c02
it[1, 2] = c12
it[2, 2] = c22
})
private constructor(memory: Float2Dimensional) : super(3, 3, memory)
override val flexible: Boolean = false
override fun factorize(memory: Float2Dimensional): Matrix3f {
requireSizeEquals(memory)
return Matrix3f(memory)
}
override val c00: Float get() = memory[0, 0]
override val c10: Float get() = memory[1, 0]
override val c20: Float get() = memory[2, 0]
override val c01: Float get() = memory[0, 1]
override val c11: Float get() = memory[1, 1]
override val c21: Float get() = memory[2, 1]
override val c02: Float get() = memory[0, 2]
override val c12: Float get() = memory[1, 2]
override val c22: Float get() = memory[2, 2]
override val r00 by this::c00
override val r01 by this::c10
override val r02 by this::c20
override val r10 by this::c01
override val r11 by this::c11
override val r12 by this::c21
override val r20 by this::c02
override val r21 by this::c12
override val r22 by this::c22
override val a11 by this::c00
override val a21 by this::c01
override val a31 by this::c02
override val a12 by this::c10
override val a22 by this::c11
override val a32 by this::c12
override val a13 by this::c20
override val a23 by this::c21
override val a33 by this::c22
override val b11 by this::c00
override val b21 by this::c10
override val b31 by this::c20
override val b12 by this::c01
override val b22 by this::c11
override val b32 by this::c21
override val b13 by this::c02
override val b23 by this::c12
override val b33 by this::c22
override val c0: Vector3f get() = Vector3f(c00, c01, c02)
override val c1: Vector3f get() = Vector3f(c10, c11, c12)
override val c2: Vector3f get() = Vector3f(c20, c21, c22)
override val r0: Vector3f get() = Vector3f(r00, r01, r02)
override val r1: Vector3f get() = Vector3f(r10, r11, r12)
override val r2: Vector3f get() = Vector3f(r20, r21, r22)
override fun translate(vector: Vector2f): Matrix3f {
return rm(
r00, r01, r02 + vector.x,
r10, r11, r12 + vector.y,
r20, r21, r22,
)
}
override fun translateWithMultiplication(vector: Vector2f): Matrix3f {
return rm(
r00, r01, r02 + vector.x * r00 + vector.y * r01,
r10, r11, r12 + vector.x * r10 + vector.y * r11,
r20, r21, r22,
)
}
override fun scale(vector: Vector3f): Matrix3f {
return rm(
r00 * vector.x, r01, r02,
r10, r11 * vector.y, r12,
r20, r21, r22 * vector.z
)
}
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Float = 1f, r01: Float = 0f, r02: Float = 0f,
r10: Float = 0f, r11: Float = 1f, r12: Float = 0f,
r20: Float = 0f, r21: Float = 0f, r22: Float = 1f,
): Matrix3f {
return Matrix3f(
c00 = r00, c10 = r01, c20 = r02,
c01 = r10, c11 = r11, c21 = r12,
c02 = r20, c12 = r21, c22 = r22,
)
}
/**
* Initializes new zero matrix.
*
* Since [Matrix3f] is immutable, it is encouraged you use [ZERO] property instead.
*/
fun zero(): Matrix3f {
return Matrix3f(
c00 = 0f, c10 = 0f, c20 = 0f,
c01 = 0f, c11 = 0f, c21 = 0f,
c02 = 0f, c12 = 0f, c22 = 0f,
)
}
/**
* All values of this matrix are zero
*/
@JvmField val ZERO = zero()
/**
* Main diagonal of this matrix is 1s
*/
@JvmField val IDENTITY = Matrix3f()
}
}
/**
* Represents mutable concrete matrix with 3x3 dimensions, storing values as [Float]s.
*
* See [MutableMatrix4d] for documentation, with reflection of this class.
*
* Vectorized access use [MutableVector3f], generic interface is represented by [IMatrix3f].
*/
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,
c20: Float = 0f, c21: Float = 0f, c22: Float = 1f,
) : super(3, 3, Float2Dimensional(3, 3).also {
it[0, 0] = c00
it[1, 0] = c10
it[2, 0] = c20
it[0, 1] = c01
it[1, 1] = c11
it[2, 1] = c21
it[0, 2] = c02
it[1, 2] = c12
it[2, 2] = c22
})
private constructor(memory: Float2Dimensional) : super(3, 3, memory)
override val flexible: Boolean = false
override fun factorize(memory: Float2Dimensional): MutableMatrix3f {
requireSizeEquals(memory)
return MutableMatrix3f(memory)
}
override fun translateWithMultiplicationMut(vector: Vector2f): MutableMatrix3f {
r02 += vector.x * r00 + vector.y * r01
r12 += vector.x * r10 + vector.y * r11
return this
}
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
}
override var c00: Float
get() = memory[0, 0]
set(value) { memory[0, 0] = value }
override var c10: Float
get() = memory[1, 0]
set(value) { memory[1, 0] = value }
override var c20: Float
get() = memory[2, 0]
set(value) { memory[2, 0] = value }
override var c01: Float
get() = memory[0, 1]
set(value) { memory[0, 1] = value }
override var c11: Float
get() = memory[1, 1]
set(value) { memory[1, 1] = value }
override var c21: Float
get() = memory[2, 1]
set(value) { memory[2, 1] = value }
override var c02: Float
get() = memory[0, 2]
set(value) { memory[0, 2] = value }
override var c12: Float
get() = memory[1, 2]
set(value) { memory[1, 2] = value }
override var c22: Float
get() = memory[2, 2]
set(value) { memory[2, 2] = value }
override var r00 by this::c00
override var r01 by this::c10
override var r02 by this::c20
override var r10 by this::c01
override var r11 by this::c11
override var r12 by this::c21
override var r20 by this::c02
override var r21 by this::c12
override var r22 by this::c22
override var a11 by this::c00
override var a21 by this::c01
override var a31 by this::c02
override var a12 by this::c10
override var a22 by this::c11
override var a32 by this::c12
override var a13 by this::c20
override var a23 by this::c21
override var a33 by this::c22
override var b11 by this::c00
override var b21 by this::c10
override var b31 by this::c20
override var b12 by this::c01
override var b22 by this::c11
override var b32 by this::c21
override var b13 by this::c02
override var b23 by this::c12
override var b33 by this::c22
override var translation: Vector2f
get() = super.translation
set(value) {
r02 = value.x
r12 = value.y
}
override var c0: Vector3f
get() = object : MutableVector3f(c00, c01, c02) {
override var x by this@MutableMatrix3f::c00
override var y by this@MutableMatrix3f::c01
override var z by this@MutableMatrix3f::c02
}
set(value) {
c00 = value.x
c01 = value.y
c02 = value.z
}
override var c1: Vector3f
get() = object : MutableVector3f(c10, c11, c12) {
override var x by this@MutableMatrix3f::c10
override var y by this@MutableMatrix3f::c11
override var z by this@MutableMatrix3f::c12
}
set(value) {
c10 = value.x
c11 = value.y
c12 = value.z
}
override var c2: Vector3f
get() = object : MutableVector3f(c20, c21, c22) {
override var x by this@MutableMatrix3f::c20
override var y by this@MutableMatrix3f::c21
override var z by this@MutableMatrix3f::c22
}
set(value) {
c20 = value.x
c21 = value.y
c22 = value.z
}
override var r0: Vector3f
get() = object : MutableVector3f(r00, r01, r02) {
override var x by this@MutableMatrix3f::r00
override var y by this@MutableMatrix3f::r01
override var z by this@MutableMatrix3f::r02
}
set(value) {
r00 = value.x
r01 = value.y
r02 = value.z
}
override var r1: Vector3f
get() = object : MutableVector3f(r10, r11, r12) {
override var x by this@MutableMatrix3f::r10
override var y by this@MutableMatrix3f::r11
override var z by this@MutableMatrix3f::r12
}
set(value) {
r10 = value.x
r11 = value.y
r12 = value.z
}
override var r2: Vector3f
get() = object : MutableVector3f(r20, r21, r22) {
override var x by this@MutableMatrix3f::r20
override var y by this@MutableMatrix3f::r21
override var z by this@MutableMatrix3f::r22
}
set(value) {
r20 = value.x
r21 = value.y
r22 = value.z
}
override fun translate(vector: Vector2f): MutableMatrix3f {
return rm(
r00, r01, r02 + vector.x,
r10, r11, r12 + vector.y,
r20, r21, r22,
)
}
override fun translateWithMultiplication(vector: Vector2f): MutableMatrix3f {
return rm(
r00, r01, r02 + vector.x * r00 + vector.y * r01,
r10, r11, r12 + vector.x * r10 + vector.y * r11,
r20, r21, r22,
)
}
override fun scale(vector: Vector3f): MutableMatrix3f {
return rm(
r00 * vector.x, r01, r02,
r10, r11 * vector.y, r12,
r20, r21, r22 * vector.z
)
}
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Float = 1f, r01: Float = 0f, r02: Float = 0f,
r10: Float = 0f, r11: Float = 1f, r12: Float = 0f,
r20: Float = 0f, r21: Float = 0f, r22: Float = 1f,
): MutableMatrix3f {
return MutableMatrix3f(
c00 = r00, c10 = r01, c20 = r02,
c01 = r10, c11 = r11, c21 = r12,
c02 = r20, c12 = r21, c22 = r22,
)
}
/**
* Initializes new zero matrix.
*/
fun zero(): MutableMatrix3f {
return MutableMatrix3f(
c00 = 0f, c10 = 0f, c20 = 0f,
c01 = 0f, c11 = 0f, c21 = 0f,
c02 = 0f, c12 = 0f, c22 = 0f,
)
}
}
}

644
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/nfloat/Matrix4f.kt
View File

@ -1,644 +0,0 @@
@file:Suppress("unused")
package ru.dbotthepony.kvector.matrix.nfloat
import ru.dbotthepony.kvector.api.IMutableSquareMatrix
import ru.dbotthepony.kvector.api.concrete.IMatrix4f
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.
*
* See [Matrix4d] for documentation, with reflection of this class.
*
* Vectorized access use [Vector4f], generic interface is represented by [IMatrix4f].
*/
class Matrix4f : AbstractMatrixVf<Matrix4f>, IMatrix4f<Matrix4f> {
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,
c20: Float = 0f, c21: Float = 0f, c22: Float = 1f, c23: Float = 0f,
c30: Float = 0f, c31: Float = 0f, c32: Float = 0f, c33: Float = 1f,
) : super(4, 4, Float2Dimensional(4, 4).also {
it[0, 0] = c00
it[1, 0] = c10
it[2, 0] = c20
it[3, 0] = c30
it[0, 1] = c01
it[1, 1] = c11
it[2, 1] = c21
it[3, 1] = c31
it[0, 2] = c02
it[1, 2] = c12
it[2, 2] = c22
it[3, 2] = c32
it[0, 3] = c03
it[1, 3] = c13
it[2, 3] = c23
it[3, 3] = c33
})
private constructor(memory: Float2Dimensional) : super(4, 4, memory)
override val flexible: Boolean = false
override fun factorize(memory: Float2Dimensional): Matrix4f {
requireSizeEquals(memory)
return Matrix4f(memory)
}
override val c00: Float get() = memory[0, 0]
override val c10: Float get() = memory[1, 0]
override val c20: Float get() = memory[2, 0]
override val c30: Float get() = memory[3, 0]
override val c01: Float get() = memory[0, 1]
override val c11: Float get() = memory[1, 1]
override val c21: Float get() = memory[2, 1]
override val c31: Float get() = memory[3, 1]
override val c02: Float get() = memory[0, 2]
override val c12: Float get() = memory[1, 2]
override val c22: Float get() = memory[2, 2]
override val c32: Float get() = memory[3, 2]
override val c03: Float get() = memory[0, 3]
override val c13: Float get() = memory[1, 3]
override val c23: Float get() = memory[2, 3]
override val c33: Float get() = memory[3, 3]
override val r00 by this::c00
override val r01 by this::c10
override val r02 by this::c20
override val r03 by this::c30
override val r10 by this::c01
override val r11 by this::c11
override val r12 by this::c21
override val r13 by this::c31
override val r20 by this::c02
override val r21 by this::c12
override val r22 by this::c22
override val r23 by this::c32
override val r30 by this::c03
override val r31 by this::c13
override val r32 by this::c23
override val r33 by this::c33
override val a11 by this::c00
override val a21 by this::c01
override val a31 by this::c02
override val a41 by this::c03
override val a12 by this::c10
override val a22 by this::c11
override val a32 by this::c12
override val a42 by this::c13
override val a13 by this::c20
override val a23 by this::c21
override val a33 by this::c22
override val a43 by this::c23
override val a14 by this::c30
override val a24 by this::c31
override val a34 by this::c32
override val a44 by this::c33
override val b11 by this::c00
override val b21 by this::c10
override val b31 by this::c20
override val b41 by this::c30
override val b12 by this::c01
override val b22 by this::c11
override val b32 by this::c21
override val b42 by this::c31
override val b13 by this::c02
override val b23 by this::c12
override val b33 by this::c22
override val b43 by this::c32
override val b14 by this::c03
override val b24 by this::c13
override val b34 by this::c23
override val b44 by this::c33
override val c0: Vector4f get() = Vector4f(c00, c01, c02, c03)
override val c1: Vector4f get() = Vector4f(c10, c11, c12, c13)
override val c2: Vector4f get() = Vector4f(c20, c21, c22, c23)
override val c3: Vector4f get() = Vector4f(c30, c31, c32, c33)
override val r0: Vector4f get() = Vector4f(r00, r01, r02, r03)
override val r1: Vector4f get() = Vector4f(r10, r11, r12, r13)
override val r2: Vector4f get() = Vector4f(r20, r21, r22, r23)
override val r3: Vector4f get() = Vector4f(r30, r31, r32, r33)
override fun translate(vector: Vector3f): Matrix4f {
return rm(
r00, r01, r02, r03 + vector.x,
r10, r11, r12, r13 + vector.y,
r20, r21, r22, r23 + vector.z,
r30, r31, r32, r33,
)
}
override fun translateWithMultiplication(vector: Vector3f): Matrix4f {
return rm(
r00, r01, r02, r03 + vector.x * r00 + vector.y * r01 + vector.z * r02,
r10, r11, r12, r13 + vector.x * r10 + vector.y * r11 + vector.z * r12,
r20, r21, r22, r23 + vector.x * r20 + vector.y * r21 + vector.z * r22,
r30, r31, r32, r33,
)
}
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 * vector.w,
)
}
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Float = 1f, r01: Float = 0f, r02: Float = 0f, r03: Float = 0f,
r10: Float = 0f, r11: Float = 1f, r12: Float = 0f, r13: Float = 0f,
r20: Float = 0f, r21: Float = 0f, r22: Float = 1f, r23: Float = 0f,
r30: Float = 0f, r31: Float = 0f, r32: Float = 0f, r33: Float = 1f,
): Matrix4f {
return Matrix4f(
c00 = r00, c10 = r01, c20 = r02, c30 = r03,
c01 = r10, c11 = r11, c21 = r12, c31 = r13,
c02 = r20, c12 = r21, c22 = r22, c32 = r23,
c03 = r30, c13 = r31, c23 = r32, c33 = r33,
)
}
/**
* Initializes new zero matrix.
*
* Since [Matrix4f] is immutable, it is encouraged you use [ZERO] property instead.
*/
fun zero(): Matrix4f {
return Matrix4f(
c00 = 0f, c10 = 0f, c20 = 0f, c30 = 0f,
c01 = 0f, c11 = 0f, c21 = 0f, c31 = 0f,
c02 = 0f, c12 = 0f, c22 = 0f, c32 = 0f,
c03 = 0f, c13 = 0f, c23 = 0f, c33 = 0f,
)
}
/**
* All values of this matrix are zero
*/
@JvmField val ZERO = zero()
/**
* 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,
)
}
}
}
/**
* Represents mutable concrete matrix with 4x4 dimensions, storing values as [Float]s.
*
* See [MutableMatrix4d] for documentation, with reflection of this class.
*
* Vectorized access use [MutableVector4f], generic interface is represented by [IMatrix4f].
*/
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,
c20: Float = 0f, c21: Float = 0f, c22: Float = 1f, c23: Float = 0f,
c30: Float = 0f, c31: Float = 0f, c32: Float = 0f, c33: Float = 1f,
) : super(4, 4, Float2Dimensional(4, 4).also {
it[0, 0] = c00
it[1, 0] = c10
it[2, 0] = c20
it[3, 0] = c30
it[0, 1] = c01
it[1, 1] = c11
it[2, 1] = c21
it[3, 1] = c31
it[0, 2] = c02
it[1, 2] = c12
it[2, 2] = c22
it[3, 2] = c32
it[0, 3] = c03
it[1, 3] = c13
it[2, 3] = c23
it[3, 3] = c33
})
private constructor(memory: Float2Dimensional) : super(4, 4, memory)
override val flexible: Boolean = false
override fun factorize(memory: Float2Dimensional): MutableMatrix4f {
requireSizeEquals(memory)
return MutableMatrix4f(memory)
}
override fun translate(vector: Vector3f): MutableMatrix4f {
return rm(
r00, r01, r02, r03 + vector.x,
r10, r11, r12, r13 + vector.y,
r20, r21, r22, r23 + vector.z,
r30, r31, r32, r33,
)
}
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,
r10, r11, r12, r13 + vector.x * r10 + vector.y * r11 + vector.z * r12,
r20, r21, r22, r23 + vector.x * r20 + vector.y * r21 + vector.z * r22,
r30, r31, r32, r33,
)
}
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 * vector.w,
)
}
override fun translateWithMultiplicationMut(vector: Vector3f): MutableMatrix4f {
r03 += vector.x * r00 + vector.y * r01 + vector.z * r02
r13 += vector.x * r10 + vector.y * r11 + vector.z * r12
r23 += vector.x * r20 + vector.y * r21 + vector.z * r22
return this
}
override fun scaleMut(vector: Vector4f): MutableMatrix4f {
c00 *= vector.x
c11 *= vector.y
c22 *= vector.z
c33 *= vector.w
return this
}
override var translation: Vector3f
get() = super.translation
set(value) {
r03 = value.x
r13 = value.y
r23 = value.z
}
override var c00: Float
get() = memory[0, 0]
set(value) { memory[0, 0] = value }
override var c10: Float
get() = memory[1, 0]
set(value) { memory[1, 0] = value }
override var c20: Float
get() = memory[2, 0]
set(value) { memory[2, 0] = value }
override var c30: Float
get() = memory[3, 0]
set(value) { memory[3, 0] = value }
override var c01: Float
get() = memory[0, 1]
set(value) { memory[0, 1] = value }
override var c11: Float
get() = memory[1, 1]
set(value) { memory[1, 1] = value }
override var c21: Float
get() = memory[2, 1]
set(value) { memory[2, 1] = value }
override var c31: Float
get() = memory[3, 1]
set(value) { memory[3, 1] = value }
override var c02: Float
get() = memory[0, 2]
set(value) { memory[0, 2] = value }
override var c12: Float
get() = memory[1, 2]
set(value) { memory[1, 2] = value }
override var c22: Float
get() = memory[2, 2]
set(value) { memory[2, 2] = value }
override var c32: Float
get() = memory[3, 2]
set(value) { memory[3, 2] = value }
override var c03: Float
get() = memory[0, 3]
set(value) { memory[0, 3] = value }
override var c13: Float
get() = memory[1, 3]
set(value) { memory[1, 3] = value }
override var c23: Float
get() = memory[2, 3]
set(value) { memory[2, 3] = value }
override var c33: Float
get() = memory[3, 3]
set(value) { memory[3, 3] = value }
override var r00 by this::c00
override var r01 by this::c10
override var r02 by this::c20
override var r03 by this::c30
override var r10 by this::c01
override var r11 by this::c11
override var r12 by this::c21
override var r13 by this::c31
override var r20 by this::c02
override var r21 by this::c12
override var r22 by this::c22
override var r23 by this::c32
override var r30 by this::c03
override var r31 by this::c13
override var r32 by this::c23
override var r33 by this::c33
override var a11 by this::c00
override var a21 by this::c01
override var a31 by this::c02
override var a41 by this::c03
override var a12 by this::c10
override var a22 by this::c11
override var a32 by this::c12
override var a42 by this::c13
override var a13 by this::c20
override var a23 by this::c21
override var a33 by this::c22
override var a43 by this::c23
override var a14 by this::c30
override var a24 by this::c31
override var a34 by this::c32
override var a44 by this::c33
override var b11 by this::c00
override var b21 by this::c01
override var b31 by this::c02
override var b41 by this::c03
override var b12 by this::c10
override var b22 by this::c11
override var b32 by this::c12
override var b42 by this::c13
override var b13 by this::c20
override var b23 by this::c21
override var b33 by this::c22
override var b43 by this::c23
override var b14 by this::c30
override var b24 by this::c31
override var b34 by this::c32
override var b44 by this::c33
override var c0: Vector4f
get() = object : MutableVector4f(c00, c01, c02, c03) {
override var x by this@MutableMatrix4f::c00
override var y by this@MutableMatrix4f::c01
override var z by this@MutableMatrix4f::c02
override var w by this@MutableMatrix4f::c03
}
set(value) {
c00 = value.x
c01 = value.y
c02 = value.z
c03 = value.w
}
override var c1: Vector4f
get() = object : MutableVector4f(c10, c11, c12, c13) {
override var x by this@MutableMatrix4f::c10
override var y by this@MutableMatrix4f::c11
override var z by this@MutableMatrix4f::c12
override var w by this@MutableMatrix4f::c13
}
set(value) {
c10 = value.x
c11 = value.y
c12 = value.z
c13 = value.w
}
override var c2: Vector4f
get() = object : MutableVector4f(c20, c21, c22, c23) {
override var x by this@MutableMatrix4f::c20
override var y by this@MutableMatrix4f::c21
override var z by this@MutableMatrix4f::c22
override var w by this@MutableMatrix4f::c23
}
set(value) {
c20 = value.x
c21 = value.y
c22 = value.z
c23 = value.w
}
override var c3: Vector4f
get() = object : MutableVector4f(c30, c31, c32, c33) {
override var x by this@MutableMatrix4f::c30
override var y by this@MutableMatrix4f::c31
override var z by this@MutableMatrix4f::c32
override var w by this@MutableMatrix4f::c33
}
set(value) {
c30 = value.x
c31 = value.y
c32 = value.z
c33 = value.w
}
override var r0: Vector4f
get() = object : MutableVector4f(r00, r01, r02, r03) {
override var x by this@MutableMatrix4f::r00
override var y by this@MutableMatrix4f::r01
override var z by this@MutableMatrix4f::r02
override var w by this@MutableMatrix4f::r03
}
set(value) {
r00 = value.x
r01 = value.y
r02 = value.z
r03 = value.w
}
override var r1: Vector4f
get() = object : MutableVector4f(r10, r11, r12, r13) {
override var x by this@MutableMatrix4f::r10
override var y by this@MutableMatrix4f::r11
override var z by this@MutableMatrix4f::r12
override var w by this@MutableMatrix4f::r13
}
set(value) {
r10 = value.x
r11 = value.y
r12 = value.z
r13 = value.w
}
override var r2: Vector4f
get() = object : MutableVector4f(r20, r21, r22, r23) {
override var x by this@MutableMatrix4f::r20
override var y by this@MutableMatrix4f::r21
override var z by this@MutableMatrix4f::r22
override var w by this@MutableMatrix4f::r23
}
set(value) {
r20 = value.x
r21 = value.y
r22 = value.z
r23 = value.w
}
override var r3: Vector4f
get() = object : MutableVector4f(r30, r31, r32, r33) {
override var x by this@MutableMatrix4f::r30
override var y by this@MutableMatrix4f::r31
override var z by this@MutableMatrix4f::r32
override var w by this@MutableMatrix4f::r33
}
set(value) {
r30 = value.x
r31 = value.y
r32 = value.z
r33 = value.w
}
companion object {
/**
* Initializes new matrix, with arguments taken in *Row-Major* order, that is, fist row of values
* is fist row in created matrix.
*/
fun rm(
r00: Float = 1f, r01: Float = 0f, r02: Float = 0f, r03: Float = 0f,
r10: Float = 0f, r11: Float = 1f, r12: Float = 0f, r13: Float = 0f,
r20: Float = 0f, r21: Float = 0f, r22: Float = 1f, r23: Float = 0f,
r30: Float = 0f, r31: Float = 0f, r32: Float = 0f, r33: Float = 1f,
): MutableMatrix4f {
return MutableMatrix4f(
c00 = r00, c10 = r01, c20 = r02, c30 = r03,
c01 = r10, c11 = r11, c21 = r12, c31 = r13,
c02 = r20, c12 = r21, c22 = r22, c32 = r23,
c03 = r30, c13 = r31, c23 = r32, c33 = r33,
)
}
/**
* Initializes new mutable zero matrix.
*/
fun zero(): MutableMatrix4f {
return MutableMatrix4f(
c00 = 0f, c10 = 0f, c20 = 0f, c30 = 0f,
c01 = 0f, c11 = 0f, c21 = 0f, c31 = 0f,
c02 = 0f, c12 = 0f, c22 = 0f, c32 = 0f,
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,
)
}
}
}

418
src/kvector/kotlin/ru/dbotthepony/kvector/matrix/nfloat/MatrixVf.kt
View File

@ -1,418 +0,0 @@
@file:Suppress("unchecked_cast", "unused")
package ru.dbotthepony.kvector.matrix.nfloat
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.matrix.*
import ru.dbotthepony.kvector.matrix.generated.MultiplicationsFloat
import ru.dbotthepony.kvector.narray.Float2Dimensional
private fun checkRows(value: Int): Int {
require(value > 0) { "Invalid amount of rows $value" }
return value
}
private fun checkColumns(value: Int): Int {
require(value > 0) { "Invalid amount of columns $value" }
return value
}
/**
* Abstract class with only one protected abstract method: [factorize].
*
* This class is meant to be only internally overridden (to implement interfaces), but if you really need to, you can
* extend this class.
*/
abstract class AbstractMatrixVf<T : AbstractMatrixVf<T>> protected constructor(
final override val columns: Int,
final override val rows: Int,
@JvmField protected val memory: Float2Dimensional
) : IMatrix<T>, IFloatMatrix<T>, ITransposable<T> {
/**
* To avoid middle-object creation when deriving from [AbstractMatrixVf] ([MatrixVf]), this method
* allows to factory produce new instances of final class
*/
protected abstract fun factorize(memory: Float2Dimensional): T
protected open val flexible = true
final override fun get(column: Int, row: Int): Float {
return memory[column, row]
}
override fun plus(other: IMatrixGetterFloat): T {
requireSizeEquals(other)
val result = Float2Dimensional(columns, rows)
for (column in 0 until columns) {
for (row in 0 until rows) {
result[column, row] = memory[column, row] + other[column, row]
}
}
return factorize(result)
}
override fun minus(other: IMatrixGetterFloat): T {
requireSizeEquals(other)
val result = Float2Dimensional(columns, rows)
for (column in 0 until columns) {
for (row in 0 until rows) {
result[column, row] = memory[column, row] - other[column, row]
}
}
return factorize(result)
}
override fun times(other: IMatrixGetterFloat): T {
require(flexible || sizeEquals(other)) { "${this::class.qualifiedName} is not a flexible" }
val result = multiplyMatrix(this, other)
return factorize(result)
}
override fun times(other: Float): T {
val result = Float2Dimensional(columns, rows)
for (column in 0 until columns) {
for (row in 0 until rows) {
result[column, row] = memory[column, row] * other
}
}
return factorize(result)
}
override fun div(other: Float): T {
val result = Float2Dimensional(columns, rows)
for (column in 0 until columns) {
for (row in 0 until rows) {
result[column, row] = memory[column, row] / other
}
}
return factorize(result)
}
override val transposed: T
get() {
val result = transposeMatrix(this)
return factorize(result)
}
override val trace: Float?
get() {
if (!isSquareMatrix)
return null
var calc = 0f
for (i in 0 until rows) {
calc += memory[columns, rows]
}
return calc
}
override val determinant: Float?
get() {
if (!isSquareMatrix)
return null
return matrixDeterminant(this)
}
override val isFinite: Boolean
get() {
for (column in 0 until columns) {
for (row in 0 until rows) {
if (!memory[column, row].isFinite()) {
return false
}
}
}
return true
}
override val isNaN: Boolean
get() {
for (column in 0 until columns) {
for (row in 0 until rows) {
if (memory[column, row].isNaN()) {
return true
}
}
}
return false
}
override val inverse: T?
get() {
if (!isSquareMatrix)
return null
val result = inverseMatrix(memory) ?: return null
return factorize(result)
}
override val cofactor: T?
get() {
if (!isSquareMatrix)
return null
return factorize(cofactorMatrix(memory))
}
override val adjugate: T?
get() {
if (!isSquareMatrix)
return null
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()
}
}
/**
* Abstract class inheriting [AbstractMatrixVf], implementing mutating methods.
*
* This class is meant to be only internally overridden (to implement interfaces), but if you really need to, you can
* extend this class.
*/
abstract class AbstractMutableMatrixVf<T : AbstractMutableMatrixVf<T>> protected constructor(
columns: Int,
rows: Int,
memory: Float2Dimensional
) : AbstractMatrixVf<T>(columns, rows, memory), IMutableFloatMatrix<T>, IMutableTransposable<T> {
final override fun set(column: Int, row: Int, value: Float) {
memory[column, row] = value
}
override fun timesMut(other: Float): T {
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] *= other
}
}
return this as T
}
override fun divMut(other: Float): T {
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] /= other
}
}
return this as T
}
override fun timesMut(other: IMatrixGetterFloat): T {
requireSizeEquals(other)
if (MultiplicationsFloat.multiplyMatrix(this, other, this) == null) {
val result = multiplyMatrix(this, other)
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] = result[column, row]
}
}
}
return this as T
}
override fun plusMut(other: IMatrixGetterFloat): T {
requireSizeEquals(other)
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] += other[column, row]
}
}
return this as T
}
override fun minusMut(other: IMatrixGetterFloat): T {
requireSizeEquals(other)
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] -= other[column, row]
}
}
return this as T
}
override fun transpose(): T {
check(isSquareMatrix) { "This matrix is not a square matrix (${columns}x${rows})" }
val result = transposeMatrix(this)
for (column in 0 until columns) {
for (row in 0 until rows) {
memory[column, row] = result[column, row]
}
}
return this as T
}
}
/**
* Represents immutable matrix with arbitrary dimensions, storing values as [Float]s,
* backed by [Float2Dimensional].
*
* Matrix is initialized to zeros upon creation.
*/
open class MatrixVf protected constructor(
columns: Int,
rows: Int,
memory: Float2Dimensional
) : AbstractMatrixVf<MatrixVf>(columns, rows, memory) {
constructor(columns: Int, rows: Int) : this(checkColumns(columns), checkRows(rows), Float2Dimensional(checkColumns(columns), checkRows(rows)))
override fun factorize(memory: Float2Dimensional): MatrixVf {
return MatrixVf(memory.columns, memory.rows, memory)
}
companion object {
/**
* Copies specified matrix into new [MatrixVf]
*/
fun copy(input: IMatrixGetterFloat): MatrixVf {
val memory = Float2Dimensional(input.columns, input.rows)
for (column in 0 until input.columns) {
for (row in 0 until input.rows) {
memory[column, row] = input[column, row]
}
}
return MatrixVf(input.columns, input.rows, memory)
}
}
}
/**
* Represents mutable matrix with arbitrary dimensions, storing values as [Float]s,
* backed by [Float2Dimensional].
*
* Matrix is initialized to zeros upon creation.
*/
open class MutableMatrixVf protected constructor(
columns: Int,
rows: Int,
memory: Float2Dimensional
) : AbstractMutableMatrixVf<MutableMatrixVf>(columns, rows, memory) {
constructor(columns: Int, rows: Int) : this(checkColumns(columns), checkRows(rows), Float2Dimensional(checkColumns(columns), checkRows(rows)))
override fun factorize(memory: Float2Dimensional): MutableMatrixVf {
return MutableMatrixVf(memory.columns, memory.rows, memory)
}
companion object {
/**
* Copies specified matrix into new [MutableMatrixVf]
*/
fun copy(input: IMatrixGetterFloat): MutableMatrixVf {
val memory = Float2Dimensional(input.columns, input.rows)
for (column in 0 until input.columns) {
for (row in 0 until input.rows) {
memory[column, row] = input[column, row]
}
}
return MutableMatrixVf(input.columns, input.rows, memory)
}
}
}

210
src/kvector/kotlin/ru/dbotthepony/kvector/narray/TwoDimensional.kt
View File

@ -1,210 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.narray
import ru.dbotthepony.kvector.api.*
private inline fun require(condition: Boolean, lazy: () -> Any) {
if (!condition) {
throw IndexOutOfBoundsException(lazy.invoke().toString())
}
}
/**
* Two-dimensional array of [Double]s, backed by primitive [DoubleArray].
*
* Has two constructors, one is for constructing fresh two-dimensional array, and second is for
* viewing already existing array as two-dimensional array.
*/
class Double2Dimensional(
override val columns: Int,
override val rows: Int,
@JvmField val memory: DoubleArray,
private val offset: Int = 0,
) : IMatrixGetterDouble, IMatrixSetterDouble {
init {
require(columns >= 0) { "Tried to create $columns columns" }
require(rows >= 0) { "Tried to create $rows rows" }
require(memory.size - offset >= columns * rows) { "Provided array (${memory.size}) does not satisfy demands of $offset offset, $columns columns and $rows rows" }
}
constructor(columns: Int, rows: Int) : this(columns, rows, DoubleArray(columns * rows))
override operator fun get(column: Int, row: Int): Double {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
return memory[offset + column + row * columns]
}
override operator fun set(column: Int, row: Int, value: Double) {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
memory[offset + column + row * columns] = value
}
}
/**
* Two-dimensional array of [Float]s, backed by primitive [FloatArray]
*
* Has two constructors, one is for constructing fresh two-dimensional array, and second is for
* viewing already existing array as two-dimensional array.
*/
class Float2Dimensional(
override val columns: Int,
override val rows: Int,
@JvmField val memory: FloatArray,
private val offset: Int = 0,
) : IMatrixGetterFloat, IMatrixSetterFloat {
init {
require(columns >= 0) { "Tried to create $columns columns" }
require(rows >= 0) { "Tried to create $rows rows" }
require(memory.size - offset >= columns * rows) { "Provided array (${memory.size}) does not satisfy demands of $offset offset, $columns columns and $rows rows" }
}
constructor(columns: Int, rows: Int) : this(columns, rows, FloatArray(columns * rows))
override operator fun get(column: Int, row: Int): Float {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
return memory[offset + column + row * columns]
}
override operator fun set(column: Int, row: Int, value: Float) {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
memory[offset + column + row * columns] = value
}
}
/**
* Two-dimensional array of [Byte]s, backed by primitive [ByteArray]
*
* Has two constructors, one is for constructing fresh two-dimensional array, and second is for
* viewing already existing array as two-dimensional array.
*/
class Byte2Dimensional(
override val columns: Int,
override val rows: Int,
@JvmField val memory: ByteArray,
private val offset: Int = 0,
) : IMatrixGetterByte, IMatrixSetterByte {
init {
require(columns >= 0) { "Tried to create $columns columns" }
require(rows >= 0) { "Tried to create $rows rows" }
require(memory.size - offset >= columns * rows) { "Provided array (${memory.size}) does not satisfy demands of $offset offset, $columns columns and $rows rows" }
}
constructor(columns: Int, rows: Int) : this(columns, rows, ByteArray(columns * rows))
override operator fun get(column: Int, row: Int): Byte {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
return memory[offset + column + row * columns]
}
override operator fun set(column: Int, row: Int, value: Byte) {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
memory[offset + column + row * columns] = value
}
}
/**
* Two-dimensional array of [Short]s, backed by primitive [ShortArray]
*
* Has two constructors, one is for constructing fresh two-dimensional array, and second is for
* viewing already existing array as two-dimensional array.
*/
class Short2Dimensional(
override val columns: Int,
override val rows: Int,
@JvmField val memory: ShortArray,
private val offset: Int = 0,
) : IMatrixGetterShort, IMatrixSetterShort {
init {
require(columns >= 0) { "Tried to create $columns columns" }
require(rows >= 0) { "Tried to create $rows rows" }
require(memory.size - offset >= columns * rows) { "Provided array (${memory.size}) does not satisfy demands of $offset offset, $columns columns and $rows rows" }
}
constructor(columns: Int, rows: Int) : this(columns, rows, ShortArray(columns * rows))
override operator fun get(column: Int, row: Int): Short {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
return memory[offset + column + row * columns]
}
override operator fun set(column: Int, row: Int, value: Short) {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
memory[offset + column + row * columns] = value
}
}
/**
* Two-dimensional array of [Int]s, backed by primitive [IntArray]
*
* Has two constructors, one is for constructing fresh two-dimensional array, and second is for
* viewing already existing array as two-dimensional array.
*/
class Int2Dimensional(
override val columns: Int,
override val rows: Int,
@JvmField val memory: IntArray,
private val offset: Int = 0,
) : IMatrixGetterInt, IMatrixSetterInt {
init {
require(columns >= 0) { "Tried to create $columns columns" }
require(rows >= 0) { "Tried to create $rows rows" }
require(memory.size - offset >= columns * rows) { "Provided array (${memory.size}) does not satisfy demands of $offset offset, $columns columns and $rows rows" }
}
constructor(columns: Int, rows: Int) : this(columns, rows, IntArray(columns * rows))
override operator fun get(column: Int, row: Int): Int {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
return memory[offset + column + row * columns]
}
override operator fun set(column: Int, row: Int, value: Int) {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
memory[offset + column + row * columns] = value
}
}
/**
* Two-dimensional array of [Long]s, backed by primitive [LongArray]
*
* Has two constructors, one is for constructing fresh two-dimensional array, and second is for
* viewing already existing array as two-dimensional array.
*/
class Long2Dimensional(
override val columns: Int,
override val rows: Int,
@JvmField val memory: LongArray,
private val offset: Int = 0,
) : IMatrixGetterLong, IMatrixSetterLong {
init {
require(columns >= 0) { "Tried to create $columns columns" }
require(rows >= 0) { "Tried to create $rows rows" }
require(memory.size - offset >= columns * rows) { "Provided array (${memory.size}) does not satisfy demands of $offset offset, $columns columns and $rows rows" }
}
constructor(columns: Int, rows: Int) : this(columns, rows, LongArray(columns * rows))
override operator fun get(column: Int, row: Int): Long {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
return memory[offset + column + row * columns]
}
override operator fun set(column: Int, row: Int, value: Long) {
require(column in 0 until columns) { "Column out of bounds: $column" }
require(row in 0 until rows) { "Row out of bounds: $row" }
memory[offset + column + row * columns] = value
}
}

477
src/kvector/kotlin/ru/dbotthepony/kvector/util2d/AABB.kt
View File

@ -1,477 +0,0 @@
@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),
)
}
@JvmField val ZERO = AABB(Vector2d.ZERO, Vector2d.ZERO)
}
}
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())
}

48
src/kvector/kotlin/ru/dbotthepony/kvector/vector/Color.kt
View File

@ -1,48 +0,0 @@
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 BLACK = Color(0f, 0f, 0f)
val TRANSLUCENT = Color(0f, 0f, 0f, 0f)
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>
}
}
}

464
src/kvector/kotlin/ru/dbotthepony/kvector/vector/ndouble/Vector2d.kt
View File

@ -1,464 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
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.acos
import kotlin.math.cos
import kotlin.math.sin
/**
* 2D Vector, representing two-dimensional coordinates as [Double]s
*/
open class Vector2d(
open val x: Double = 0.0,
open val y: Double = 0.0
) : AbstractVector<Vector2d>(), IFractionalVector<Vector2d>, IDoubleVector<Vector2d>, IStruct2d, IMatrixGetterDouble {
final override fun component1() = x
final override fun component2() = y
constructor(input: IStruct2d) : this(input.component1(), input.component2())
final override fun get(column: Int, row: Int): Double {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
return when (row) {
0 -> x
1 -> y
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
final override val rows: Int = 2
final override val lengthSquared: Double
get() = x * x + y * y
final override val isFinite: Boolean
get() = x.isFinite() && y.isFinite()
final override val isNaN: Boolean
get() = x.isNaN() || y.isNaN()
open val r by this::x
open val g by this::y
open val s by this::x
open val t by this::y
override fun plus(other: Vector2d): Vector2d {
return Vector2d(x + other.x, y + other.y)
}
override fun minus(other: Vector2d): Vector2d {
return Vector2d(x - other.x, y - other.y)
}
override fun times(other: Vector2d): Vector2d {
return Vector2d(x * other.x, y * other.y)
}
override fun div(other: Vector2d): Vector2d {
return Vector2d(x / other.x, y / other.y)
}
override val absoluteValue: Vector2d
get() = Vector2d(x.absoluteValue, y.absoluteValue)
override fun coerceAtMost(maximal: Vector2d): Vector2d {
return Vector2d(x.coerceAtMost(maximal.x), y.coerceAtMost(maximal.y))
}
override fun coerceAtLeast(minimal: Vector2d): Vector2d {
return Vector2d(x.coerceAtLeast(minimal.x), y.coerceAtLeast(minimal.y))
}
override fun clamp(minimal: Vector2d, maximal: Vector2d): Vector2d {
return Vector2d(x.coerceAtLeast(minimal.x).coerceAtMost(maximal.x), y.coerceAtLeast(minimal.y).coerceAtMost(maximal.y))
}
override fun unaryMinus(): Vector2d {
return Vector2d(-x, -y)
}
override fun distanceSquared(other: Vector2d): Double {
val x = x - other.x
val y = y - other.y
return x * x + y * y
}
override val normalized: Vector2d
get() {
val len = length
if (len == 0.0) {
return ZERO
}
return Vector2d(x / len, y / len)
}
override fun times(other: Double): Vector2d {
return Vector2d(x * other, y * other)
}
override fun div(other: Double): Vector2d {
return Vector2d(x / other, y / other)
}
fun times(other: Float): Vector2d {
return Vector2d(x * other, y * other)
}
fun div(other: Float): Vector2d {
return Vector2d(x / other, y / other)
}
fun times(other: Int): Vector2d {
return Vector2d(x * other, y * other)
}
fun div(other: Int): Vector2d {
return Vector2d(x / other, y / other)
}
/**
* Rotates this vector by given [angle] in radians
*
* @return rotated vector
*/
fun rotate(angle: Double): Vector2d {
val s = sin(angle)
val c = cos(angle)
return Vector2d(x * c - s * y, s * x + c * y)
}
/**
* Returns the angle in radians this unit vector points to,
* treating zero angle as [POSITIVE_X].
*
* If this vector is not normalized (not a unit vector),
* behavior of this method is undefined.
*/
fun toAngle(): Double {
val dot = dot(POSITIVE_X)
if (y > 0.0) {
return acos(dot)
} else {
return -acos(dot)
}
}
/**
* 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.
*
* @return new vector
*/
fun cross(other: Double): Vector2d {
return Vector2d(other * y, -other * x)
}
/**
* 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].
*/
fun dot(other: Vector3d): Double {
return other.x * x + other.y * y
}
/**
* Calculates scalar vector * vector, returning result as [Double].
*/
fun dot(other: Vector4d): Double {
return other.x * x + other.y * y
}
/**
* Casts components to [Float] and returns new vector as result
*/
open fun toFloatVector(): Vector2f {
return Vector2f(x.toFloat(), y.toFloat())
}
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as Vector2d
if (x != other.x) return false
if (y != other.y) return false
return true
}
override fun toString(): String {
return "[$x $y]"
}
override fun hashCode(): Int {
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)
inline val yy get() = Vector2d(y, y)
inline val xxx get() = Vector3d(x, x, x)
inline val xxy get() = Vector3d(x, x, y)
inline val xyx get() = Vector3d(x, y, x)
inline val xyy get() = Vector3d(x, y, y)
inline val yxx get() = Vector3d(y, x, x)
inline val yxy get() = Vector3d(y, x, y)
inline val yyx get() = Vector3d(y, y, x)
inline val yyy get() = Vector3d(y, y, y)
inline val xxxx get() = Vector4d(x, x, x, x)
inline val xxxy get() = Vector4d(x, x, x, y)
inline val xxyx get() = Vector4d(x, x, y, x)
inline val xxyy get() = Vector4d(x, x, y, y)
inline val xyxx get() = Vector4d(x, y, x, x)
inline val xyxy get() = Vector4d(x, y, x, y)
inline val xyyx get() = Vector4d(x, y, y, x)
inline val xyyy get() = Vector4d(x, y, y, y)
inline val yxxx get() = Vector4d(y, x, x, x)
inline val yxxy get() = Vector4d(y, x, x, y)
inline val yxyx get() = Vector4d(y, x, y, x)
inline val yxyy get() = Vector4d(y, x, y, y)
inline val yyxx get() = Vector4d(y, y, x, x)
inline val yyxy get() = Vector4d(y, y, x, y)
inline val yyyx get() = Vector4d(y, y, y, x)
inline val yyyy get() = Vector4d(y, y, y, y)
inline val rr get() = Vector2d(r, r)
inline val rg get() = Vector2d(r, g)
inline val gr get() = Vector2d(g, r)
inline val gg get() = Vector2d(g, g)
inline val rrr get() = Vector3d(r, r, r)
inline val rrg get() = Vector3d(r, r, g)
inline val rgr get() = Vector3d(r, g, r)
inline val rgg get() = Vector3d(r, g, g)
inline val grr get() = Vector3d(g, r, r)
inline val grg get() = Vector3d(g, r, g)
inline val ggr get() = Vector3d(g, g, r)
inline val ggg get() = Vector3d(g, g, g)
inline val rrrr get() = Vector4d(r, r, r, r)
inline val rrrg get() = Vector4d(r, r, r, g)
inline val rrgr get() = Vector4d(r, r, g, r)
inline val rrgg get() = Vector4d(r, r, g, g)
inline val rgrr get() = Vector4d(r, g, r, r)
inline val rgrg get() = Vector4d(r, g, r, g)
inline val rggr get() = Vector4d(r, g, g, r)
inline val rggg get() = Vector4d(r, g, g, g)
inline val grrr get() = Vector4d(g, r, r, r)
inline val grrg get() = Vector4d(g, r, r, g)
inline val grgr get() = Vector4d(g, r, g, r)
inline val grgg get() = Vector4d(g, r, g, g)
inline val ggrr get() = Vector4d(g, g, r, r)
inline val ggrg get() = Vector4d(g, g, r, g)
inline val gggr get() = Vector4d(g, g, g, r)
inline val gggg get() = Vector4d(g, g, g, g)
inline val ss get() = Vector2d(s, s)
inline val st get() = Vector2d(s, t)
inline val ts get() = Vector2d(t, s)
inline val tt get() = Vector2d(t, t)
inline val sss get() = Vector3d(s, s, s)
inline val sst get() = Vector3d(s, s, t)
inline val sts get() = Vector3d(s, t, s)
inline val stt get() = Vector3d(s, t, t)
inline val tss get() = Vector3d(t, s, s)
inline val tst get() = Vector3d(t, s, t)
inline val tts get() = Vector3d(t, t, s)
inline val ttt get() = Vector3d(t, t, t)
inline val ssss get() = Vector4d(s, s, s, s)
inline val ssst get() = Vector4d(s, s, s, t)
inline val ssts get() = Vector4d(s, s, t, s)
inline val sstt get() = Vector4d(s, s, t, t)
inline val stss get() = Vector4d(s, t, s, s)
inline val stst get() = Vector4d(s, t, s, t)
inline val stts get() = Vector4d(s, t, t, s)
inline val sttt get() = Vector4d(s, t, t, t)
inline val tsss get() = Vector4d(t, s, s, s)
inline val tsst get() = Vector4d(t, s, s, t)
inline val tsts get() = Vector4d(t, s, t, s)
inline val tstt get() = Vector4d(t, s, t, t)
inline val ttss get() = Vector4d(t, t, s, s)
inline val ttst get() = Vector4d(t, t, s, t)
inline val ttts get() = Vector4d(t, t, t, s)
inline val tttt get() = Vector4d(t, t, t, t)
companion object {
@JvmField val ZERO = Vector2d()
@JvmField val POSITIVE_X = Vector2d(x = 1.0)
@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)
}
}
operator fun Double.div(other: Vector2d): Vector2d {
return Vector2d(this / other.x, this / other.y)
}
operator fun Double.times(other: Vector2d): Vector2d {
return Vector2d(this * other.x, this * other.y)
}
/**
* Calculates 2D cross product between scalar and vector
*
* @return new vector
*/
fun Double.cross(other: Vector2d): Vector2d {
return Vector2d(-this * other.y, this * other.x)
}
/**
* Mutable 2D Vector, representing two-dimensional coordinates as [Double]s
*
* It can be safely passed around to methods which expect immutable [Vector2d], **AND** do not store it,
* as mutable operations are separated from immutable ones
*/
open class MutableVector2d(
override var x: Double = 0.0,
override var y: Double = 0.0
) : Vector2d(x, y), IMutableDoubleVector<MutableVector2d>, IMutableFractionalVector<MutableVector2d>, IMutableVector<MutableVector2d, Vector2d>, IMatrixSetterDouble {
constructor(input: IStruct2d) : this(input.component1(), input.component2())
override fun set(column: Int, row: Int, value: Double) {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
when (row) {
0 -> x = value
1 -> y = value
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
override var r by this::x
override val g by this::y
override val s by this::x
override val t by this::y
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as MutableVector2d
if (x != other.x) return false
if (y != other.y) return false
return true
}
override fun plusMut(other: Vector2d): MutableVector2d {
x += other.x
y += other.y
return this
}
override fun minusMut(other: Vector2d): MutableVector2d {
x -= other.x
y -= other.y
return this
}
override fun timesMut(other: Vector2d): MutableVector2d {
x *= other.x
y *= other.y
return this
}
override fun divMut(other: Vector2d): MutableVector2d {
x /= other.x
y /= other.y
return this
}
override fun normalize(): Double {
val len = length
if (len == 0.0) {
return len
}
x /= len
y /= len
return len
}
override fun timesMut(other: Double): MutableVector2d {
x *= other
y *= other
return this
}
override fun divMut(other: Double): MutableVector2d {
x /= other
y /= other
return this
}
/**
* Calculates 2D cross product between scalar and vector,
* writing result into this vector
*
* @return this vector
*/
fun crossMut(other: Double): Vector2d {
val x = x
val y = y
this.x = other * y
this.y = -other * x
return this
}
/**
* Returns copy of this vector as [Vector2d]
*/
fun toVector(): Vector2d {
return Vector2d(x, y)
}
}

663
src/kvector/kotlin/ru/dbotthepony/kvector/vector/ndouble/Vector3d.kt
View File

@ -1,663 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.vector.ndouble
import ru.dbotthepony.kvector.api.*
import kotlin.math.absoluteValue
/**
* 3D Vector, representing three-dimensional coordinates as [Double]s
*/
open class Vector3d(
open val x: Double = 0.0,
open val y: Double = 0.0,
open val z: Double = 0.0,
) : AbstractVector<Vector3d>(), IFractionalVector<Vector3d>, IDoubleVector<Vector3d>, IStruct3d, IMatrixGetterDouble {
final override fun component1() = x
final override fun component2() = y
final override fun component3() = z
override fun get(column: Int, row: Int): Double {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
return when (row) {
0 -> x
1 -> y
2 -> z
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
constructor(input: IStruct2d, z: Double = 0.0) : this(input.component1(), input.component2(), z)
constructor(x: Double, input: IStruct2d) : this(x, input.component1(), input.component2())
constructor(input: IStruct3d) : this(input.component1(), input.component2(), input.component3())
final override val rows: Int = 3
final override val lengthSquared: Double
get() = x * x + y * y + z * z
final override val isFinite: Boolean
get() = x.isFinite() && y.isFinite() && z.isFinite()
final override val isNaN: Boolean
get() = x.isNaN() || y.isNaN() || z.isNaN()
open val r by this::x
open val g by this::y
open val b by this::z
open val s by this::x
open val t by this::y
open val p by this::z
override fun plus(other: Vector3d): Vector3d {
return Vector3d(
x + other.x,
y + other.y,
z + other.z
)
}
override fun minus(other: Vector3d): Vector3d {
return Vector3d(x - other.x, y - other.y, z - other.z)
}
override fun times(other: Vector3d): Vector3d {
return Vector3d(x * other.x, y * other.y, z * other.z)
}
override fun div(other: Vector3d): Vector3d {
return Vector3d(x / other.x, y / other.y, z / other.z)
}
override val absoluteValue: Vector3d
get() = Vector3d(x.absoluteValue, y.absoluteValue, z.absoluteValue)
override fun coerceAtMost(maximal: Vector3d): Vector3d {
return Vector3d(
x.coerceAtMost(maximal.x),
y.coerceAtMost(maximal.y),
z.coerceAtMost(maximal.z),
)
}
override fun coerceAtLeast(minimal: Vector3d): Vector3d {
return Vector3d(
x.coerceAtLeast(minimal.x),
y.coerceAtLeast(minimal.y),
z.coerceAtLeast(minimal.z),
)
}
override fun clamp(minimal: Vector3d, maximal: Vector3d): Vector3d {
return Vector3d(
x.coerceAtLeast(minimal.x).coerceAtMost(maximal.x),
y.coerceAtLeast(minimal.y).coerceAtMost(maximal.y),
z.coerceAtLeast(minimal.z).coerceAtMost(maximal.z),
)
}
override fun unaryMinus(): Vector3d {
return Vector3d(-x, -y, -z)
}
override fun distanceSquared(other: Vector3d): Double {
val x = x - other.x
val y = y - other.y
val z = z - other.z
return x * x + y * y + z * z
}
override val normalized: Vector3d
get() {
val len = length
if (len == 0.0) {
return ZERO
}
return Vector3d(x / len, y / len, z / len)
}
override fun times(other: Double): Vector3d {
return Vector3d(x * other, y * other, z * other)
}
override fun div(other: Double): Vector3d {
return Vector3d(x / other, y / other, z / other)
}
/**
* Calculates scalar vector * vector, returning result as [Double]
*/
fun dot(other: Vector4d): Double {
return other.x * x + other.y * y + other.z * z
}
/**
* Calculates scalar vector * vector, returning result as [Double]
*/
fun dot(other: Vector3d): Double {
return other.x * x + other.y * y + other.z * z
}
/**
* Calculates scalar vector * vector, returning result as [Double]
*/
fun dot(other: Vector2d): Double {
return other.x * x + other.y * y
}
/**
* Calculates vector vector * vector, returning result as new vector
*/
fun cross(other: Vector3d): Vector3d {
return Vector3d(
y * other.z - z * other.y,
z * other.x - x * other.z,
x * other.y - y * other.x,
)
}
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as Vector3d
if (x != other.x) return false
if (y != other.y) return false
if (z != other.z) return false
return true
}
override fun toString(): String {
return "[$x $y $z]"
}
override fun hashCode(): Int {
var result = x.hashCode()
result = 31 * result + y.hashCode()
result = 31 * result + z.hashCode()
return result
}
inline val xx get() = Vector2d(x, x)
inline val xy get() = Vector2d(x, y)
inline val xz get() = Vector2d(x, z)
inline val yx get() = Vector2d(y, x)
inline val yy get() = Vector2d(y, y)
inline val yz get() = Vector2d(y, z)
inline val zx get() = Vector2d(z, x)
inline val zy get() = Vector2d(z, y)
inline val zz get() = Vector2d(z, z)
inline val xxx get() = Vector3d(x, x, x)
inline val xxy get() = Vector3d(x, x, y)
inline val xxz get() = Vector3d(x, x, z)
inline val xyx get() = Vector3d(x, y, x)
inline val xyy get() = Vector3d(x, y, y)
inline val xyz get() = Vector3d(x, y, z)
inline val xzx get() = Vector3d(x, z, x)
inline val xzy get() = Vector3d(x, z, y)
inline val xzz get() = Vector3d(x, z, z)
inline val yxx get() = Vector3d(y, x, x)
inline val yxy get() = Vector3d(y, x, y)
inline val yxz get() = Vector3d(y, x, z)
inline val yyx get() = Vector3d(y, y, x)
inline val yyy get() = Vector3d(y, y, y)
inline val yyz get() = Vector3d(y, y, z)
inline val yzx get() = Vector3d(y, z, x)
inline val yzy get() = Vector3d(y, z, y)
inline val yzz get() = Vector3d(y, z, z)
inline val zxx get() = Vector3d(z, x, x)
inline val zxy get() = Vector3d(z, x, y)
inline val zxz get() = Vector3d(z, x, z)
inline val zyx get() = Vector3d(z, y, x)
inline val zyy get() = Vector3d(z, y, y)
inline val zyz get() = Vector3d(z, y, z)
inline val zzx get() = Vector3d(z, z, x)
inline val zzy get() = Vector3d(z, z, y)
inline val zzz get() = Vector3d(z, z, z)
inline val xxxx get() = Vector4d(x, x, x, x)
inline val xxxy get() = Vector4d(x, x, x, y)
inline val xxxz get() = Vector4d(x, x, x, z)
inline val xxyx get() = Vector4d(x, x, y, x)
inline val xxyy get() = Vector4d(x, x, y, y)
inline val xxyz get() = Vector4d(x, x, y, z)
inline val xxzx get() = Vector4d(x, x, z, x)
inline val xxzy get() = Vector4d(x, x, z, y)
inline val xxzz get() = Vector4d(x, x, z, z)
inline val xyxx get() = Vector4d(x, y, x, x)
inline val xyxy get() = Vector4d(x, y, x, y)
inline val xyxz get() = Vector4d(x, y, x, z)
inline val xyyx get() = Vector4d(x, y, y, x)
inline val xyyy get() = Vector4d(x, y, y, y)
inline val xyyz get() = Vector4d(x, y, y, z)
inline val xyzx get() = Vector4d(x, y, z, x)
inline val xyzy get() = Vector4d(x, y, z, y)
inline val xyzz get() = Vector4d(x, y, z, z)
inline val xzxx get() = Vector4d(x, z, x, x)
inline val xzxy get() = Vector4d(x, z, x, y)
inline val xzxz get() = Vector4d(x, z, x, z)
inline val xzyx get() = Vector4d(x, z, y, x)
inline val xzyy get() = Vector4d(x, z, y, y)
inline val xzyz get() = Vector4d(x, z, y, z)
inline val xzzx get() = Vector4d(x, z, z, x)
inline val xzzy get() = Vector4d(x, z, z, y)
inline val xzzz get() = Vector4d(x, z, z, z)
inline val yxxx get() = Vector4d(y, x, x, x)
inline val yxxy get() = Vector4d(y, x, x, y)
inline val yxxz get() = Vector4d(y, x, x, z)
inline val yxyx get() = Vector4d(y, x, y, x)
inline val yxyy get() = Vector4d(y, x, y, y)
inline val yxyz get() = Vector4d(y, x, y, z)
inline val yxzx get() = Vector4d(y, x, z, x)
inline val yxzy get() = Vector4d(y, x, z, y)
inline val yxzz get() = Vector4d(y, x, z, z)
inline val yyxx get() = Vector4d(y, y, x, x)
inline val yyxy get() = Vector4d(y, y, x, y)
inline val yyxz get() = Vector4d(y, y, x, z)
inline val yyyx get() = Vector4d(y, y, y, x)
inline val yyyy get() = Vector4d(y, y, y, y)
inline val yyyz get() = Vector4d(y, y, y, z)
inline val yyzx get() = Vector4d(y, y, z, x)
inline val yyzy get() = Vector4d(y, y, z, y)
inline val yyzz get() = Vector4d(y, y, z, z)
inline val yzxx get() = Vector4d(y, z, x, x)
inline val yzxy get() = Vector4d(y, z, x, y)
inline val yzxz get() = Vector4d(y, z, x, z)
inline val yzyx get() = Vector4d(y, z, y, x)
inline val yzyy get() = Vector4d(y, z, y, y)
inline val yzyz get() = Vector4d(y, z, y, z)
inline val yzzx get() = Vector4d(y, z, z, x)
inline val yzzy get() = Vector4d(y, z, z, y)
inline val yzzz get() = Vector4d(y, z, z, z)
inline val zxxx get() = Vector4d(z, x, x, x)
inline val zxxy get() = Vector4d(z, x, x, y)
inline val zxxz get() = Vector4d(z, x, x, z)
inline val zxyx get() = Vector4d(z, x, y, x)
inline val zxyy get() = Vector4d(z, x, y, y)
inline val zxyz get() = Vector4d(z, x, y, z)
inline val zxzx get() = Vector4d(z, x, z, x)
inline val zxzy get() = Vector4d(z, x, z, y)
inline val zxzz get() = Vector4d(z, x, z, z)
inline val zyxx get() = Vector4d(z, y, x, x)
inline val zyxy get() = Vector4d(z, y, x, y)
inline val zyxz get() = Vector4d(z, y, x, z)
inline val zyyx get() = Vector4d(z, y, y, x)
inline val zyyy get() = Vector4d(z, y, y, y)
inline val zyyz get() = Vector4d(z, y, y, z)
inline val zyzx get() = Vector4d(z, y, z, x)
inline val zyzy get() = Vector4d(z, y, z, y)
inline val zyzz get() = Vector4d(z, y, z, z)
inline val zzxx get() = Vector4d(z, z, x, x)
inline val zzxy get() = Vector4d(z, z, x, y)
inline val zzxz get() = Vector4d(z, z, x, z)
inline val zzyx get() = Vector4d(z, z, y, x)
inline val zzyy get() = Vector4d(z, z, y, y)
inline val zzyz get() = Vector4d(z, z, y, z)
inline val zzzx get() = Vector4d(z, z, z, x)
inline val zzzy get() = Vector4d(z, z, z, y)
inline val zzzz get() = Vector4d(z, z, z, z)
inline val rr get() = Vector2d(r, r)
inline val rg get() = Vector2d(r, g)
inline val rb get() = Vector2d(r, b)
inline val gr get() = Vector2d(g, r)
inline val gg get() = Vector2d(g, g)
inline val gb get() = Vector2d(g, b)
inline val br get() = Vector2d(b, r)
inline val bg get() = Vector2d(b, g)
inline val bb get() = Vector2d(b, b)
inline val rrr get() = Vector3d(r, r, r)
inline val rrg get() = Vector3d(r, r, g)
inline val rrb get() = Vector3d(r, r, b)
inline val rgr get() = Vector3d(r, g, r)
inline val rgg get() = Vector3d(r, g, g)
inline val rgb get() = Vector3d(r, g, b)
inline val rbr get() = Vector3d(r, b, r)
inline val rbg get() = Vector3d(r, b, g)
inline val rbb get() = Vector3d(r, b, b)
inline val grr get() = Vector3d(g, r, r)
inline val grg get() = Vector3d(g, r, g)
inline val grb get() = Vector3d(g, r, b)
inline val ggr get() = Vector3d(g, g, r)
inline val ggg get() = Vector3d(g, g, g)
inline val ggb get() = Vector3d(g, g, b)
inline val gbr get() = Vector3d(g, b, r)
inline val gbg get() = Vector3d(g, b, g)
inline val gbb get() = Vector3d(g, b, b)
inline val brr get() = Vector3d(b, r, r)
inline val brg get() = Vector3d(b, r, g)
inline val brb get() = Vector3d(b, r, b)
inline val bgr get() = Vector3d(b, g, r)
inline val bgg get() = Vector3d(b, g, g)
inline val bgb get() = Vector3d(b, g, b)
inline val bbr get() = Vector3d(b, b, r)
inline val bbg get() = Vector3d(b, b, g)
inline val bbb get() = Vector3d(b, b, b)
inline val rrrr get() = Vector4d(r, r, r, r)
inline val rrrg get() = Vector4d(r, r, r, g)
inline val rrrb get() = Vector4d(r, r, r, b)
inline val rrgr get() = Vector4d(r, r, g, r)
inline val rrgg get() = Vector4d(r, r, g, g)
inline val rrgb get() = Vector4d(r, r, g, b)
inline val rrbr get() = Vector4d(r, r, b, r)
inline val rrbg get() = Vector4d(r, r, b, g)
inline val rrbb get() = Vector4d(r, r, b, b)
inline val rgrr get() = Vector4d(r, g, r, r)
inline val rgrg get() = Vector4d(r, g, r, g)
inline val rgrb get() = Vector4d(r, g, r, b)
inline val rggr get() = Vector4d(r, g, g, r)
inline val rggg get() = Vector4d(r, g, g, g)
inline val rggb get() = Vector4d(r, g, g, b)
inline val rgbr get() = Vector4d(r, g, b, r)
inline val rgbg get() = Vector4d(r, g, b, g)
inline val rgbb get() = Vector4d(r, g, b, b)
inline val rbrr get() = Vector4d(r, b, r, r)
inline val rbrg get() = Vector4d(r, b, r, g)
inline val rbrb get() = Vector4d(r, b, r, b)
inline val rbgr get() = Vector4d(r, b, g, r)
inline val rbgg get() = Vector4d(r, b, g, g)
inline val rbgb get() = Vector4d(r, b, g, b)
inline val rbbr get() = Vector4d(r, b, b, r)
inline val rbbg get() = Vector4d(r, b, b, g)
inline val rbbb get() = Vector4d(r, b, b, b)
inline val grrr get() = Vector4d(g, r, r, r)
inline val grrg get() = Vector4d(g, r, r, g)
inline val grrb get() = Vector4d(g, r, r, b)
inline val grgr get() = Vector4d(g, r, g, r)
inline val grgg get() = Vector4d(g, r, g, g)
inline val grgb get() = Vector4d(g, r, g, b)
inline val grbr get() = Vector4d(g, r, b, r)
inline val grbg get() = Vector4d(g, r, b, g)
inline val grbb get() = Vector4d(g, r, b, b)
inline val ggrr get() = Vector4d(g, g, r, r)
inline val ggrg get() = Vector4d(g, g, r, g)
inline val ggrb get() = Vector4d(g, g, r, b)
inline val gggr get() = Vector4d(g, g, g, r)
inline val gggg get() = Vector4d(g, g, g, g)
inline val gggb get() = Vector4d(g, g, g, b)
inline val ggbr get() = Vector4d(g, g, b, r)
inline val ggbg get() = Vector4d(g, g, b, g)
inline val ggbb get() = Vector4d(g, g, b, b)
inline val gbrr get() = Vector4d(g, b, r, r)
inline val gbrg get() = Vector4d(g, b, r, g)
inline val gbrb get() = Vector4d(g, b, r, b)
inline val gbgr get() = Vector4d(g, b, g, r)
inline val gbgg get() = Vector4d(g, b, g, g)
inline val gbgb get() = Vector4d(g, b, g, b)
inline val gbbr get() = Vector4d(g, b, b, r)
inline val gbbg get() = Vector4d(g, b, b, g)
inline val gbbb get() = Vector4d(g, b, b, b)
inline val brrr get() = Vector4d(b, r, r, r)
inline val brrg get() = Vector4d(b, r, r, g)
inline val brrb get() = Vector4d(b, r, r, b)
inline val brgr get() = Vector4d(b, r, g, r)
inline val brgg get() = Vector4d(b, r, g, g)
inline val brgb get() = Vector4d(b, r, g, b)
inline val brbr get() = Vector4d(b, r, b, r)
inline val brbg get() = Vector4d(b, r, b, g)
inline val brbb get() = Vector4d(b, r, b, b)
inline val bgrr get() = Vector4d(b, g, r, r)
inline val bgrg get() = Vector4d(b, g, r, g)
inline val bgrb get() = Vector4d(b, g, r, b)
inline val bggr get() = Vector4d(b, g, g, r)
inline val bggg get() = Vector4d(b, g, g, g)
inline val bggb get() = Vector4d(b, g, g, b)
inline val bgbr get() = Vector4d(b, g, b, r)
inline val bgbg get() = Vector4d(b, g, b, g)
inline val bgbb get() = Vector4d(b, g, b, b)
inline val bbrr get() = Vector4d(b, b, r, r)
inline val bbrg get() = Vector4d(b, b, r, g)
inline val bbrb get() = Vector4d(b, b, r, b)
inline val bbgr get() = Vector4d(b, b, g, r)
inline val bbgg get() = Vector4d(b, b, g, g)
inline val bbgb get() = Vector4d(b, b, g, b)
inline val bbbr get() = Vector4d(b, b, b, r)
inline val bbbg get() = Vector4d(b, b, b, g)
inline val bbbb get() = Vector4d(b, b, b, b)
inline val ss get() = Vector2d(s, s)
inline val st get() = Vector2d(s, t)
inline val sp get() = Vector2d(s, p)
inline val ts get() = Vector2d(t, s)
inline val tt get() = Vector2d(t, t)
inline val tp get() = Vector2d(t, p)
inline val ps get() = Vector2d(p, s)
inline val pt get() = Vector2d(p, t)
inline val pp get() = Vector2d(p, p)
inline val sss get() = Vector3d(s, s, s)
inline val sst get() = Vector3d(s, s, t)
inline val ssp get() = Vector3d(s, s, p)
inline val sts get() = Vector3d(s, t, s)
inline val stt get() = Vector3d(s, t, t)
inline val stp get() = Vector3d(s, t, p)
inline val sps get() = Vector3d(s, p, s)
inline val spt get() = Vector3d(s, p, t)
inline val spp get() = Vector3d(s, p, p)
inline val tss get() = Vector3d(t, s, s)
inline val tst get() = Vector3d(t, s, t)
inline val tsp get() = Vector3d(t, s, p)
inline val tts get() = Vector3d(t, t, s)
inline val ttt get() = Vector3d(t, t, t)
inline val ttp get() = Vector3d(t, t, p)
inline val tps get() = Vector3d(t, p, s)
inline val tpt get() = Vector3d(t, p, t)
inline val tpp get() = Vector3d(t, p, p)
inline val pss get() = Vector3d(p, s, s)
inline val pst get() = Vector3d(p, s, t)
inline val psp get() = Vector3d(p, s, p)
inline val pts get() = Vector3d(p, t, s)
inline val ptt get() = Vector3d(p, t, t)
inline val ptp get() = Vector3d(p, t, p)
inline val pps get() = Vector3d(p, p, s)
inline val ppt get() = Vector3d(p, p, t)
inline val ppp get() = Vector3d(p, p, p)
inline val ssss get() = Vector4d(s, s, s, s)
inline val ssst get() = Vector4d(s, s, s, t)
inline val sssp get() = Vector4d(s, s, s, p)
inline val ssts get() = Vector4d(s, s, t, s)
inline val sstt get() = Vector4d(s, s, t, t)
inline val sstp get() = Vector4d(s, s, t, p)
inline val ssps get() = Vector4d(s, s, p, s)
inline val sspt get() = Vector4d(s, s, p, t)
inline val sspp get() = Vector4d(s, s, p, p)
inline val stss get() = Vector4d(s, t, s, s)
inline val stst get() = Vector4d(s, t, s, t)
inline val stsp get() = Vector4d(s, t, s, p)
inline val stts get() = Vector4d(s, t, t, s)
inline val sttt get() = Vector4d(s, t, t, t)
inline val sttp get() = Vector4d(s, t, t, p)
inline val stps get() = Vector4d(s, t, p, s)
inline val stpt get() = Vector4d(s, t, p, t)
inline val stpp get() = Vector4d(s, t, p, p)
inline val spss get() = Vector4d(s, p, s, s)
inline val spst get() = Vector4d(s, p, s, t)
inline val spsp get() = Vector4d(s, p, s, p)
inline val spts get() = Vector4d(s, p, t, s)
inline val sptt get() = Vector4d(s, p, t, t)
inline val sptp get() = Vector4d(s, p, t, p)
inline val spps get() = Vector4d(s, p, p, s)
inline val sppt get() = Vector4d(s, p, p, t)
inline val sppp get() = Vector4d(s, p, p, p)
inline val tsss get() = Vector4d(t, s, s, s)
inline val tsst get() = Vector4d(t, s, s, t)
inline val tssp get() = Vector4d(t, s, s, p)
inline val tsts get() = Vector4d(t, s, t, s)
inline val tstt get() = Vector4d(t, s, t, t)
inline val tstp get() = Vector4d(t, s, t, p)
inline val tsps get() = Vector4d(t, s, p, s)
inline val tspt get() = Vector4d(t, s, p, t)
inline val tspp get() = Vector4d(t, s, p, p)
inline val ttss get() = Vector4d(t, t, s, s)
inline val ttst get() = Vector4d(t, t, s, t)
inline val ttsp get() = Vector4d(t, t, s, p)
inline val ttts get() = Vector4d(t, t, t, s)
inline val tttt get() = Vector4d(t, t, t, t)
inline val tttp get() = Vector4d(t, t, t, p)
inline val ttps get() = Vector4d(t, t, p, s)
inline val ttpt get() = Vector4d(t, t, p, t)
inline val ttpp get() = Vector4d(t, t, p, p)
inline val tpss get() = Vector4d(t, p, s, s)
inline val tpst get() = Vector4d(t, p, s, t)
inline val tpsp get() = Vector4d(t, p, s, p)
inline val tpts get() = Vector4d(t, p, t, s)
inline val tptt get() = Vector4d(t, p, t, t)
inline val tptp get() = Vector4d(t, p, t, p)
inline val tpps get() = Vector4d(t, p, p, s)
inline val tppt get() = Vector4d(t, p, p, t)
inline val tppp get() = Vector4d(t, p, p, p)
inline val psss get() = Vector4d(p, s, s, s)
inline val psst get() = Vector4d(p, s, s, t)
inline val pssp get() = Vector4d(p, s, s, p)
inline val psts get() = Vector4d(p, s, t, s)
inline val pstt get() = Vector4d(p, s, t, t)
inline val pstp get() = Vector4d(p, s, t, p)
inline val psps get() = Vector4d(p, s, p, s)
inline val pspt get() = Vector4d(p, s, p, t)
inline val pspp get() = Vector4d(p, s, p, p)
inline val ptss get() = Vector4d(p, t, s, s)
inline val ptst get() = Vector4d(p, t, s, t)
inline val ptsp get() = Vector4d(p, t, s, p)
inline val ptts get() = Vector4d(p, t, t, s)
inline val pttt get() = Vector4d(p, t, t, t)
inline val pttp get() = Vector4d(p, t, t, p)
inline val ptps get() = Vector4d(p, t, p, s)
inline val ptpt get() = Vector4d(p, t, p, t)
inline val ptpp get() = Vector4d(p, t, p, p)
inline val ppss get() = Vector4d(p, p, s, s)
inline val ppst get() = Vector4d(p, p, s, t)
inline val ppsp get() = Vector4d(p, p, s, p)
inline val ppts get() = Vector4d(p, p, t, s)
inline val pptt get() = Vector4d(p, p, t, t)
inline val pptp get() = Vector4d(p, p, t, p)
inline val ppps get() = Vector4d(p, p, p, s)
inline val pppt get() = Vector4d(p, p, p, t)
inline val pppp get() = Vector4d(p, p, p, p)
companion object {
@JvmField val ZERO = Vector3d()
@JvmField val POSITIVE_X = Vector3d(x = 1.0)
@JvmField val NEGATIVE_X = Vector3d(x = -1.0)
@JvmField val POSITIVE_Y = Vector3d(y = 1.0)
@JvmField val NEGATIVE_Y = Vector3d(y = -1.0)
@JvmField val POSITIVE_Z = Vector3d(z = 1.0)
@JvmField val NEGATIVE_Z = Vector3d(z = -1.0)
}
}
operator fun Double.div(other: Vector3d): Vector3d {
return Vector3d(this / other.x, this / other.y, this / other.z)
}
operator fun Double.times(other: Vector3d): Vector3d {
return Vector3d(this * other.x, this * other.y, this * other.z)
}
/**
* Mutable 3D Vector, representing three-dimensional coordinates as [Double]s
*
* It can be safely passed around to methods which expect immutable [Vector3d], **AND** do not store it,
* as mutable operations are separated from immutable ones
*/
open class MutableVector3d(
override var x: Double = 0.0,
override var y: Double = 0.0,
override var z: Double = 0.0,
) : Vector3d(x, y, z), IMutableDoubleVector<MutableVector3d>, IMutableFractionalVector<MutableVector3d>, IMutableVector<MutableVector3d, Vector3d>, IMatrixSetterDouble {
constructor(input: IStruct2d, z: Double = 0.0) : this(input.component1(), input.component2(), z)
constructor(x: Double, input: IStruct2d) : this(x, input.component1(), input.component2())
constructor(input: IStruct3d) : this(input.component1(), input.component2(), input.component3())
override fun set(column: Int, row: Int, value: Double) {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
when (row) {
0 -> x = value
1 -> y = value
2 -> z = value
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
override var r by this::x
override var g by this::y
override var b by this::z
override var s by this::x
override var t by this::y
override var p by this::z
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as Vector3d
if (x != other.x) return false
if (y != other.y) return false
if (z != other.z) return false
return true
}
override fun normalize(): Double {
val len = length
if (len == 0.0) {
return 0.0
}
x /= len
y /= len
z /= len
return len
}
override fun plusMut(other: Vector3d): MutableVector3d {
x += other.x
y += other.y
z += other.z
return this
}
override fun minusMut(other: Vector3d): MutableVector3d {
x -= other.x
y -= other.y
z -= other.z
return this
}
override fun timesMut(other: Vector3d): MutableVector3d {
x *= other.x
y *= other.y
z *= other.z
return this
}
override fun divMut(other: Vector3d): MutableVector3d {
x /= other.x
y /= other.y
z /= other.z
return this
}
override fun timesMut(other: Double): MutableVector3d {
x *= other
y *= other
z *= other
return this
}
override fun divMut(other: Double): MutableVector3d {
x /= other
y /= other
z /= other
return this
}
}

1372
src/kvector/kotlin/ru/dbotthepony/kvector/vector/ndouble/Vector4d.kt
View File

File diff suppressed because it is too large Load Diff

441
src/kvector/kotlin/ru/dbotthepony/kvector/vector/nfloat/Vector2f.kt
View File

@ -1,441 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.vector.nfloat
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
import ru.dbotthepony.kvector.vector.ndouble.Vector2d.Companion.POSITIVE_X
import kotlin.math.absoluteValue
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sin
/**
* 2D Vector, representing two-dimensional coordinates as [Float]s
*/
open class Vector2f(
open val x: Float = 0f,
open val y: Float = 0f
) : AbstractVector<Vector2f>(), IFractionalVector<Vector2f>, IFloatVector<Vector2f>, IStruct2f, IMatrixGetterFloat {
final override fun component1() = x
final override fun component2() = y
constructor(input: IStruct2f) : this(input.component1(), input.component2())
final override fun get(column: Int, row: Int): Float {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
return when (row) {
0 -> x
1 -> y
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
final override val rows: Int = 2
final override val lengthSquared: Double
get() = x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble()
final override val isFinite: Boolean
get() = x.isFinite() && y.isFinite()
final override val isNaN: Boolean
get() = x.isNaN() || y.isNaN()
open val r by this::x
open val g by this::y
open val s by this::x
open val t by this::y
override fun plus(other: Vector2f): Vector2f {
return Vector2f(
x + other.x,
y + other.y,
)
}
override fun minus(other: Vector2f): Vector2f {
return Vector2f(
x - other.x,
y - other.y,
)
}
override fun times(other: Vector2f): Vector2f {
return Vector2f(
x * other.x,
y * other.y,
)
}
override fun div(other: Vector2f): Vector2f {
return Vector2f(
x / other.x,
y / other.y,
)
}
override val absoluteValue: Vector2f
get() = Vector2f(x.absoluteValue, y.absoluteValue)
override fun coerceAtMost(maximal: Vector2f): Vector2f {
return Vector2f(
x.coerceAtMost(maximal.x),
y.coerceAtMost(maximal.y),
)
}
override fun coerceAtLeast(minimal: Vector2f): Vector2f {
return Vector2f(
x.coerceAtLeast(minimal.x),
y.coerceAtLeast(minimal.y),
)
}
override fun clamp(minimal: Vector2f, maximal: Vector2f): Vector2f {
return Vector2f(
x.coerceAtLeast(minimal.x).coerceAtMost(maximal.x),
y.coerceAtLeast(minimal.y).coerceAtMost(maximal.y),
)
}
override fun unaryMinus(): Vector2f {
return Vector2f(-x, -y)
}
override fun distanceSquared(other: Vector2f): Double {
val x = x.toDouble() - other.x.toDouble()
val y = y.toDouble() - other.y.toDouble()
return x * x + y * y
}
override val normalized: Vector2f
get() {
val len = length
if (len == 0.0) {
return ZERO
}
return Vector2f((x / len).toFloat(), (y / len).toFloat())
}
override fun times(other: Float): Vector2f {
return Vector2f(
x * other,
y * other
)
}
override fun div(other: Float): Vector2f {
return Vector2f(
x / other,
y / other
)
}
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as Vector2f
if (x != other.x) return false
if (y != other.y) return false
return true
}
override fun hashCode(): Int {
var result = x.hashCode()
result = 31 * result + y.hashCode()
return result
}
override fun toString(): String {
return "[${x}f ${y}f]"
}
/**
* Rotates this vector by given [angle] in radians
*
* @return rotated vector
*/
fun rotate(angle: Double): Vector2f {
val s = sin(angle).toFloat()
val c = cos(angle).toFloat()
return Vector2f(x * c - s * y, s * x + c * y)
}
/**
* Returns the angle in radians this unit vector points to,
* treating zero angle as [POSITIVE_X].
*
* If this vector is not normalized (not a unit vector),
* behavior of this method is undefined.
*/
fun toAngle(): Float {
val dot = dot(POSITIVE_X)
if (y > 0.0) {
return acos(dot)
} else {
return -acos(dot)
}
}
/**
* Calculates vector vector * vector, returning result as [Double]
*/
fun cross(other: Vector2f): Double {
return x.toDouble() * other.y.toDouble() - y.toDouble() * other.x.toDouble()
}
/**
* Calculates 2D cross product between scalar and vector
*
* @return new vector
*/
fun cross(other: Float): Vector2f {
return Vector2f(other * y, -other * x)
}
/**
* Calculates scalar vector * vector, returning result as [Float],
* but using sine instead of cosine.
*
* See [dot] and this method code for better explanation.
*/
fun pseudoDot(other: Vector2f): Float {
return other.x * y + other.y * x
}
/**
* Calculates scalar vector * vector, returning result as [Float]
*/
fun dot(other: Vector2f): Float {
return other.x * x + other.y * y
}
/**
* Calculates scalar vector * vector, returning result as [Float]
*/
fun dot(other: Vector3f): Float {
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
}
/**
* Casts components to [Double] and returns new vector as result
*/
open fun toDoubleVector(): Vector2d {
return Vector2d(x.toDouble(), y.toDouble())
}
inline val xx get() = Vector2f(x, x)
inline val xy get() = Vector2f(x, y)
inline val yx get() = Vector2f(y, x)
inline val yy get() = Vector2f(y, y)
inline val xxx get() = Vector3f(x, x, x)
inline val xxy get() = Vector3f(x, x, y)
inline val xyx get() = Vector3f(x, y, x)
inline val xyy get() = Vector3f(x, y, y)
inline val yxx get() = Vector3f(y, x, x)
inline val yxy get() = Vector3f(y, x, y)
inline val yyx get() = Vector3f(y, y, x)
inline val yyy get() = Vector3f(y, y, y)
inline val xxxx get() = Vector4f(x, x, x, x)
inline val xxxy get() = Vector4f(x, x, x, y)
inline val xxyx get() = Vector4f(x, x, y, x)
inline val xxyy get() = Vector4f(x, x, y, y)
inline val xyxx get() = Vector4f(x, y, x, x)
inline val xyxy get() = Vector4f(x, y, x, y)
inline val xyyx get() = Vector4f(x, y, y, x)
inline val xyyy get() = Vector4f(x, y, y, y)
inline val yxxx get() = Vector4f(y, x, x, x)
inline val yxxy get() = Vector4f(y, x, x, y)
inline val yxyx get() = Vector4f(y, x, y, x)
inline val yxyy get() = Vector4f(y, x, y, y)
inline val yyxx get() = Vector4f(y, y, x, x)
inline val yyxy get() = Vector4f(y, y, x, y)
inline val yyyx get() = Vector4f(y, y, y, x)
inline val yyyy get() = Vector4f(y, y, y, y)
inline val rr get() = Vector2f(r, r)
inline val rg get() = Vector2f(r, g)
inline val gr get() = Vector2f(g, r)
inline val gg get() = Vector2f(g, g)
inline val rrr get() = Vector3f(r, r, r)
inline val rrg get() = Vector3f(r, r, g)
inline val rgr get() = Vector3f(r, g, r)
inline val rgg get() = Vector3f(r, g, g)
inline val grr get() = Vector3f(g, r, r)
inline val grg get() = Vector3f(g, r, g)
inline val ggr get() = Vector3f(g, g, r)
inline val ggg get() = Vector3f(g, g, g)
inline val rrrr get() = Vector4f(r, r, r, r)
inline val rrrg get() = Vector4f(r, r, r, g)
inline val rrgr get() = Vector4f(r, r, g, r)
inline val rrgg get() = Vector4f(r, r, g, g)
inline val rgrr get() = Vector4f(r, g, r, r)
inline val rgrg get() = Vector4f(r, g, r, g)
inline val rggr get() = Vector4f(r, g, g, r)
inline val rggg get() = Vector4f(r, g, g, g)
inline val grrr get() = Vector4f(g, r, r, r)
inline val grrg get() = Vector4f(g, r, r, g)
inline val grgr get() = Vector4f(g, r, g, r)
inline val grgg get() = Vector4f(g, r, g, g)
inline val ggrr get() = Vector4f(g, g, r, r)
inline val ggrg get() = Vector4f(g, g, r, g)
inline val gggr get() = Vector4f(g, g, g, r)
inline val gggg get() = Vector4f(g, g, g, g)
inline val ss get() = Vector2f(s, s)
inline val st get() = Vector2f(s, t)
inline val ts get() = Vector2f(t, s)
inline val tt get() = Vector2f(t, t)
inline val sss get() = Vector3f(s, s, s)
inline val sst get() = Vector3f(s, s, t)
inline val sts get() = Vector3f(s, t, s)
inline val stt get() = Vector3f(s, t, t)
inline val tss get() = Vector3f(t, s, s)
inline val tst get() = Vector3f(t, s, t)
inline val tts get() = Vector3f(t, t, s)
inline val ttt get() = Vector3f(t, t, t)
inline val ssss get() = Vector4f(s, s, s, s)
inline val ssst get() = Vector4f(s, s, s, t)
inline val ssts get() = Vector4f(s, s, t, s)
inline val sstt get() = Vector4f(s, s, t, t)
inline val stss get() = Vector4f(s, t, s, s)
inline val stst get() = Vector4f(s, t, s, t)
inline val stts get() = Vector4f(s, t, t, s)
inline val sttt get() = Vector4f(s, t, t, t)
inline val tsss get() = Vector4f(t, s, s, s)
inline val tsst get() = Vector4f(t, s, s, t)
inline val tsts get() = Vector4f(t, s, t, s)
inline val tstt get() = Vector4f(t, s, t, t)
inline val ttss get() = Vector4f(t, t, s, s)
inline val ttst get() = Vector4f(t, t, s, t)
inline val ttts get() = Vector4f(t, t, t, s)
inline val tttt get() = Vector4f(t, t, t, t)
companion object {
@JvmField val ZERO = Vector2f()
@JvmField val POSITIVE_X = Vector2f(x = 1f)
@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)
}
}
operator fun Float.times(other: Vector2f): Vector2f {
return Vector2f(this * other.x, this * other.y)
}
operator fun Float.div(other: Vector2f): Vector2f {
return Vector2f(this / other.x, this / other.y)
}
/**
* Mutable 2D Vector, representing two-dimensional coordinates as [Float]s
*
* It can be safely passed around to methods which expect immutable [Vector2f], **AND** do not store it,
* as mutable operations are separated from immutable ones
*/
open class MutableVector2f(
override var x: Float = 0f,
override var y: Float = 0f
) : Vector2f(x, y), IMutableFloatVector<MutableVector2f>, IMutableFractionalVector<MutableVector2f>, IMutableVector<MutableVector2f, Vector2f>, IMatrixSetterFloat {
constructor(input: IStruct2f) : this(input.component1(), input.component2())
override fun set(column: Int, row: Int, value: Float) {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
when (row) {
0 -> x = value
1 -> y = value
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
override var r by this::x
override var g by this::y
override var s by this::x
override var t by this::y
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as MutableVector2f
if (x != other.x) return false
if (y != other.y) return false
return true
}
override fun normalize(): Double {
val len = length
if (len == 0.0) {
return len
}
x = (x / len).toFloat()
y = (y / len).toFloat()
return len
}
override fun plusMut(other: Vector2f): MutableVector2f {
x += other.x
y += other.y
return this
}
override fun minusMut(other: Vector2f): MutableVector2f {
x -= other.x
y -= other.y
return this
}
override fun timesMut(other: Vector2f): MutableVector2f {
x *= other.x
y *= other.y
return this
}
override fun divMut(other: Vector2f): MutableVector2f {
x /= other.x
y /= other.y
return this
}
override fun timesMut(other: Float): MutableVector2f {
x *= other
y *= other
return this
}
override fun divMut(other: Float): MutableVector2f {
x /= other
y /= other
return this
}
}

680
src/kvector/kotlin/ru/dbotthepony/kvector/vector/nfloat/Vector3f.kt
View File

@ -1,680 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.vector.nfloat
import ru.dbotthepony.kvector.api.*
import kotlin.math.absoluteValue
/**
* 3D Vector, representing three-dimensional coordinates as [Float]s
*/
open class Vector3f(
open val x: Float = 0f,
open val y: Float = 0f,
open val z: Float = 0f,
) : AbstractVector<Vector3f>(), IFractionalVector<Vector3f>, IFloatVector<Vector3f>, IStruct3f, IMatrixGetterFloat {
final override fun component1() = x
final override fun component2() = y
final override fun component3() = z
override fun get(column: Int, row: Int): Float {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
return when (row) {
0 -> x
1 -> y
2 -> z
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
constructor(input: IStruct2f, z: Float = 0f) : this(input.component1(), input.component2(), z)
constructor(x: Float, input: IStruct2f) : this(x, input.component1(), input.component2())
constructor(input: IStruct3f) : this(input.component1(), input.component2(), input.component3())
final override val rows: Int = 3
final override val lengthSquared: Double
get() = x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble() + z.toDouble() * z.toDouble()
final override val isFinite: Boolean
get() = x.isFinite() && y.isFinite() && z.isFinite()
final override val isNaN: Boolean
get() = x.isNaN() || y.isNaN() || z.isNaN()
open val r by this::x
open val g by this::y
open val b by this::z
open val s by this::x
open val t by this::y
open val p by this::z
override fun plus(other: Vector3f): Vector3f {
return Vector3f(
x + other.x,
y + other.y,
z + other.z,
)
}
override fun minus(other: Vector3f): Vector3f {
return Vector3f(
x - other.x,
y - other.y,
z - other.z,
)
}
override fun times(other: Vector3f): Vector3f {
return Vector3f(
x * other.x,
y * other.y,
z * other.z,
)
}
override fun div(other: Vector3f): Vector3f {
return Vector3f(
x / other.x,
y / other.y,
z / other.z,
)
}
override val absoluteValue: Vector3f
get() = Vector3f(x.absoluteValue, y.absoluteValue, z.absoluteValue)
override fun coerceAtMost(maximal: Vector3f): Vector3f {
return Vector3f(
x.coerceAtMost(maximal.x),
y.coerceAtMost(maximal.y),
z.coerceAtMost(maximal.z),
)
}
override fun coerceAtLeast(minimal: Vector3f): Vector3f {
return Vector3f(
x.coerceAtLeast(minimal.x),
y.coerceAtLeast(minimal.y),
z.coerceAtLeast(minimal.z),
)
}
override fun clamp(minimal: Vector3f, maximal: Vector3f): Vector3f {
return Vector3f(
x.coerceAtLeast(minimal.x).coerceAtMost(maximal.x),
y.coerceAtLeast(minimal.y).coerceAtMost(maximal.y),
z.coerceAtLeast(minimal.z).coerceAtMost(maximal.z),
)
}
override fun unaryMinus(): Vector3f {
return Vector3f(-x, -y, -z)
}
override fun distanceSquared(other: Vector3f): Double {
val x = x.toDouble() - other.x.toDouble()
val y = y.toDouble() - other.y.toDouble()
val z = z.toDouble() - other.z.toDouble()
return x * x + y * y + z * z
}
override val normalized: Vector3f
get() {
val len = length
if (len == 0.0) {
return ZERO
}
return Vector3f(
(x / len).toFloat(),
(y / len).toFloat(),
(z / len).toFloat()
)
}
override fun times(other: Float): Vector3f {
return Vector3f(x * other, y * other, z * other)
}
override fun div(other: Float): Vector3f {
return Vector3f(x / other, y / other, z / other)
}
/**
* Calculates scalar vector * vector, returning result as [Float]
*/
fun dot(other: Vector4f): Float {
return other.x * x + other.y * y + other.z * z
}
/**
* Calculates scalar vector * vector, returning result as [Float]
*/
fun dot(other: Vector3f): Float {
return other.x * x + other.y * y + other.z * z
}
/**
* Calculates scalar vector * vector, returning result as [Float]
*/
fun dot(other: Vector2f): Float {
return other.x * x + other.y * y
}
/**
* Calculates vector vector * vector, returning result as new vector
*/
fun cross(other: Vector3f): Vector3f {
return Vector3f(
y * other.z - z * other.y,
z * other.x - x * other.z,
x * other.y - y * other.x,
)
}
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as Vector3f
if (x != other.x) return false
if (y != other.y) return false
if (z != other.z) return false
return true
}
override fun hashCode(): Int {
var result = x.hashCode()
result = 31 * result + y.hashCode()
result = 31 * result + z.hashCode()
return result
}
override fun toString(): String {
return "[${x}f ${y}f ${z}f]"
}
inline val xx get() = Vector2f(x, x)
inline val xy get() = Vector2f(x, y)
inline val xz get() = Vector2f(x, z)
inline val yx get() = Vector2f(y, x)
inline val yy get() = Vector2f(y, y)
inline val yz get() = Vector2f(y, z)
inline val zx get() = Vector2f(z, x)
inline val zy get() = Vector2f(z, y)
inline val zz get() = Vector2f(z, z)
inline val xxx get() = Vector3f(x, x, x)
inline val xxy get() = Vector3f(x, x, y)
inline val xxz get() = Vector3f(x, x, z)
inline val xyx get() = Vector3f(x, y, x)
inline val xyy get() = Vector3f(x, y, y)
inline val xyz get() = Vector3f(x, y, z)
inline val xzx get() = Vector3f(x, z, x)
inline val xzy get() = Vector3f(x, z, y)
inline val xzz get() = Vector3f(x, z, z)
inline val yxx get() = Vector3f(y, x, x)
inline val yxy get() = Vector3f(y, x, y)
inline val yxz get() = Vector3f(y, x, z)
inline val yyx get() = Vector3f(y, y, x)
inline val yyy get() = Vector3f(y, y, y)
inline val yyz get() = Vector3f(y, y, z)
inline val yzx get() = Vector3f(y, z, x)
inline val yzy get() = Vector3f(y, z, y)
inline val yzz get() = Vector3f(y, z, z)
inline val zxx get() = Vector3f(z, x, x)
inline val zxy get() = Vector3f(z, x, y)
inline val zxz get() = Vector3f(z, x, z)
inline val zyx get() = Vector3f(z, y, x)
inline val zyy get() = Vector3f(z, y, y)
inline val zyz get() = Vector3f(z, y, z)
inline val zzx get() = Vector3f(z, z, x)
inline val zzy get() = Vector3f(z, z, y)
inline val zzz get() = Vector3f(z, z, z)
inline val xxxx get() = Vector4f(x, x, x, x)
inline val xxxy get() = Vector4f(x, x, x, y)
inline val xxxz get() = Vector4f(x, x, x, z)
inline val xxyx get() = Vector4f(x, x, y, x)
inline val xxyy get() = Vector4f(x, x, y, y)
inline val xxyz get() = Vector4f(x, x, y, z)
inline val xxzx get() = Vector4f(x, x, z, x)
inline val xxzy get() = Vector4f(x, x, z, y)
inline val xxzz get() = Vector4f(x, x, z, z)
inline val xyxx get() = Vector4f(x, y, x, x)
inline val xyxy get() = Vector4f(x, y, x, y)
inline val xyxz get() = Vector4f(x, y, x, z)
inline val xyyx get() = Vector4f(x, y, y, x)
inline val xyyy get() = Vector4f(x, y, y, y)
inline val xyyz get() = Vector4f(x, y, y, z)
inline val xyzx get() = Vector4f(x, y, z, x)
inline val xyzy get() = Vector4f(x, y, z, y)
inline val xyzz get() = Vector4f(x, y, z, z)
inline val xzxx get() = Vector4f(x, z, x, x)
inline val xzxy get() = Vector4f(x, z, x, y)
inline val xzxz get() = Vector4f(x, z, x, z)
inline val xzyx get() = Vector4f(x, z, y, x)
inline val xzyy get() = Vector4f(x, z, y, y)
inline val xzyz get() = Vector4f(x, z, y, z)
inline val xzzx get() = Vector4f(x, z, z, x)
inline val xzzy get() = Vector4f(x, z, z, y)
inline val xzzz get() = Vector4f(x, z, z, z)
inline val yxxx get() = Vector4f(y, x, x, x)
inline val yxxy get() = Vector4f(y, x, x, y)
inline val yxxz get() = Vector4f(y, x, x, z)
inline val yxyx get() = Vector4f(y, x, y, x)
inline val yxyy get() = Vector4f(y, x, y, y)
inline val yxyz get() = Vector4f(y, x, y, z)
inline val yxzx get() = Vector4f(y, x, z, x)
inline val yxzy get() = Vector4f(y, x, z, y)
inline val yxzz get() = Vector4f(y, x, z, z)
inline val yyxx get() = Vector4f(y, y, x, x)
inline val yyxy get() = Vector4f(y, y, x, y)
inline val yyxz get() = Vector4f(y, y, x, z)
inline val yyyx get() = Vector4f(y, y, y, x)
inline val yyyy get() = Vector4f(y, y, y, y)
inline val yyyz get() = Vector4f(y, y, y, z)
inline val yyzx get() = Vector4f(y, y, z, x)
inline val yyzy get() = Vector4f(y, y, z, y)
inline val yyzz get() = Vector4f(y, y, z, z)
inline val yzxx get() = Vector4f(y, z, x, x)
inline val yzxy get() = Vector4f(y, z, x, y)
inline val yzxz get() = Vector4f(y, z, x, z)
inline val yzyx get() = Vector4f(y, z, y, x)
inline val yzyy get() = Vector4f(y, z, y, y)
inline val yzyz get() = Vector4f(y, z, y, z)
inline val yzzx get() = Vector4f(y, z, z, x)
inline val yzzy get() = Vector4f(y, z, z, y)
inline val yzzz get() = Vector4f(y, z, z, z)
inline val zxxx get() = Vector4f(z, x, x, x)
inline val zxxy get() = Vector4f(z, x, x, y)
inline val zxxz get() = Vector4f(z, x, x, z)
inline val zxyx get() = Vector4f(z, x, y, x)
inline val zxyy get() = Vector4f(z, x, y, y)
inline val zxyz get() = Vector4f(z, x, y, z)
inline val zxzx get() = Vector4f(z, x, z, x)
inline val zxzy get() = Vector4f(z, x, z, y)
inline val zxzz get() = Vector4f(z, x, z, z)
inline val zyxx get() = Vector4f(z, y, x, x)
inline val zyxy get() = Vector4f(z, y, x, y)
inline val zyxz get() = Vector4f(z, y, x, z)
inline val zyyx get() = Vector4f(z, y, y, x)
inline val zyyy get() = Vector4f(z, y, y, y)
inline val zyyz get() = Vector4f(z, y, y, z)
inline val zyzx get() = Vector4f(z, y, z, x)
inline val zyzy get() = Vector4f(z, y, z, y)
inline val zyzz get() = Vector4f(z, y, z, z)
inline val zzxx get() = Vector4f(z, z, x, x)
inline val zzxy get() = Vector4f(z, z, x, y)
inline val zzxz get() = Vector4f(z, z, x, z)
inline val zzyx get() = Vector4f(z, z, y, x)
inline val zzyy get() = Vector4f(z, z, y, y)
inline val zzyz get() = Vector4f(z, z, y, z)
inline val zzzx get() = Vector4f(z, z, z, x)
inline val zzzy get() = Vector4f(z, z, z, y)
inline val zzzz get() = Vector4f(z, z, z, z)
inline val rr get() = Vector2f(r, r)
inline val rg get() = Vector2f(r, g)
inline val rb get() = Vector2f(r, b)
inline val gr get() = Vector2f(g, r)
inline val gg get() = Vector2f(g, g)
inline val gb get() = Vector2f(g, b)
inline val br get() = Vector2f(b, r)
inline val bg get() = Vector2f(b, g)
inline val bb get() = Vector2f(b, b)
inline val rrr get() = Vector3f(r, r, r)
inline val rrg get() = Vector3f(r, r, g)
inline val rrb get() = Vector3f(r, r, b)
inline val rgr get() = Vector3f(r, g, r)
inline val rgg get() = Vector3f(r, g, g)
inline val rgb get() = Vector3f(r, g, b)
inline val rbr get() = Vector3f(r, b, r)
inline val rbg get() = Vector3f(r, b, g)
inline val rbb get() = Vector3f(r, b, b)
inline val grr get() = Vector3f(g, r, r)
inline val grg get() = Vector3f(g, r, g)
inline val grb get() = Vector3f(g, r, b)
inline val ggr get() = Vector3f(g, g, r)
inline val ggg get() = Vector3f(g, g, g)
inline val ggb get() = Vector3f(g, g, b)
inline val gbr get() = Vector3f(g, b, r)
inline val gbg get() = Vector3f(g, b, g)
inline val gbb get() = Vector3f(g, b, b)
inline val brr get() = Vector3f(b, r, r)
inline val brg get() = Vector3f(b, r, g)
inline val brb get() = Vector3f(b, r, b)
inline val bgr get() = Vector3f(b, g, r)
inline val bgg get() = Vector3f(b, g, g)
inline val bgb get() = Vector3f(b, g, b)
inline val bbr get() = Vector3f(b, b, r)
inline val bbg get() = Vector3f(b, b, g)
inline val bbb get() = Vector3f(b, b, b)
inline val rrrr get() = Vector4f(r, r, r, r)
inline val rrrg get() = Vector4f(r, r, r, g)
inline val rrrb get() = Vector4f(r, r, r, b)
inline val rrgr get() = Vector4f(r, r, g, r)
inline val rrgg get() = Vector4f(r, r, g, g)
inline val rrgb get() = Vector4f(r, r, g, b)
inline val rrbr get() = Vector4f(r, r, b, r)
inline val rrbg get() = Vector4f(r, r, b, g)
inline val rrbb get() = Vector4f(r, r, b, b)
inline val rgrr get() = Vector4f(r, g, r, r)
inline val rgrg get() = Vector4f(r, g, r, g)
inline val rgrb get() = Vector4f(r, g, r, b)
inline val rggr get() = Vector4f(r, g, g, r)
inline val rggg get() = Vector4f(r, g, g, g)
inline val rggb get() = Vector4f(r, g, g, b)
inline val rgbr get() = Vector4f(r, g, b, r)
inline val rgbg get() = Vector4f(r, g, b, g)
inline val rgbb get() = Vector4f(r, g, b, b)
inline val rbrr get() = Vector4f(r, b, r, r)
inline val rbrg get() = Vector4f(r, b, r, g)
inline val rbrb get() = Vector4f(r, b, r, b)
inline val rbgr get() = Vector4f(r, b, g, r)
inline val rbgg get() = Vector4f(r, b, g, g)
inline val rbgb get() = Vector4f(r, b, g, b)
inline val rbbr get() = Vector4f(r, b, b, r)
inline val rbbg get() = Vector4f(r, b, b, g)
inline val rbbb get() = Vector4f(r, b, b, b)
inline val grrr get() = Vector4f(g, r, r, r)
inline val grrg get() = Vector4f(g, r, r, g)
inline val grrb get() = Vector4f(g, r, r, b)
inline val grgr get() = Vector4f(g, r, g, r)
inline val grgg get() = Vector4f(g, r, g, g)
inline val grgb get() = Vector4f(g, r, g, b)
inline val grbr get() = Vector4f(g, r, b, r)
inline val grbg get() = Vector4f(g, r, b, g)
inline val grbb get() = Vector4f(g, r, b, b)
inline val ggrr get() = Vector4f(g, g, r, r)
inline val ggrg get() = Vector4f(g, g, r, g)
inline val ggrb get() = Vector4f(g, g, r, b)
inline val gggr get() = Vector4f(g, g, g, r)
inline val gggg get() = Vector4f(g, g, g, g)
inline val gggb get() = Vector4f(g, g, g, b)
inline val ggbr get() = Vector4f(g, g, b, r)
inline val ggbg get() = Vector4f(g, g, b, g)
inline val ggbb get() = Vector4f(g, g, b, b)
inline val gbrr get() = Vector4f(g, b, r, r)
inline val gbrg get() = Vector4f(g, b, r, g)
inline val gbrb get() = Vector4f(g, b, r, b)
inline val gbgr get() = Vector4f(g, b, g, r)
inline val gbgg get() = Vector4f(g, b, g, g)
inline val gbgb get() = Vector4f(g, b, g, b)
inline val gbbr get() = Vector4f(g, b, b, r)
inline val gbbg get() = Vector4f(g, b, b, g)
inline val gbbb get() = Vector4f(g, b, b, b)
inline val brrr get() = Vector4f(b, r, r, r)
inline val brrg get() = Vector4f(b, r, r, g)
inline val brrb get() = Vector4f(b, r, r, b)
inline val brgr get() = Vector4f(b, r, g, r)
inline val brgg get() = Vector4f(b, r, g, g)
inline val brgb get() = Vector4f(b, r, g, b)
inline val brbr get() = Vector4f(b, r, b, r)
inline val brbg get() = Vector4f(b, r, b, g)
inline val brbb get() = Vector4f(b, r, b, b)
inline val bgrr get() = Vector4f(b, g, r, r)
inline val bgrg get() = Vector4f(b, g, r, g)
inline val bgrb get() = Vector4f(b, g, r, b)
inline val bggr get() = Vector4f(b, g, g, r)
inline val bggg get() = Vector4f(b, g, g, g)
inline val bggb get() = Vector4f(b, g, g, b)
inline val bgbr get() = Vector4f(b, g, b, r)
inline val bgbg get() = Vector4f(b, g, b, g)
inline val bgbb get() = Vector4f(b, g, b, b)
inline val bbrr get() = Vector4f(b, b, r, r)
inline val bbrg get() = Vector4f(b, b, r, g)
inline val bbrb get() = Vector4f(b, b, r, b)
inline val bbgr get() = Vector4f(b, b, g, r)
inline val bbgg get() = Vector4f(b, b, g, g)
inline val bbgb get() = Vector4f(b, b, g, b)
inline val bbbr get() = Vector4f(b, b, b, r)
inline val bbbg get() = Vector4f(b, b, b, g)
inline val bbbb get() = Vector4f(b, b, b, b)
inline val ss get() = Vector2f(s, s)
inline val st get() = Vector2f(s, t)
inline val sp get() = Vector2f(s, p)
inline val ts get() = Vector2f(t, s)
inline val tt get() = Vector2f(t, t)
inline val tp get() = Vector2f(t, p)
inline val ps get() = Vector2f(p, s)
inline val pt get() = Vector2f(p, t)
inline val pp get() = Vector2f(p, p)
inline val sss get() = Vector3f(s, s, s)
inline val sst get() = Vector3f(s, s, t)
inline val ssp get() = Vector3f(s, s, p)
inline val sts get() = Vector3f(s, t, s)
inline val stt get() = Vector3f(s, t, t)
inline val stp get() = Vector3f(s, t, p)
inline val sps get() = Vector3f(s, p, s)
inline val spt get() = Vector3f(s, p, t)
inline val spp get() = Vector3f(s, p, p)
inline val tss get() = Vector3f(t, s, s)
inline val tst get() = Vector3f(t, s, t)
inline val tsp get() = Vector3f(t, s, p)
inline val tts get() = Vector3f(t, t, s)
inline val ttt get() = Vector3f(t, t, t)
inline val ttp get() = Vector3f(t, t, p)
inline val tps get() = Vector3f(t, p, s)
inline val tpt get() = Vector3f(t, p, t)
inline val tpp get() = Vector3f(t, p, p)
inline val pss get() = Vector3f(p, s, s)
inline val pst get() = Vector3f(p, s, t)
inline val psp get() = Vector3f(p, s, p)
inline val pts get() = Vector3f(p, t, s)
inline val ptt get() = Vector3f(p, t, t)
inline val ptp get() = Vector3f(p, t, p)
inline val pps get() = Vector3f(p, p, s)
inline val ppt get() = Vector3f(p, p, t)
inline val ppp get() = Vector3f(p, p, p)
inline val ssss get() = Vector4f(s, s, s, s)
inline val ssst get() = Vector4f(s, s, s, t)
inline val sssp get() = Vector4f(s, s, s, p)
inline val ssts get() = Vector4f(s, s, t, s)
inline val sstt get() = Vector4f(s, s, t, t)
inline val sstp get() = Vector4f(s, s, t, p)
inline val ssps get() = Vector4f(s, s, p, s)
inline val sspt get() = Vector4f(s, s, p, t)
inline val sspp get() = Vector4f(s, s, p, p)
inline val stss get() = Vector4f(s, t, s, s)
inline val stst get() = Vector4f(s, t, s, t)
inline val stsp get() = Vector4f(s, t, s, p)
inline val stts get() = Vector4f(s, t, t, s)
inline val sttt get() = Vector4f(s, t, t, t)
inline val sttp get() = Vector4f(s, t, t, p)
inline val stps get() = Vector4f(s, t, p, s)
inline val stpt get() = Vector4f(s, t, p, t)
inline val stpp get() = Vector4f(s, t, p, p)
inline val spss get() = Vector4f(s, p, s, s)
inline val spst get() = Vector4f(s, p, s, t)
inline val spsp get() = Vector4f(s, p, s, p)
inline val spts get() = Vector4f(s, p, t, s)
inline val sptt get() = Vector4f(s, p, t, t)
inline val sptp get() = Vector4f(s, p, t, p)
inline val spps get() = Vector4f(s, p, p, s)
inline val sppt get() = Vector4f(s, p, p, t)
inline val sppp get() = Vector4f(s, p, p, p)
inline val tsss get() = Vector4f(t, s, s, s)
inline val tsst get() = Vector4f(t, s, s, t)
inline val tssp get() = Vector4f(t, s, s, p)
inline val tsts get() = Vector4f(t, s, t, s)
inline val tstt get() = Vector4f(t, s, t, t)
inline val tstp get() = Vector4f(t, s, t, p)
inline val tsps get() = Vector4f(t, s, p, s)
inline val tspt get() = Vector4f(t, s, p, t)
inline val tspp get() = Vector4f(t, s, p, p)
inline val ttss get() = Vector4f(t, t, s, s)
inline val ttst get() = Vector4f(t, t, s, t)
inline val ttsp get() = Vector4f(t, t, s, p)
inline val ttts get() = Vector4f(t, t, t, s)
inline val tttt get() = Vector4f(t, t, t, t)
inline val tttp get() = Vector4f(t, t, t, p)
inline val ttps get() = Vector4f(t, t, p, s)
inline val ttpt get() = Vector4f(t, t, p, t)
inline val ttpp get() = Vector4f(t, t, p, p)
inline val tpss get() = Vector4f(t, p, s, s)
inline val tpst get() = Vector4f(t, p, s, t)
inline val tpsp get() = Vector4f(t, p, s, p)
inline val tpts get() = Vector4f(t, p, t, s)
inline val tptt get() = Vector4f(t, p, t, t)
inline val tptp get() = Vector4f(t, p, t, p)
inline val tpps get() = Vector4f(t, p, p, s)
inline val tppt get() = Vector4f(t, p, p, t)
inline val tppp get() = Vector4f(t, p, p, p)
inline val psss get() = Vector4f(p, s, s, s)
inline val psst get() = Vector4f(p, s, s, t)
inline val pssp get() = Vector4f(p, s, s, p)
inline val psts get() = Vector4f(p, s, t, s)
inline val pstt get() = Vector4f(p, s, t, t)
inline val pstp get() = Vector4f(p, s, t, p)
inline val psps get() = Vector4f(p, s, p, s)
inline val pspt get() = Vector4f(p, s, p, t)
inline val pspp get() = Vector4f(p, s, p, p)
inline val ptss get() = Vector4f(p, t, s, s)
inline val ptst get() = Vector4f(p, t, s, t)
inline val ptsp get() = Vector4f(p, t, s, p)
inline val ptts get() = Vector4f(p, t, t, s)
inline val pttt get() = Vector4f(p, t, t, t)
inline val pttp get() = Vector4f(p, t, t, p)
inline val ptps get() = Vector4f(p, t, p, s)
inline val ptpt get() = Vector4f(p, t, p, t)
inline val ptpp get() = Vector4f(p, t, p, p)
inline val ppss get() = Vector4f(p, p, s, s)
inline val ppst get() = Vector4f(p, p, s, t)
inline val ppsp get() = Vector4f(p, p, s, p)
inline val ppts get() = Vector4f(p, p, t, s)
inline val pptt get() = Vector4f(p, p, t, t)
inline val pptp get() = Vector4f(p, p, t, p)
inline val ppps get() = Vector4f(p, p, p, s)
inline val pppt get() = Vector4f(p, p, p, t)
inline val pppp get() = Vector4f(p, p, p, p)
companion object {
@JvmField val ZERO = Vector3f()
@JvmField val POSITIVE_X = Vector3f(x = 1f)
@JvmField val NEGATIVE_X = Vector3f(x = -1f)
@JvmField val POSITIVE_Y = Vector3f(y = 1f)
@JvmField val NEGATIVE_Y = Vector3f(y = -1f)
@JvmField val POSITIVE_Z = Vector3f(z = 1f)
@JvmField val NEGATIVE_Z = Vector3f(z = -1f)
}
}
operator fun Float.times(other: Vector3f): Vector3f {
return Vector3f(this * other.x, this * other.y, this * other.z)
}
operator fun Float.div(other: Vector3f): Vector3f {
return Vector3f(this / other.x, this / other.y, this / other.z)
}
/**
* Mutable 3D Vector, representing three-dimensional coordinates as [Float]s
*
* It can be safely passed around to methods which expect immutable [Vector3f], **AND** do not store it,
* as mutable operations are separated from immutable ones
*/
open class MutableVector3f(
override var x: Float = 0f,
override var y: Float = 0f,
override var z: Float = 0f,
) : Vector3f(x, y), IMutableFloatVector<MutableVector3f>, IMutableFractionalVector<MutableVector3f>, IMutableVector<MutableVector3f, Vector3f>, IMatrixSetterFloat {
constructor(input: IStruct2f, z: Float = 0f) : this(input.component1(), input.component2(), z)
constructor(x: Float, input: IStruct2f) : this(x, input.component1(), input.component2())
constructor(input: IStruct3f) : this(input.component1(), input.component2(), input.component3())
override fun set(column: Int, row: Int, value: Float) {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
when (row) {
0 -> x = value
1 -> y = value
2 -> z = value
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
override var r by this::x
override var g by this::y
override var b by this::z
override var s by this::x
override var t by this::y
override var p by this::z
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as MutableVector3f
if (x != other.x) return false
if (y != other.y) return false
if (z != other.z) return false
return true
}
override fun normalize(): Double {
val len = length
if (len == 0.0) {
return 0.0
}
x = (x / len).toFloat()
y = (y / len).toFloat()
z = (z / len).toFloat()
return len
}
override fun plusMut(other: Vector3f): MutableVector3f {
x += other.x
y += other.y
z += other.z
return this
}
override fun minusMut(other: Vector3f): MutableVector3f {
x -= other.x
y -= other.y
z -= other.z
return this
}
override fun timesMut(other: Vector3f): MutableVector3f {
x *= other.x
y *= other.y
z *= other.z
return this
}
override fun divMut(other: Vector3f): MutableVector3f {
x /= other.x
y /= other.y
z /= other.z
return this
}
override fun timesMut(other: Float): MutableVector3f {
x *= other
y *= other
z *= other
return this
}
override fun divMut(other: Float): MutableVector3f {
x /= other
y /= other
z /= other
return this
}
}

1363
src/kvector/kotlin/ru/dbotthepony/kvector/vector/nfloat/Vector4f.kt
View File

File diff suppressed because it is too large Load Diff

337
src/kvector/kotlin/ru/dbotthepony/kvector/vector/nint/Vector2i.kt
View File

@ -1,337 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.vector.nint
import ru.dbotthepony.kvector.api.*
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
import kotlin.math.absoluteValue
/**
* 2D Vector, representing two-dimensional coordinates as [Int]s
*/
open class Vector2i(
open val x: Int = 0,
open val y: Int = 0,
) : AbstractVector<Vector2i>(), IWholeVector<Vector2i>, IIntVector<Vector2i>, IStruct2i, IMatrixGetterInt {
constructor(input: IStruct2i) : this(input.component1(), input.component2())
final override fun get(column: Int, row: Int): Int {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
return when (row) {
0 -> x
1 -> y
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
final override val rows: Int = 2
final override fun component1() = x
final override fun component2() = y
open val r by this::x
open val g by this::y
open val s by this::x
open val t by this::y
final override val lengthSquared: Double
get() = (x.toLong() * x.toLong() + y.toLong() * y.toLong()).toDouble()
final override val isFinite: Boolean = true
final override val isNaN: Boolean = false
override fun plus(other: Vector2i): Vector2i {
return Vector2i(
x + other.x,
y + other.y,
)
}
override fun minus(other: Vector2i): Vector2i {
return Vector2i(
x - other.x,
y - other.y,
)
}
override fun times(other: Vector2i): Vector2i {
return Vector2i(
x * other.x,
y * other.y,
)
}
override fun div(other: Vector2i): Vector2i {
return Vector2i(
x / other.x,
y / other.y,
)
}
override val absoluteValue: Vector2i
get() = Vector2i(x.absoluteValue, y.absoluteValue)
override fun coerceAtMost(maximal: Vector2i): Vector2i {
return Vector2i(
x.coerceAtMost(maximal.x),
y.coerceAtMost(maximal.y),
)
}
override fun coerceAtLeast(minimal: Vector2i): Vector2i {
return Vector2i(
x.coerceAtLeast(minimal.x),
y.coerceAtLeast(minimal.y),
)
}
override fun clamp(minimal: Vector2i, maximal: Vector2i): Vector2i {
return Vector2i(
x.coerceAtLeast(minimal.x).coerceAtMost(maximal.x),
y.coerceAtLeast(minimal.y).coerceAtMost(maximal.y),
)
}
override fun unaryMinus(): Vector2i {
return Vector2i(-x, -y)
}
override fun distanceSquared(other: Vector2i): Double {
val x = x.toDouble() - other.x.toDouble()
val y = y.toDouble() - other.y.toDouble()
return x * x + y * y
}
override fun wholeDistanceSquared(other: Vector2i): Long {
val x = x.toLong() - other.x.toLong()
val y = y.toLong() - other.y.toLong()
return x * x + y * y
}
override fun times(other: Int): Vector2i {
return Vector2i(
x * other,
y * other,
)
}
override fun div(other: Int): Vector2i {
return Vector2i(
x / other,
y / other,
)
}
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as Vector2i
if (x != other.x) return false
if (y != other.y) return false
return true
}
override fun hashCode(): Int {
var result = x
result = 31 * result + y
return result
}
override fun toString(): String {
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)
inline val yy get() = Vector2i(y, y)
inline val xxx get() = Vector3i(x, x, x)
inline val xxy get() = Vector3i(x, x, y)
inline val xyx get() = Vector3i(x, y, x)
inline val xyy get() = Vector3i(x, y, y)
inline val yxx get() = Vector3i(y, x, x)
inline val yxy get() = Vector3i(y, x, y)
inline val yyx get() = Vector3i(y, y, x)
inline val yyy get() = Vector3i(y, y, y)
inline val xxxx get() = Vector4i(x, x, x, x)
inline val xxxy get() = Vector4i(x, x, x, y)
inline val xxyx get() = Vector4i(x, x, y, x)
inline val xxyy get() = Vector4i(x, x, y, y)
inline val xyxx get() = Vector4i(x, y, x, x)
inline val xyxy get() = Vector4i(x, y, x, y)
inline val xyyx get() = Vector4i(x, y, y, x)
inline val xyyy get() = Vector4i(x, y, y, y)
inline val yxxx get() = Vector4i(y, x, x, x)
inline val yxxy get() = Vector4i(y, x, x, y)
inline val yxyx get() = Vector4i(y, x, y, x)
inline val yxyy get() = Vector4i(y, x, y, y)
inline val yyxx get() = Vector4i(y, y, x, x)
inline val yyxy get() = Vector4i(y, y, x, y)
inline val yyyx get() = Vector4i(y, y, y, x)
inline val yyyy get() = Vector4i(y, y, y, y)
inline val rr get() = Vector2i(r, r)
inline val rg get() = Vector2i(r, g)
inline val gr get() = Vector2i(g, r)
inline val gg get() = Vector2i(g, g)
inline val rrr get() = Vector3i(r, r, r)
inline val rrg get() = Vector3i(r, r, g)
inline val rgr get() = Vector3i(r, g, r)
inline val rgg get() = Vector3i(r, g, g)
inline val grr get() = Vector3i(g, r, r)
inline val grg get() = Vector3i(g, r, g)
inline val ggr get() = Vector3i(g, g, r)
inline val ggg get() = Vector3i(g, g, g)
inline val rrrr get() = Vector4i(r, r, r, r)
inline val rrrg get() = Vector4i(r, r, r, g)
inline val rrgr get() = Vector4i(r, r, g, r)
inline val rrgg get() = Vector4i(r, r, g, g)
inline val rgrr get() = Vector4i(r, g, r, r)
inline val rgrg get() = Vector4i(r, g, r, g)
inline val rggr get() = Vector4i(r, g, g, r)
inline val rggg get() = Vector4i(r, g, g, g)
inline val grrr get() = Vector4i(g, r, r, r)
inline val grrg get() = Vector4i(g, r, r, g)
inline val grgr get() = Vector4i(g, r, g, r)
inline val grgg get() = Vector4i(g, r, g, g)
inline val ggrr get() = Vector4i(g, g, r, r)
inline val ggrg get() = Vector4i(g, g, r, g)
inline val gggr get() = Vector4i(g, g, g, r)
inline val gggg get() = Vector4i(g, g, g, g)
inline val ss get() = Vector2i(s, s)
inline val st get() = Vector2i(s, t)
inline val ts get() = Vector2i(t, s)
inline val tt get() = Vector2i(t, t)
inline val sss get() = Vector3i(s, s, s)
inline val sst get() = Vector3i(s, s, t)
inline val sts get() = Vector3i(s, t, s)
inline val stt get() = Vector3i(s, t, t)
inline val tss get() = Vector3i(t, s, s)
inline val tst get() = Vector3i(t, s, t)
inline val tts get() = Vector3i(t, t, s)
inline val ttt get() = Vector3i(t, t, t)
inline val ssss get() = Vector4i(s, s, s, s)
inline val ssst get() = Vector4i(s, s, s, t)
inline val ssts get() = Vector4i(s, s, t, s)
inline val sstt get() = Vector4i(s, s, t, t)
inline val stss get() = Vector4i(s, t, s, s)
inline val stst get() = Vector4i(s, t, s, t)
inline val stts get() = Vector4i(s, t, t, s)
inline val sttt get() = Vector4i(s, t, t, t)
inline val tsss get() = Vector4i(t, s, s, s)
inline val tsst get() = Vector4i(t, s, s, t)
inline val tsts get() = Vector4i(t, s, t, s)
inline val tstt get() = Vector4i(t, s, t, t)
inline val ttss get() = Vector4i(t, t, s, s)
inline val ttst get() = Vector4i(t, t, s, t)
inline val ttts get() = Vector4i(t, t, t, s)
inline val tttt get() = Vector4i(t, t, t, t)
companion object {
@JvmField val ZERO = Vector2i()
@JvmField val POSITIVE_X = Vector2i(x = 1)
@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)
}
}
operator fun Int.times(other: Vector2i): Vector2i {
return Vector2i(this * other.x, this * other.y)
}
operator fun Int.div(other: Vector2i): Vector2i {
return Vector2i(this / other.x, this / other.y)
}
/**
* Mutable 2D Vector, representing two-dimensional coordinates as [Int]s
*
* It can be safely passed around to methods which expect immutable [Vector2i], **AND** do not store it,
* as mutable operations are separated from immutable ones
*/
open class MutableVector2i(
override var x: Int = 0,
override var y: Int = 0,
) : Vector2i(x, y), IMutableIntVector<MutableVector2i>, IMutableWholeVector<MutableVector2i>, IMutableVector<MutableVector2i, Vector2i>, IMatrixSetterInt {
constructor(input: IStruct2i) : this(input.component1(), input.component2())
override fun set(column: Int, row: Int, value: Int) {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
when (row) {
0 -> x = value
1 -> y = value
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
override var r by this::x
override var g by this::y
override var s by this::x
override var t by this::y
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as MutableVector2i
if (x != other.x) return false
if (y != other.y) return false
return true
}
override fun plusMut(other: Vector2i): MutableVector2i {
x += other.x
y += other.y
return this
}
override fun minusMut(other: Vector2i): MutableVector2i {
x -= other.x
y -= other.y
return this
}
override fun timesMut(other: Vector2i): MutableVector2i {
x *= other.x
y *= other.y
return this
}
override fun divMut(other: Vector2i): MutableVector2i {
x /= other.x
y /= other.y
return this
}
override fun timesMut(other: Int): MutableVector2i {
x *= other
y *= other
return this
}
override fun divMut(other: Int): MutableVector2i {
x /= other
y /= other
return this
}
}

632
src/kvector/kotlin/ru/dbotthepony/kvector/vector/nint/Vector3i.kt
View File

@ -1,632 +0,0 @@
@file:Suppress("nothing_to_inline", "unused")
package ru.dbotthepony.kvector.vector.nint
import ru.dbotthepony.kvector.api.*
import kotlin.math.absoluteValue
/**
* 3D Vector, representing three-dimensional coordinates as [Int]s
*/
open class Vector3i(
open val x: Int = 0,
open val y: Int = 0,
open val z: Int = 0,
) : AbstractVector<Vector3i>(), IWholeVector<Vector3i>, IIntVector<Vector3i>, IStruct3i, IMatrixGetterInt {
constructor(input: IStruct2i, z: Int = 0) : this(input.component1(), input.component2(), z)
constructor(x: Int, input: IStruct2i) : this(x, input.component1(), input.component2())
constructor(input: IStruct3i) : this(input.component1(), input.component2(), input.component3())
override fun get(column: Int, row: Int): Int {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
return when (row) {
0 -> x
1 -> y
2 -> z
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
final override val rows: Int = 3
final override fun component1() = x
final override fun component2() = y
final override fun component3() = z
open val r by this::x
open val g by this::y
open val b by this::z
open val s by this::x
open val t by this::y
open val p by this::z
final override val lengthSquared: Double
get() = (x.toLong() * x.toLong() + y.toLong() * y.toLong() + z.toLong() * z.toLong()).toDouble()
final override val isFinite: Boolean = true
final override val isNaN: Boolean = false
override fun plus(other: Vector3i): Vector3i {
return Vector3i(
x + other.x,
y + other.y,
z + other.z,
)
}
override fun minus(other: Vector3i): Vector3i {
return Vector3i(
x - other.x,
y - other.y,
z - other.z,
)
}
override fun times(other: Vector3i): Vector3i {
return Vector3i(
x * other.x,
y * other.y,
z * other.z,
)
}
override fun div(other: Vector3i): Vector3i {
return Vector3i(
x / other.x,
y / other.y,
z / other.z,
)
}
override val absoluteValue: Vector3i
get() = Vector3i(x.absoluteValue, y.absoluteValue, z.absoluteValue)
override fun coerceAtMost(maximal: Vector3i): Vector3i {
return Vector3i(
x.coerceAtMost(maximal.x),
y.coerceAtMost(maximal.y),
z.coerceAtMost(maximal.z),
)
}
override fun coerceAtLeast(minimal: Vector3i): Vector3i {
return Vector3i(
x.coerceAtLeast(minimal.x),
y.coerceAtLeast(minimal.y),
z.coerceAtLeast(minimal.z),
)
}
override fun clamp(minimal: Vector3i, maximal: Vector3i): Vector3i {
return Vector3i(
x.coerceAtLeast(minimal.x).coerceAtMost(maximal.x),
y.coerceAtLeast(minimal.y).coerceAtMost(maximal.y),
z.coerceAtLeast(minimal.z).coerceAtMost(maximal.z),
)
}
override fun unaryMinus(): Vector3i {
return Vector3i(-x, -y, -z)
}
override fun distanceSquared(other: Vector3i): Double {
val x = x.toDouble() - other.x.toDouble()
val y = y.toDouble() - other.y.toDouble()
val z = z.toDouble() - other.z.toDouble()
return x * x + y * y + z * z
}
override fun wholeDistanceSquared(other: Vector3i): Long {
val x = x.toLong() - other.x.toLong()
val y = y.toLong() - other.y.toLong()
val z = z.toLong() - other.z.toLong()
return x * x + y * y + z * z
}
override fun times(other: Int): Vector3i {
return Vector3i(
x * other,
y * other,
z * other,
)
}
override fun div(other: Int): Vector3i {
return Vector3i(
x / other,
y / other,
z / other,
)
}
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as Vector3i
if (x != other.x) return false
if (y != other.y) return false
if (z != other.z) return false
return true
}
override fun hashCode(): Int {
var result = x
result = 31 * result + y
result = 31 * result + z
return result
}
override fun toString(): String {
return "[${x}i ${y}i ${z}i]"
}
inline val xx get() = Vector2i(x, x)
inline val xy get() = Vector2i(x, y)
inline val xz get() = Vector2i(x, z)
inline val yx get() = Vector2i(y, x)
inline val yy get() = Vector2i(y, y)
inline val yz get() = Vector2i(y, z)
inline val zx get() = Vector2i(z, x)
inline val zy get() = Vector2i(z, y)
inline val zz get() = Vector2i(z, z)
inline val xxx get() = Vector3i(x, x, x)
inline val xxy get() = Vector3i(x, x, y)
inline val xxz get() = Vector3i(x, x, z)
inline val xyx get() = Vector3i(x, y, x)
inline val xyy get() = Vector3i(x, y, y)
inline val xyz get() = Vector3i(x, y, z)
inline val xzx get() = Vector3i(x, z, x)
inline val xzy get() = Vector3i(x, z, y)
inline val xzz get() = Vector3i(x, z, z)
inline val yxx get() = Vector3i(y, x, x)
inline val yxy get() = Vector3i(y, x, y)
inline val yxz get() = Vector3i(y, x, z)
inline val yyx get() = Vector3i(y, y, x)
inline val yyy get() = Vector3i(y, y, y)
inline val yyz get() = Vector3i(y, y, z)
inline val yzx get() = Vector3i(y, z, x)
inline val yzy get() = Vector3i(y, z, y)
inline val yzz get() = Vector3i(y, z, z)
inline val zxx get() = Vector3i(z, x, x)
inline val zxy get() = Vector3i(z, x, y)
inline val zxz get() = Vector3i(z, x, z)
inline val zyx get() = Vector3i(z, y, x)
inline val zyy get() = Vector3i(z, y, y)
inline val zyz get() = Vector3i(z, y, z)
inline val zzx get() = Vector3i(z, z, x)
inline val zzy get() = Vector3i(z, z, y)
inline val zzz get() = Vector3i(z, z, z)
inline val xxxx get() = Vector4i(x, x, x, x)
inline val xxxy get() = Vector4i(x, x, x, y)
inline val xxxz get() = Vector4i(x, x, x, z)
inline val xxyx get() = Vector4i(x, x, y, x)
inline val xxyy get() = Vector4i(x, x, y, y)
inline val xxyz get() = Vector4i(x, x, y, z)
inline val xxzx get() = Vector4i(x, x, z, x)
inline val xxzy get() = Vector4i(x, x, z, y)
inline val xxzz get() = Vector4i(x, x, z, z)
inline val xyxx get() = Vector4i(x, y, x, x)
inline val xyxy get() = Vector4i(x, y, x, y)
inline val xyxz get() = Vector4i(x, y, x, z)
inline val xyyx get() = Vector4i(x, y, y, x)
inline val xyyy get() = Vector4i(x, y, y, y)
inline val xyyz get() = Vector4i(x, y, y, z)
inline val xyzx get() = Vector4i(x, y, z, x)
inline val xyzy get() = Vector4i(x, y, z, y)
inline val xyzz get() = Vector4i(x, y, z, z)
inline val xzxx get() = Vector4i(x, z, x, x)
inline val xzxy get() = Vector4i(x, z, x, y)
inline val xzxz get() = Vector4i(x, z, x, z)
inline val xzyx get() = Vector4i(x, z, y, x)
inline val xzyy get() = Vector4i(x, z, y, y)
inline val xzyz get() = Vector4i(x, z, y, z)
inline val xzzx get() = Vector4i(x, z, z, x)
inline val xzzy get() = Vector4i(x, z, z, y)
inline val xzzz get() = Vector4i(x, z, z, z)
inline val yxxx get() = Vector4i(y, x, x, x)
inline val yxxy get() = Vector4i(y, x, x, y)
inline val yxxz get() = Vector4i(y, x, x, z)
inline val yxyx get() = Vector4i(y, x, y, x)
inline val yxyy get() = Vector4i(y, x, y, y)
inline val yxyz get() = Vector4i(y, x, y, z)
inline val yxzx get() = Vector4i(y, x, z, x)
inline val yxzy get() = Vector4i(y, x, z, y)
inline val yxzz get() = Vector4i(y, x, z, z)
inline val yyxx get() = Vector4i(y, y, x, x)
inline val yyxy get() = Vector4i(y, y, x, y)
inline val yyxz get() = Vector4i(y, y, x, z)
inline val yyyx get() = Vector4i(y, y, y, x)
inline val yyyy get() = Vector4i(y, y, y, y)
inline val yyyz get() = Vector4i(y, y, y, z)
inline val yyzx get() = Vector4i(y, y, z, x)
inline val yyzy get() = Vector4i(y, y, z, y)
inline val yyzz get() = Vector4i(y, y, z, z)
inline val yzxx get() = Vector4i(y, z, x, x)
inline val yzxy get() = Vector4i(y, z, x, y)
inline val yzxz get() = Vector4i(y, z, x, z)
inline val yzyx get() = Vector4i(y, z, y, x)
inline val yzyy get() = Vector4i(y, z, y, y)
inline val yzyz get() = Vector4i(y, z, y, z)
inline val yzzx get() = Vector4i(y, z, z, x)
inline val yzzy get() = Vector4i(y, z, z, y)
inline val yzzz get() = Vector4i(y, z, z, z)
inline val zxxx get() = Vector4i(z, x, x, x)
inline val zxxy get() = Vector4i(z, x, x, y)
inline val zxxz get() = Vector4i(z, x, x, z)
inline val zxyx get() = Vector4i(z, x, y, x)
inline val zxyy get() = Vector4i(z, x, y, y)
inline val zxyz get() = Vector4i(z, x, y, z)
inline val zxzx get() = Vector4i(z, x, z, x)
inline val zxzy get() = Vector4i(z, x, z, y)
inline val zxzz get() = Vector4i(z, x, z, z)
inline val zyxx get() = Vector4i(z, y, x, x)
inline val zyxy get() = Vector4i(z, y, x, y)
inline val zyxz get() = Vector4i(z, y, x, z)
inline val zyyx get() = Vector4i(z, y, y, x)
inline val zyyy get() = Vector4i(z, y, y, y)
inline val zyyz get() = Vector4i(z, y, y, z)
inline val zyzx get() = Vector4i(z, y, z, x)
inline val zyzy get() = Vector4i(z, y, z, y)
inline val zyzz get() = Vector4i(z, y, z, z)
inline val zzxx get() = Vector4i(z, z, x, x)
inline val zzxy get() = Vector4i(z, z, x, y)
inline val zzxz get() = Vector4i(z, z, x, z)
inline val zzyx get() = Vector4i(z, z, y, x)
inline val zzyy get() = Vector4i(z, z, y, y)
inline val zzyz get() = Vector4i(z, z, y, z)
inline val zzzx get() = Vector4i(z, z, z, x)
inline val zzzy get() = Vector4i(z, z, z, y)
inline val zzzz get() = Vector4i(z, z, z, z)
inline val rr get() = Vector2i(r, r)
inline val rg get() = Vector2i(r, g)
inline val rb get() = Vector2i(r, b)
inline val gr get() = Vector2i(g, r)
inline val gg get() = Vector2i(g, g)
inline val gb get() = Vector2i(g, b)
inline val br get() = Vector2i(b, r)
inline val bg get() = Vector2i(b, g)
inline val bb get() = Vector2i(b, b)
inline val rrr get() = Vector3i(r, r, r)
inline val rrg get() = Vector3i(r, r, g)
inline val rrb get() = Vector3i(r, r, b)
inline val rgr get() = Vector3i(r, g, r)
inline val rgg get() = Vector3i(r, g, g)
inline val rgb get() = Vector3i(r, g, b)
inline val rbr get() = Vector3i(r, b, r)
inline val rbg get() = Vector3i(r, b, g)
inline val rbb get() = Vector3i(r, b, b)
inline val grr get() = Vector3i(g, r, r)
inline val grg get() = Vector3i(g, r, g)
inline val grb get() = Vector3i(g, r, b)
inline val ggr get() = Vector3i(g, g, r)
inline val ggg get() = Vector3i(g, g, g)
inline val ggb get() = Vector3i(g, g, b)
inline val gbr get() = Vector3i(g, b, r)
inline val gbg get() = Vector3i(g, b, g)
inline val gbb get() = Vector3i(g, b, b)
inline val brr get() = Vector3i(b, r, r)
inline val brg get() = Vector3i(b, r, g)
inline val brb get() = Vector3i(b, r, b)
inline val bgr get() = Vector3i(b, g, r)
inline val bgg get() = Vector3i(b, g, g)
inline val bgb get() = Vector3i(b, g, b)
inline val bbr get() = Vector3i(b, b, r)
inline val bbg get() = Vector3i(b, b, g)
inline val bbb get() = Vector3i(b, b, b)
inline val rrrr get() = Vector4i(r, r, r, r)
inline val rrrg get() = Vector4i(r, r, r, g)
inline val rrrb get() = Vector4i(r, r, r, b)
inline val rrgr get() = Vector4i(r, r, g, r)
inline val rrgg get() = Vector4i(r, r, g, g)
inline val rrgb get() = Vector4i(r, r, g, b)
inline val rrbr get() = Vector4i(r, r, b, r)
inline val rrbg get() = Vector4i(r, r, b, g)
inline val rrbb get() = Vector4i(r, r, b, b)
inline val rgrr get() = Vector4i(r, g, r, r)
inline val rgrg get() = Vector4i(r, g, r, g)
inline val rgrb get() = Vector4i(r, g, r, b)
inline val rggr get() = Vector4i(r, g, g, r)
inline val rggg get() = Vector4i(r, g, g, g)
inline val rggb get() = Vector4i(r, g, g, b)
inline val rgbr get() = Vector4i(r, g, b, r)
inline val rgbg get() = Vector4i(r, g, b, g)
inline val rgbb get() = Vector4i(r, g, b, b)
inline val rbrr get() = Vector4i(r, b, r, r)
inline val rbrg get() = Vector4i(r, b, r, g)
inline val rbrb get() = Vector4i(r, b, r, b)
inline val rbgr get() = Vector4i(r, b, g, r)
inline val rbgg get() = Vector4i(r, b, g, g)
inline val rbgb get() = Vector4i(r, b, g, b)
inline val rbbr get() = Vector4i(r, b, b, r)
inline val rbbg get() = Vector4i(r, b, b, g)
inline val rbbb get() = Vector4i(r, b, b, b)
inline val grrr get() = Vector4i(g, r, r, r)
inline val grrg get() = Vector4i(g, r, r, g)
inline val grrb get() = Vector4i(g, r, r, b)
inline val grgr get() = Vector4i(g, r, g, r)
inline val grgg get() = Vector4i(g, r, g, g)
inline val grgb get() = Vector4i(g, r, g, b)
inline val grbr get() = Vector4i(g, r, b, r)
inline val grbg get() = Vector4i(g, r, b, g)
inline val grbb get() = Vector4i(g, r, b, b)
inline val ggrr get() = Vector4i(g, g, r, r)
inline val ggrg get() = Vector4i(g, g, r, g)
inline val ggrb get() = Vector4i(g, g, r, b)
inline val gggr get() = Vector4i(g, g, g, r)
inline val gggg get() = Vector4i(g, g, g, g)
inline val gggb get() = Vector4i(g, g, g, b)
inline val ggbr get() = Vector4i(g, g, b, r)
inline val ggbg get() = Vector4i(g, g, b, g)
inline val ggbb get() = Vector4i(g, g, b, b)
inline val gbrr get() = Vector4i(g, b, r, r)
inline val gbrg get() = Vector4i(g, b, r, g)
inline val gbrb get() = Vector4i(g, b, r, b)
inline val gbgr get() = Vector4i(g, b, g, r)
inline val gbgg get() = Vector4i(g, b, g, g)
inline val gbgb get() = Vector4i(g, b, g, b)
inline val gbbr get() = Vector4i(g, b, b, r)
inline val gbbg get() = Vector4i(g, b, b, g)
inline val gbbb get() = Vector4i(g, b, b, b)
inline val brrr get() = Vector4i(b, r, r, r)
inline val brrg get() = Vector4i(b, r, r, g)
inline val brrb get() = Vector4i(b, r, r, b)
inline val brgr get() = Vector4i(b, r, g, r)
inline val brgg get() = Vector4i(b, r, g, g)
inline val brgb get() = Vector4i(b, r, g, b)
inline val brbr get() = Vector4i(b, r, b, r)
inline val brbg get() = Vector4i(b, r, b, g)
inline val brbb get() = Vector4i(b, r, b, b)
inline val bgrr get() = Vector4i(b, g, r, r)
inline val bgrg get() = Vector4i(b, g, r, g)
inline val bgrb get() = Vector4i(b, g, r, b)
inline val bggr get() = Vector4i(b, g, g, r)
inline val bggg get() = Vector4i(b, g, g, g)
inline val bggb get() = Vector4i(b, g, g, b)
inline val bgbr get() = Vector4i(b, g, b, r)
inline val bgbg get() = Vector4i(b, g, b, g)
inline val bgbb get() = Vector4i(b, g, b, b)
inline val bbrr get() = Vector4i(b, b, r, r)
inline val bbrg get() = Vector4i(b, b, r, g)
inline val bbrb get() = Vector4i(b, b, r, b)
inline val bbgr get() = Vector4i(b, b, g, r)
inline val bbgg get() = Vector4i(b, b, g, g)
inline val bbgb get() = Vector4i(b, b, g, b)
inline val bbbr get() = Vector4i(b, b, b, r)
inline val bbbg get() = Vector4i(b, b, b, g)
inline val bbbb get() = Vector4i(b, b, b, b)
inline val ss get() = Vector2i(s, s)
inline val st get() = Vector2i(s, t)
inline val sp get() = Vector2i(s, p)
inline val ts get() = Vector2i(t, s)
inline val tt get() = Vector2i(t, t)
inline val tp get() = Vector2i(t, p)
inline val ps get() = Vector2i(p, s)
inline val pt get() = Vector2i(p, t)
inline val pp get() = Vector2i(p, p)
inline val sss get() = Vector3i(s, s, s)
inline val sst get() = Vector3i(s, s, t)
inline val ssp get() = Vector3i(s, s, p)
inline val sts get() = Vector3i(s, t, s)
inline val stt get() = Vector3i(s, t, t)
inline val stp get() = Vector3i(s, t, p)
inline val sps get() = Vector3i(s, p, s)
inline val spt get() = Vector3i(s, p, t)
inline val spp get() = Vector3i(s, p, p)
inline val tss get() = Vector3i(t, s, s)
inline val tst get() = Vector3i(t, s, t)
inline val tsp get() = Vector3i(t, s, p)
inline val tts get() = Vector3i(t, t, s)
inline val ttt get() = Vector3i(t, t, t)
inline val ttp get() = Vector3i(t, t, p)
inline val tps get() = Vector3i(t, p, s)
inline val tpt get() = Vector3i(t, p, t)
inline val tpp get() = Vector3i(t, p, p)
inline val pss get() = Vector3i(p, s, s)
inline val pst get() = Vector3i(p, s, t)
inline val psp get() = Vector3i(p, s, p)
inline val pts get() = Vector3i(p, t, s)
inline val ptt get() = Vector3i(p, t, t)
inline val ptp get() = Vector3i(p, t, p)
inline val pps get() = Vector3i(p, p, s)
inline val ppt get() = Vector3i(p, p, t)
inline val ppp get() = Vector3i(p, p, p)
inline val ssss get() = Vector4i(s, s, s, s)
inline val ssst get() = Vector4i(s, s, s, t)
inline val sssp get() = Vector4i(s, s, s, p)
inline val ssts get() = Vector4i(s, s, t, s)
inline val sstt get() = Vector4i(s, s, t, t)
inline val sstp get() = Vector4i(s, s, t, p)
inline val ssps get() = Vector4i(s, s, p, s)
inline val sspt get() = Vector4i(s, s, p, t)
inline val sspp get() = Vector4i(s, s, p, p)
inline val stss get() = Vector4i(s, t, s, s)
inline val stst get() = Vector4i(s, t, s, t)
inline val stsp get() = Vector4i(s, t, s, p)
inline val stts get() = Vector4i(s, t, t, s)
inline val sttt get() = Vector4i(s, t, t, t)
inline val sttp get() = Vector4i(s, t, t, p)
inline val stps get() = Vector4i(s, t, p, s)
inline val stpt get() = Vector4i(s, t, p, t)
inline val stpp get() = Vector4i(s, t, p, p)
inline val spss get() = Vector4i(s, p, s, s)
inline val spst get() = Vector4i(s, p, s, t)
inline val spsp get() = Vector4i(s, p, s, p)
inline val spts get() = Vector4i(s, p, t, s)
inline val sptt get() = Vector4i(s, p, t, t)
inline val sptp get() = Vector4i(s, p, t, p)
inline val spps get() = Vector4i(s, p, p, s)
inline val sppt get() = Vector4i(s, p, p, t)
inline val sppp get() = Vector4i(s, p, p, p)
inline val tsss get() = Vector4i(t, s, s, s)
inline val tsst get() = Vector4i(t, s, s, t)
inline val tssp get() = Vector4i(t, s, s, p)
inline val tsts get() = Vector4i(t, s, t, s)
inline val tstt get() = Vector4i(t, s, t, t)
inline val tstp get() = Vector4i(t, s, t, p)
inline val tsps get() = Vector4i(t, s, p, s)
inline val tspt get() = Vector4i(t, s, p, t)
inline val tspp get() = Vector4i(t, s, p, p)
inline val ttss get() = Vector4i(t, t, s, s)
inline val ttst get() = Vector4i(t, t, s, t)
inline val ttsp get() = Vector4i(t, t, s, p)
inline val ttts get() = Vector4i(t, t, t, s)
inline val tttt get() = Vector4i(t, t, t, t)
inline val tttp get() = Vector4i(t, t, t, p)
inline val ttps get() = Vector4i(t, t, p, s)
inline val ttpt get() = Vector4i(t, t, p, t)
inline val ttpp get() = Vector4i(t, t, p, p)
inline val tpss get() = Vector4i(t, p, s, s)
inline val tpst get() = Vector4i(t, p, s, t)
inline val tpsp get() = Vector4i(t, p, s, p)
inline val tpts get() = Vector4i(t, p, t, s)
inline val tptt get() = Vector4i(t, p, t, t)
inline val tptp get() = Vector4i(t, p, t, p)
inline val tpps get() = Vector4i(t, p, p, s)
inline val tppt get() = Vector4i(t, p, p, t)
inline val tppp get() = Vector4i(t, p, p, p)
inline val psss get() = Vector4i(p, s, s, s)
inline val psst get() = Vector4i(p, s, s, t)
inline val pssp get() = Vector4i(p, s, s, p)
inline val psts get() = Vector4i(p, s, t, s)
inline val pstt get() = Vector4i(p, s, t, t)
inline val pstp get() = Vector4i(p, s, t, p)
inline val psps get() = Vector4i(p, s, p, s)
inline val pspt get() = Vector4i(p, s, p, t)
inline val pspp get() = Vector4i(p, s, p, p)
inline val ptss get() = Vector4i(p, t, s, s)
inline val ptst get() = Vector4i(p, t, s, t)
inline val ptsp get() = Vector4i(p, t, s, p)
inline val ptts get() = Vector4i(p, t, t, s)
inline val pttt get() = Vector4i(p, t, t, t)
inline val pttp get() = Vector4i(p, t, t, p)
inline val ptps get() = Vector4i(p, t, p, s)
inline val ptpt get() = Vector4i(p, t, p, t)
inline val ptpp get() = Vector4i(p, t, p, p)
inline val ppss get() = Vector4i(p, p, s, s)
inline val ppst get() = Vector4i(p, p, s, t)
inline val ppsp get() = Vector4i(p, p, s, p)
inline val ppts get() = Vector4i(p, p, t, s)
inline val pptt get() = Vector4i(p, p, t, t)
inline val pptp get() = Vector4i(p, p, t, p)
inline val ppps get() = Vector4i(p, p, p, s)
inline val pppt get() = Vector4i(p, p, p, t)
inline val pppp get() = Vector4i(p, p, p, p)
companion object {
@JvmField val ZERO = Vector3i()
@JvmField val POSITIVE_X = Vector3i(x = 1)
@JvmField val NEGATIVE_X = Vector3i(x = -1)
@JvmField val POSITIVE_Y = Vector3i(y = 1)
@JvmField val NEGATIVE_Y = Vector3i(y = -1)
@JvmField val POSITIVE_Z = Vector3i(z = 1)
@JvmField val NEGATIVE_Z = Vector3i(z = -1)
}
}
operator fun Int.times(other: Vector3i): Vector3i {
return Vector3i(this * other.x, this * other.y, this * other.z)
}
operator fun Int.div(other: Vector3i): Vector3i {
return Vector3i(this / other.x, this / other.y, this / other.z)
}
/**
* Mutable 3D Vector, representing three-dimensional coordinates as [Int]s
*
* It can be safely passed around to methods which expect immutable [Vector3i], **AND** do not store it,
* as mutable operations are separated from immutable ones
*/
open class MutableVector3i(
override var x: Int = 0,
override var y: Int = 0,
override var z: Int = 0,
) : Vector3i(x, y, z), IMutableIntVector<MutableVector3i>, IMutableWholeVector<MutableVector3i>, IMutableVector<MutableVector3i, Vector3i>, IMatrixSetterInt {
constructor(input: IStruct2i, z: Int = 0) : this(input.component1(), input.component2(), z)
constructor(x: Int, input: IStruct2i) : this(x, input.component1(), input.component2())
constructor(input: IStruct3i) : this(input.component1(), input.component2(), input.component3())
override fun set(column: Int, row: Int, value: Int) {
if (column != 0) {
throw IndexOutOfBoundsException("Vectors are 1 column matrices ($column given)")
}
when (row) {
0 -> x = value
1 -> y = value
2 -> z = value
else -> throw IndexOutOfBoundsException("Row out of bounds: $row")
}
}
override var r by this::x
override var g by this::y
override var b by this::z
override var s by this::x
override var t by this::y
override var p by this::z
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (javaClass != other?.javaClass) return false
other as MutableVector3i
if (x != other.x) return false
if (y != other.y) return false
if (z != other.z) return false
return true
}
override fun plusMut(other: Vector3i): MutableVector3i {
x += other.x
y += other.y
z += other.z
return this
}
override fun minusMut(other: Vector3i): MutableVector3i {
x -= other.x
y -= other.y
z -= other.z
return this
}
override fun timesMut(other: Vector3i): MutableVector3i {
x *= other.x
y *= other.y
z *= other.z
return this
}
override fun divMut(other: Vector3i): MutableVector3i {
x /= other.x
y /= other.y
z /= other.z
return this
}
override fun timesMut(other: Int): MutableVector3i {
x *= other
y *= other
z *= other
return this
}
override fun divMut(other: Int): MutableVector3i {
x /= other
y /= other
z /= other
return this
}
}

1329
src/kvector/kotlin/ru/dbotthepony/kvector/vector/nint/Vector4i.kt
View File

File diff suppressed because it is too large Load Diff