From d715aa35a182bb19724fa417260cafe92dae0ba5 Mon Sep 17 00:00:00 2001 From: DBotThePony Date: Thu, 17 Feb 2022 11:49:50 +0700 Subject: [PATCH] KBox2D --- .../ru/dbotthepony/kbox2d/api/B2TimeStep.kt | 155 ++ .../kotlin/ru/dbotthepony/kbox2d/api/Body.kt | 524 ++++++ .../ru/dbotthepony/kbox2d/api/BroadPhase.kt | 50 + .../ru/dbotthepony/kbox2d/api/Collision.kt | 126 ++ .../ru/dbotthepony/kbox2d/api/Constants.kt | 85 + .../ru/dbotthepony/kbox2d/api/Contact.kt | 146 ++ .../dbotthepony/kbox2d/api/ContactManager.kt | 37 + .../ru/dbotthepony/kbox2d/api/Distance.kt | 72 + .../ru/dbotthepony/kbox2d/api/DynamicTree.kt | 76 + .../ru/dbotthepony/kbox2d/api/Fixture.kt | 181 ++ .../ru/dbotthepony/kbox2d/api/IDebugDraw.kt | 52 + .../ru/dbotthepony/kbox2d/api/IMovable.kt | 12 + .../ru/dbotthepony/kbox2d/api/IProxieable.kt | 76 + .../kotlin/ru/dbotthepony/kbox2d/api/Joint.kt | 773 ++++++++ .../kotlin/ru/dbotthepony/kbox2d/api/Math.kt | 510 ++++++ .../kotlin/ru/dbotthepony/kbox2d/api/Shape.kt | 60 + .../ru/dbotthepony/kbox2d/api/ShapeDefs.kt | 2 + .../ru/dbotthepony/kbox2d/api/TimeOfImpact.kt | 28 + .../kotlin/ru/dbotthepony/kbox2d/api/World.kt | 246 +++ .../dbotthepony/kbox2d/api/WorldCallbacks.kt | 139 ++ .../kbox2d/collision/BroadPhase.kt | 152 ++ .../dbotthepony/kbox2d/collision/Collision.kt | 184 ++ .../dbotthepony/kbox2d/collision/Distance.kt | 688 ++++++++ .../kbox2d/collision/DynamicTree.kt | 715 ++++++++ .../kbox2d/collision/TimeOfImpact.kt | 407 +++++ .../kbox2d/collision/handler/CollideCircle.kt | 149 ++ .../kbox2d/collision/handler/CollideEdge.kt | 469 +++++ .../collision/handler/CollidePolygon.kt | 250 +++ .../kbox2d/collision/shapes/ChainShape.kt | 191 ++ .../kbox2d/collision/shapes/CircleShape.kt | 79 + .../kbox2d/collision/shapes/EdgeShape.kt | 188 ++ .../kbox2d/collision/shapes/PolygonShape.kt | 449 +++++ .../ru/dbotthepony/kbox2d/dynamics/B2World.kt | 916 ++++++++++ .../ru/dbotthepony/kbox2d/dynamics/Body.kt | 622 +++++++ .../kbox2d/dynamics/ContactManager.kt | 193 ++ .../ru/dbotthepony/kbox2d/dynamics/Fixture.kt | 147 ++ .../dynamics/contact/AbstractContact.kt | 215 +++ .../dynamics/contact/ChainCircleContact.kt | 28 + .../dynamics/contact/ChainPolygonContact.kt | 28 + .../kbox2d/dynamics/contact/CircleContact.kt | 22 + .../dynamics/contact/EdgeCircleContact.kt | 23 + .../dynamics/contact/EdgePolygonContact.kt | 23 + .../dynamics/contact/PolygonCircleContact.kt | 23 + .../kbox2d/dynamics/contact/PolygonContact.kt | 26 + .../kbox2d/dynamics/internal/ContactSolver.kt | 820 +++++++++ .../kbox2d/dynamics/internal/Island.kt | 480 +++++ .../kbox2d/dynamics/joint/AbstractJoint.kt | 144 ++ .../kbox2d/dynamics/joint/DistanceJoint.kt | 336 ++++ .../kbox2d/dynamics/joint/FrictionJoint.kt | 210 +++ .../kbox2d/dynamics/joint/GearJoint.kt | 468 +++++ .../kbox2d/dynamics/joint/MotorJoint.kt | 225 +++ .../kbox2d/dynamics/joint/MouseJoint.kt | 150 ++ .../kbox2d/dynamics/joint/PrismaticJoint.kt | 605 +++++++ .../kbox2d/dynamics/joint/PulleyJoint.kt | 281 +++ .../kbox2d/dynamics/joint/RevoluteJoint.kt | 463 +++++ .../kbox2d/dynamics/joint/WeldJoint.kt | 281 +++ .../kbox2d/dynamics/joint/WheelJoint.kt | 534 ++++++ .../kotlin/ru/dbotthepony/kstarbound/Main.kt | 459 ++++- .../kstarbound/client/gl/GLStateTracker.kt | 23 + .../kstarbound/client/gl/VertexBuilder.kt | 25 + .../kstarbound/client/render/Box2DRenderer.kt | 128 ++ .../client/render/EntityRenderer.kt | 6 +- .../client/render/FrameSetAnimator.kt | 2 + .../ru/dbotthepony/kstarbound/math/AABB.kt | 25 +- .../dbotthepony/kstarbound/math/FastMath.kt | 24 + .../ru/dbotthepony/kstarbound/math/Matrix.kt | 1561 ++++++++++++++++- .../ru/dbotthepony/kstarbound/math/Vector.kt | 367 +++- 67 files changed, 17112 insertions(+), 42 deletions(-) create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/B2TimeStep.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/Body.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/BroadPhase.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/Collision.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/Constants.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/Contact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/ContactManager.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/Distance.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/DynamicTree.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/Fixture.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/IDebugDraw.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/IMovable.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/IProxieable.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/Joint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/Math.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/Shape.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/ShapeDefs.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/TimeOfImpact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/World.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/api/WorldCallbacks.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/BroadPhase.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/Collision.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/Distance.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/DynamicTree.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/TimeOfImpact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideCircle.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideEdge.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollidePolygon.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/ChainShape.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/CircleShape.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/EdgeShape.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/PolygonShape.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/B2World.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/Body.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/ContactManager.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/Fixture.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/AbstractContact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainCircleContact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainPolygonContact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/CircleContact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgeCircleContact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgePolygonContact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonCircleContact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonContact.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/ContactSolver.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/Island.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/AbstractJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/DistanceJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/FrictionJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/GearJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MotorJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MouseJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PrismaticJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PulleyJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/RevoluteJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WeldJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WheelJoint.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kstarbound/client/render/Box2DRenderer.kt create mode 100644 src/main/kotlin/ru/dbotthepony/kstarbound/math/FastMath.kt diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/B2TimeStep.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/B2TimeStep.kt new file mode 100644 index 00000000..fdb51783 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/B2TimeStep.kt @@ -0,0 +1,155 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.Vector2d + +/** + * Profiling data. Times are in milliseconds. + */ +interface IProfileData { + val step: Long + val collide: Long + val solve: Long + val solveInit: Long + val solveVelocity: Long + val solvePosition: Long + val broadphase: Long + val solveTOI: Long + val integratePositions: Long +} + +/** + * Profiling data. Times are in nanoseconds. + */ +data class ProfileSnapshot( + override val step: Long, + override val collide: Long, + override val solve: Long, + override val solveInit: Long, + override val solveVelocity: Long, + override val solvePosition: Long, + override val broadphase: Long, + override val solveTOI: Long, + override val integratePositions: Long, +) : IProfileData + +/** + * Profiling data. Times are in nanoseconds. + */ +internal data class ProfileData( + override var step: Long = 0L, + override var collide: Long = 0L, + override var solve: Long = 0L, + override var solveInit: Long = 0L, + override var solveVelocity: Long = 0L, + override var solvePosition: Long = 0L, + override var broadphase: Long = 0L, + override var solveTOI: Long = 0L, + override var integratePositions: Long = 0L, +) : IProfileData { + fun snapshot(): ProfileSnapshot { + return ProfileSnapshot( + step = step, + collide = collide, + solve = solve, + solveInit = solveInit, + solveVelocity = solveVelocity, + solvePosition = solvePosition, + broadphase = broadphase, + solveTOI = solveTOI, + integratePositions = integratePositions, + ) + } +} + +/** + * This is an internal structure. + */ +data class B2TimeStep( + var dt: Double, + var inv_dt: Double, + var dtRatio: Double, + var velocityIterations: Int, + var positionIterations: Int, + var warmStarting: Boolean, +) + +/** + * This is an internal structure. + */ +internal class B2Position( + c: Vector2d = Vector2d.ZERO, + a: Double = 0.0, +) { + var c: Vector2d = c + set(value) { + if (!value.isFinite) { + throw IllegalArgumentException("Tried to set illegal position $value") + } + + field = value + } + + var a: Double = a + set(value) { + if (!value.isFinite()) { + throw IllegalArgumentException("Tried to set non-finite angle $value") + } + + if (value.isNaN()) { + throw IllegalArgumentException("Tried to set NaN angle") + } + + field = value + } + + init { + // KBox2D: trigger sanity checks at least once + this.c = c + this.a = a + } +} + +/** + * This is an internal structure. + */ +internal class B2Velocity( + v: Vector2d = Vector2d.ZERO, + w: Double = 0.0, +) { + var v: Vector2d = v + set(value) { + if (!value.isFinite) { + throw IllegalArgumentException("Tried to set illegal velocity $value") + } + + field = value + } + + var w: Double = w + set(value) { + if (!value.isFinite()) { + throw IllegalArgumentException("Tried to set non-finite angular angle $value") + } + + if (value.isNaN()) { + throw IllegalArgumentException("Tried to set NaN angular angle") + } + + field = value + } + + init { + // KBox2D: trigger sanity checks at least once + this.v = v + this.w = w + } +} + +/** + * Solver Data + */ +internal class B2SolverData( + var step: B2TimeStep, + var positions: List, + var velocities: List +) diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/Body.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Body.kt new file mode 100644 index 00000000..466eb6a8 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Body.kt @@ -0,0 +1,524 @@ +package ru.dbotthepony.kbox2d.api + +/* +MIT License + +Original author Erin Catto (c) 2019 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +import ru.dbotthepony.kstarbound.math.Vector2d + +/// The body type. +/// static: zero mass, zero velocity, may be manually moved +/// kinematic: zero mass, non-zero velocity set by user, moved by solver +/// dynamic: positive mass, non-zero velocity determined by forces, moved by solver +enum class BodyType { + STATIC, + KINEMATIC, + DYNAMIC +} + +/// A body definition holds all the data needed to construct a rigid body. +/// You can safely re-use body definitions. Shapes are added to a body after construction. +data class BodyDef( + /** + * The world position of the body. Avoid creating bodies at the origin + * since this can lead to many overlapping shapes. + */ + var position: Vector2d = Vector2d.ZERO, + + /** + * The world angle of the body in radians. + */ + var angle: Double = 0.0, + + /** + * The linear velocity of the body's origin in world co-ordinates. + */ + var linearVelocity: Vector2d = Vector2d.ZERO, + + /** + * The angular velocity of the body. + */ + var angularVelocity: Double = 0.0, + + /** + * Linear damping is use to reduce the linear velocity. The damping parameter + * can be larger than 1.0f but the damping effect becomes sensitive to the + * time step when the damping parameter is large. + * Units are 1/time + */ + var linearDamping: Double = 0.0, + + /** + * Angular damping is use to reduce the angular velocity. The damping parameter + * can be larger than 1.0f but the damping effect becomes sensitive to the + * time step when the damping parameter is large. + * Units are 1/time + */ + var angularDamping: Double = 0.0, + + /** + * Set this flag to false if this body should never fall asleep. Note that + * this increases CPU usage. + */ + var allowSleep: Boolean = true, + + /** + * Is this body initially awake or sleeping? + */ + var awake: Boolean = true, + + /** + * Should this body be prevented from rotating? Useful for characters. + */ + var fixedRotation: Boolean = false, + + /** + * Is this a fast moving body that should be prevented from tunneling through + * other moving bodies? Note that all bodies are prevented from tunneling through + * kinematic and static bodies. This setting is only considered on dynamic bodies. + * @warning You should use this flag sparingly since it increases processing time. + */ + var bullet: Boolean = false, + + /** + * The body type: static, kinematic, or dynamic. + * Note: if a dynamic body would have zero mass, the mass is set to one. + */ + var type: BodyType = BodyType.STATIC, + + /** + * Does this body start out enabled? + */ + var enabled: Boolean = true, + + /** + * Use this to store application specific body data. + */ + var userData: Any? = null, + + /** + * Scale the gravity applied to this body. + */ + var gravityScale: Double = 1.0, +) { + fun validate() { + require(angularVelocity.isFinite()) { "Angular velocity is infinite" } + require(linearDamping.isFinite()) { "Linear damping is infinite" } + require(angularDamping.isFinite()) { "Angular damping is infinite" } + require(angle.isFinite()) { "Angular velocity is infinite" } + require(gravityScale.isFinite()) { "Gravity scale is infinite" } + require(position.isFinite) { "Position is infinite" } + + require(!angularVelocity.isNaN()) { "Angular velocity is NaN" } + require(!linearDamping.isNaN()) { "Linear damping is NaN" } + require(!angularDamping.isNaN()) { "Angular damping is NaN" } + require(!angle.isNaN()) { "Angular velocity is NaN" } + require(!gravityScale.isNaN()) { "Gravity scale is NaN" } + + require(angularDamping >= 0.0) { "Angular damping must be non negative, $angularDamping given" } + require(linearDamping >= 0.0) { "Linear damping must be non negative, $linearDamping given" } + } +} + +enum class BodyFlags(val bitmask: Int) { + ISLAND(0x1), + AWAKE(0x2), + AUTO_SLEEP(0x4), + BULLET(0x8), + FIXED_ROTATION(0x10), + ENABLED(0x20), + TOI(0x40); + + fun isit(other: Int): Boolean { + return (other and bitmask) == bitmask + } + + fun update(other: Int, state: Boolean): Int { + if (state) + return or(other) + else + return not(other) + } + + fun and(other: Int): Int { + return other and bitmask + } + + fun or(other: Int): Int { + return other or bitmask + } + + fun not(other: Int): Int { + return other and bitmask.inv() + } +} + +interface IBody { + /** Creates a fixture and attach it to this body. Use this function if you need + * to set some fixture parameters, like friction. Otherwise you can create the + * fixture directly from a shape. + * If the density is non-zero, this function automatically updates the mass of the body. + * Contacts are not created until the next time step. + * @param def the fixture definition. + * @warning This function is locked during callbacks. + */ + fun createFixture(def: FixtureDef): IFixture + + /** + * Creates a fixture from a shape and attach it to this body. + * This is a convenience function. Use b2FixtureDef if you need to set parameters + * like friction, restitution, user data, or filtering. + * If the density is non-zero, this function automatically updates the mass of the body. + * @param shape the shape to be cloned. + * @param density the shape density (set to zero for static bodies). + * @warning This function is locked during callbacks. + */ + fun createFixture(shape: IShape<*>, density: Double): IFixture { + return createFixture( + FixtureDef( + shape = shape, + density = density + ) + ) + } + + /** + * Destroy a fixture. This removes the fixture from the broad-phase and + * destroys all contacts associated with this fixture. This will + * automatically adjust the mass of the body if the body is dynamic and the + * fixture has positive density. + * All fixtures attached to a body are implicitly destroyed when the body is destroyed. + * @param fixture the fixture to be removed. + * @warning This function is locked during callbacks. + */ + fun destroyFixture(fixture: IFixture) + + /** + * Set the position of the body's origin and rotation. + * Manipulating a body's transform may cause non-physical behavior. + * Note: contacts are updated on the next call to b2World::Step. + * @param position the world position of the body's local origin. + * @param angle the world rotation in radians. + */ + fun setTransform(position: Vector2d, angle: Double) + + /** + * Get the body transform for the body's origin. + * @return the world transform of the body's origin. + */ + val transform: Transform + + /** + * Get the world body origin position. + * @return the world position of the body's origin. + */ + val position: Vector2d + + /** + * Get the angle in radians. + * @return the current world rotation angle in radians. + */ + val angle: Double + + /** + * Get the world position of the center of mass. + */ + val worldCenter: Vector2d + + /** + * Get the local position of the center of mass. + */ + val localCenter: Vector2d + + /** + * The linear velocity of the body's origin in world co-ordinates. + */ + var linearVelocity: Vector2d + + /** + * The angular velocity of the body. + */ + var angularVelocity: Double + + /** + * Apply a force at a world point. If the force is not + * applied at the center of mass, it will generate a torque and + * affect the angular velocity. This wakes up the body. + * @param force the world force vector, usually in Newtons (N). + * @param point the world position of the point of application. + * @param wake also wake up the body + */ + fun applyForce(force: Vector2d, point: Vector2d, wake: Boolean = true) + + /** + * Apply a force to the center of mass. This wakes up the body. + * @param force the world force vector, usually in Newtons (N). + * @param wake also wake up the body + */ + fun applyForceToCenter(force: Vector2d, wake: Boolean = true) + + /** + * Apply a torque. This affects the angular velocity + * without affecting the linear velocity of the center of mass. + * @param torque about the z-axis (out of the screen), usually in N-m. + * @param wake also wake up the body + */ + fun applyTorque(torque: Double, wake: Boolean = true) + + /** + * Apply an impulse at a point. This immediately modifies the velocity. + * It also modifies the angular velocity if the point of application + * is not at the center of mass. This wakes up the body. + * @param impulse the world impulse vector, usually in N-seconds or kg-m/s. + * @param point the world position of the point of application. + * @param wake also wake up the body + */ + fun applyLinearImpulse(impulse: Vector2d, point: Vector2d, wake: Boolean = true) + + /** + * Apply an impulse to the center of mass. This immediately modifies the velocity. + * @param impulse the world impulse vector, usually in N-seconds or kg-m/s. + * @param wake also wake up the body + */ + fun applyLinearImpulseToCenter(impulse: Vector2d, wake: Boolean = true) + + /** + * Apply an angular impulse. + * @param impulse the angular impulse in units of kg*m*m/s + * @param wake also wake up the body + */ + fun applyAngularImpulse(impulse: Double, wake: Boolean = true) + + /** + * Get the total mass of the body. + * @return the mass, usually in kilograms (kg). + */ + val mass: Double + + /** + * Get the rotational inertia of the body about the local origin. + * @return the rotational inertia, usually in kg-m^2. + */ + val inertia: Double + + /** + * Get the mass data of the body. + * @return a struct containing the mass, inertia and center of the body. + * Set the mass properties to override the mass properties of the fixtures. + * Note that this changes the center of mass position. + * Note that creating or destroying fixtures can also alter the mass. + * This function has no effect if the body isn't dynamic. + * @param data the mass properties. + */ + var massData: MassData + + /** + * This resets the mass properties to the sum of the mass properties of the fixtures. + * This normally does not need to be called unless you called SetMassData to override + * the mass and you later want to reset the mass. + */ + fun resetMassData() + + /** + * Get the world coordinates of a point given the local coordinates. + * @param localPoint a point on the body measured relative the the body's origin. + * @return the same point expressed in world coordinates. + */ + fun getWorldPoint(localPoint: Vector2d): Vector2d + + /** + * Get the world coordinates of a vector given the local coordinates. + * @param localVector a vector fixed in the body. + * @return the same vector expressed in world coordinates. + */ + fun getWorldVector(localPoint: Vector2d): Vector2d + + /** + * Gets a local point relative to the body's origin given a world point. + * @param worldPoint a point in world coordinates. + * @return the corresponding local point relative to the body's origin. + */ + fun getLocalPoint(worldPoint: Vector2d): Vector2d + + /** + * Gets a local vector given a world vector. + * @param worldVector a vector in world coordinates. + * @return the corresponding local vector. + */ + fun getLocalVector(worldVector: Vector2d): Vector2d + + /** + * Get the world linear velocity of a world point attached to this body. + * @param worldPoint a point in world coordinates. + * @return the world velocity of a point. + */ + fun getLinearVelocityFromWorldPoint(worldPoint: Vector2d): Vector2d + + /** + * Get the world velocity of a local point. + * @param localPoint a point in local coordinates. + * @return the world velocity of a point. + */ + fun getLinearVelocityFromLocalPoint(localPoint: Vector2d): Vector2d + + var linearDamping: Double + var angularDamping: Double + var gravityScale: Double + + /** + * Set the type of this body. This may alter the mass and velocity. + */ + var type: BodyType + + /** + * Should this body be treated like a bullet for continuous collision detection? + */ + var isBullet: Boolean + + /** + * You can disable sleeping on this body. If you disable sleeping, the + * body will be woken. + */ + var allowAutoSleep: Boolean + + /** + * Set the sleep state of the body. A sleeping body has very + * low CPU cost. + * @param flag set to true to wake the body, false to put it to sleep. + */ + var isAwake: Boolean + + /** + * Allow a body to be disabled. A disabled body is not simulated and cannot + * be collided with or woken up. + * + * If you pass a flag of true, all fixtures will be added to the broad-phase. + * + * If you pass a flag of false, all fixtures will be removed from the + * broad-phase and all contacts will be destroyed. + * + * Fixtures and joints are otherwise unaffected. You may continue + * to create/destroy fixtures and joints on disabled bodies. + * + * Fixtures on a disabled body are implicitly disabled and will + * not participate in collisions, ray-casts, or queries. + * + * Joints connected to a disabled body are implicitly disabled. + * + * An diabled body is still owned by a b2World object and remains + * in the body list. + */ + var isEnabled: Boolean + + /** + * Set this body to have fixed rotation. This causes the mass to be reset. + */ + var isFixedRotation: Boolean + + /** + * Get the list of all fixtures attached to this body. + * + * Kotlin: This is not a list, but, as in C++ impl, a custom + * linked list. + */ + val fixtureList: IFixture? + + val fixtureIterator: Iterator get() { + return object : Iterator { + private var node = fixtureList + + override fun hasNext(): Boolean { + return node != null + } + + override fun next(): IFixture { + val old = node!! + node = old.next + return old + } + } + } + + /** + * Get the list of all joints attached to this body. + * + * Kotlin: This is not a list, but, as in C++ impl, a custom + * linked list. + */ + val jointList: JointEdge? + + val jointIterator: Iterator get() { + return object : Iterator { + private var node = jointList + + override fun hasNext(): Boolean { + return node != null + } + + override fun next(): JointEdge { + val old = node!! + node = old.next + return old + } + } + } + + /** + * Get the list of all contacts attached to this body. + * @warning this list changes during the time step and you may + * miss some collisions if you don't use b2ContactListener. + * + * Kotlin: This is not a list, but, as in C++ impl, a custom + * linked list. + */ + val contactEdge: ContactEdge? + + val contactEdgeIterator: Iterator get() { + return object : Iterator { + private var node = contactEdge + + override fun hasNext(): Boolean { + return node != null + } + + override fun next(): ContactEdge { + val old = node!! + node = old.next + return old + } + } + } + + val next: IBody? + val prev: IBody? + + val userData: Any? + + /** + * Get the parent world of this body. + */ + val world: IB2World + + /// Dump this body to a file + fun dump() +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/BroadPhase.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/BroadPhase.kt new file mode 100644 index 00000000..a32de111 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/BroadPhase.kt @@ -0,0 +1,50 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d + +typealias b2Pair = Pair + +const val e_nullProxy = -1 + +/** + * The broad-phase is used for computing pairs and performing volume queries and ray casts. + * This broad-phase does not persist pairs. Instead, this reports potentially new pairs. + * It is up to the client to consume the new pairs and to track subsequent overlap. + */ +interface IBroadPhase : IProxieable, IMovable { + /** + * Call to trigger a re-processing of it's pairs on the next call to UpdatePairs. + */ + fun touchProxy(proxyID: Int) + + /** + * Test overlap of fat AABBs. + */ + fun testOverlap(proxyIDA: Int, proxyIDB: Int): Boolean + + /** + * Get the number of proxies. + */ + val proxyCount: Int + + /** + * Update the pairs. This results in pair callbacks. This can only add pairs. + */ + fun updatePairs(callback: (Any?, Any?) -> Unit) + + /** + * Get the height of the embedded tree. + */ + val treeHeight: Int + + /** + * Get the balance of the embedded tree. + */ + val treeBalance: Int + + /** + * Get the quality metric of the embedded tree. + */ + val treeQuality: Double +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/Collision.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Collision.kt new file mode 100644 index 00000000..43d11587 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Collision.kt @@ -0,0 +1,126 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.Vector2d + +/// The features that intersect to form the contact point +/// This must be 4 bytes or less. ? +data class ContactFeature( + var indexA: Int = 0, ///< Feature index on shapeA + var indexB: Int = 0, ///< Feature index on shapeB + var typeA: Type = Type.VERTEX, ///< The feature type on shapeA + var typeB: Type = Type.VERTEX, ///< The feature type on shapeB +) { + enum class Type { + VERTEX, + FACE, + } +} + +/// Contact ids to facilitate warm starting. +data class ContactID( + var key: Int = 0, ///< Used to quickly compare contact ids. + val cf: ContactFeature = ContactFeature(), +) + +/** + * A manifold point is a contact point belonging to a contact + * manifold. It holds details related to the geometry and dynamics + * of the contact points. + * + * The local point usage depends on the manifold type: + * - e_circles: the local center of circleB + * - e_faceA: the local center of cirlceB or the clip point of polygonB + * - e_faceB: the clip point of polygonA + * + * This structure is stored across time steps, so we keep it small. + * + * Note: the impulses are used for internal caching and may not + * provide reliable contact forces, especially for high speed collisions. + */ +data class ManifoldPoint( + var localPoint: Vector2d = Vector2d.ZERO, ///< usage depends on manifold type + var normalImpulse: Double = 0.0, ///< the non-penetration impulse + var tangentImpulse: Double = 0.0, ///< the friction impulse + var id: ContactID = ContactID(), ///< uniquely identifies a contact point between two shapes +) + +/** + * A manifold for two touching convex shapes. + * + * Box2D supports multiple types of contact: + * - clip point versus plane with radius + * - point versus point with radius (circles) + * + * The local point usage depends on the manifold type: + * - e_circles: the local center of circleA + * - e_faceA: the center of faceA + * - e_faceB: the center of faceB + * + * Similarly the local normal usage: + * - e_circles: not used + * - e_faceA: the normal on polygonA + * - e_faceB: the normal on polygonB + * + * We store contacts in this way so that position correction can + * account for movement, which is critical for continuous physics. + * + * All contact scenarios must be expressed in one of these types. + * + * This structure is stored across time steps, so we keep it small. + */ +data class Manifold( + val localNormal: Vector2d = Vector2d.ZERO, + val localPoint: Vector2d = Vector2d.ZERO, + val type: Type? = null, + val points: List = listOf(), +) { + enum class Type { + CIRCLES, + FACE_A, + FACE_B, + } + + companion object { + val EMPTY = Manifold() + } +} + +interface IWorldManifold { + val normal: Vector2d + val points: Array + 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) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/Constants.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Constants.kt new file mode 100644 index 00000000..210e6819 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Constants.kt @@ -0,0 +1,85 @@ +package ru.dbotthepony.kbox2d.api + +import kotlin.math.PI + +/// You can use this to change the length scale used by your game. +/// For example for inches you could use 39.4. +const val b2_lengthUnitsPerMeter = 1.0 + +/// The maximum number of vertices on a convex polygon. You cannot increase +/// this too much because b2BlockAllocator has a maximum object size. +const val b2_maxPolygonVertices = 8 + +/// Collision + +/// The maximum number of contact points between two convex shapes. Do +/// not change this value. +const val b2_maxManifoldPoints = 2 + +/// This is used to fatten AABBs in the dynamic tree. This allows proxies +/// to move by a small amount without triggering a tree adjustment. +/// This is in meters. +const val b2_aabbExtension = b2_lengthUnitsPerMeter * 0.1 + +/// This is used to fatten AABBs in the dynamic tree. This is used to predict +/// the future position based on the current displacement. +/// This is a dimensionless multiplier. +const val b2_aabbMultiplier = 4.0 + +/// A small length used as a collision and constraint tolerance. Usually it is +/// chosen to be numerically significant, but visually insignificant. In meters. +const val b2_linearSlop = 0.005 * b2_lengthUnitsPerMeter + +/// A small angle used as a collision and constraint tolerance. Usually it is +/// chosen to be numerically significant, but visually insignificant. +const val b2_angularSlop = 2.0 / 180.0 * PI + +/// The radius of the polygon/edge shape skin. This should not be modified. Making +/// this smaller means polygons will have an insufficient buffer for continuous collision. +/// Making it larger may create artifacts for vertex collision. +const val b2_polygonRadius = 2.0 * b2_linearSlop + +/// Maximum number of sub-steps per contact in continuous physics simulation. +const val b2_maxSubSteps = 8 + +// Dynamics + +/// Maximum number of contacts to be handled to solve a TOI impact. +const val b2_maxTOIContacts = 32 + +/// The maximum linear position correction used when solving constraints. This helps to +/// prevent overshoot. Meters. +const val b2_maxLinearCorrection = (0.2 * b2_lengthUnitsPerMeter) + +/// The maximum angular position correction used when solving constraints. This helps to +/// prevent overshoot. +const val b2_maxAngularCorrection = (8.0 / 180.0 * PI) + +/// The maximum linear translation of a body per step. This limit is very large and is used +/// to prevent numerical problems. You shouldn't need to adjust this. Meters. +const val b2_maxTranslation = (2.0 * b2_lengthUnitsPerMeter) +const val b2_maxTranslationSquared = (b2_maxTranslation * b2_maxTranslation) + +/// The maximum angular velocity of a body. This limit is very large and is used +/// to prevent numerical problems. You shouldn't need to adjust this. +const val b2_maxRotation = (0.5 * PI) +const val b2_maxRotationSquared = (b2_maxRotation * b2_maxRotation) + +/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so +/// that overlap is removed in one time step. However using values close to 1 often lead +/// to overshoot. +const val b2_baumgarte = 0.2 +const val b2_toiBaumgarte = 0.75 + +// Sleep + +/// The time that a body must be still before it will go to sleep. +const val b2_timeToSleep = 0.5 + +/// A body cannot sleep if its linear velocity is above this tolerance. +const val b2_linearSleepTolerance = (0.01 * b2_lengthUnitsPerMeter) + +/// A body cannot sleep if its angular velocity is above this tolerance. +const val b2_angularSleepTolerance = (2.0f / 180.0f * PI) + +const val b2_epsilon = 1E-9 diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/Contact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Contact.kt new file mode 100644 index 00000000..a57ccd69 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Contact.kt @@ -0,0 +1,146 @@ +package ru.dbotthepony.kbox2d.api + +import kotlin.math.sqrt + +/// Friction mixing law. The idea is to allow either fixture to drive the friction to zero. +/// For example, anything slides on ice. +fun b2MixFriction(friction1: Double, friction2: Double): Double { + return sqrt(friction1 * friction2) +} + +/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface. +/// For example, a superball bounces on anything. +fun b2MixRestitution(restitution1: Double, restitution2: Double): Double { + return if (restitution1 > restitution2) restitution1 else restitution2 +} + +/// Restitution mixing law. This picks the lowest value. +fun b2MixRestitutionThreshold(threshold1: Double, threshold2: Double): Double { + return if (threshold1 < threshold2) threshold1 else threshold2 +} + +data class ContactRegister( + val primary: Boolean, +) + +/** + * A contact edge is used to connect bodies and contacts together + * in a contact graph where each body is a node and each contact + * is an edge. A contact edge belongs to a doubly linked list + * maintained in each attached body. Each contact has two contact + * nodes, one for each attached body. + */ +data class ContactEdge( + val other: ru.dbotthepony.kbox2d.api.IBody, ///< provides quick access to the other body attached. + val contact: IContact, ///< the contact + var prev: ContactEdge? = null, ///< the previous contact edge in the body's contact list + var next: ContactEdge? = null, ///< the next contact edge in the body's contact list +) + +enum class ContactFlags(val bitmask: Int) { + ISLAND(0x1), + TOUCHING(0x2), + ENABLED(0x4), + FILTER(0x8), + BULLET_HIT(0x10), + TOI(0x20); + + fun isit(other: Int): Boolean { + return (other and bitmask) == bitmask + } + + fun update(other: Int, state: Boolean): Int { + if (state) + return or(other) + else + return not(other) + } + + fun and(other: Int): Int { + return other and bitmask + } + + fun or(other: Int): Int { + return other or bitmask + } + + fun not(other: Int): Int { + return other and bitmask.inv() + } +} + +/** + * The class manages contact between two shapes. A contact exists for each overlapping + * AABB in the broad-phase (except if filtered). Therefore a contact object may exist + * that has no contact points. + */ +interface IContact { + /// Get the contact manifold. Do not modify the manifold unless you understand the + /// internals of Box2D. + val manifold: Manifold + + /// Get the world manifold. + val worldManifold: IWorldManifold + + /// Is this contact touching? + val isTouching: Boolean + + /// Enable/disable this contact. This can be used inside the pre-solve + /// contact listener. The contact is only disabled for the current + /// time step (or sub-step in continuous collisions). + var isEnabled: Boolean + + /// Get the next contact in the world's contact list. + val next: IContact? + val prev: IContact? + + /// Get fixture A in this contact. + val fixtureA: IFixture + + /// Get the child primitive index for fixture A. + val childIndexA: Int + + /// Get fixture B in this contact. + val fixtureB: IFixture + + /// Get the child primitive index for fixture B. + val childIndexB: Int + + /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve. + /// This value persists until set or reset. + /// Get the friction. + var friction: Double + + /// Reset the friction mixture to the default value. + fun resetFriction() { + friction = b2MixFriction(fixtureA.friction, fixtureB.friction) + } + + fun flagForFiltering() + + /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve. + /// The value persists until you set or reset. + var restitution: Double + + /// Reset the restitution to the default value. + fun resetRestitution() { + restitution = b2MixRestitution(fixtureA.restitution, fixtureB.restitution) + } + + /// Override the default restitution velocity threshold mixture. You can call this in b2ContactListener::PreSolve. + /// The value persists until you set or reset. + /// Get the restitution threshold. + var restitutionThreshold: Double + + /// Reset the restitution threshold to the default value. + fun resetRestitutionThreshold() { + restitutionThreshold = b2MixRestitutionThreshold(fixtureA.restitutionThreshold, fixtureB.restitutionThreshold) + } + + /// Set the desired tangent speed for a conveyor belt behavior. In meters per second. + /// Get the desired tangent speed. In meters per second. + var tangentSpeed: Double + + /// Evaluate this contact with your own manifold and transforms. + fun evaluate(xfA: Transform, xfB: Transform): Manifold +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/ContactManager.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/ContactManager.kt new file mode 100644 index 00000000..594f02c5 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/ContactManager.kt @@ -0,0 +1,37 @@ +package ru.dbotthepony.kbox2d.api + +/** + * Delegate of b2World. + */ +interface IContactManager { + fun addPair(proxyUserDataA: Any?, proxyUserDataB: Any?) + + fun findNewContacts() + + fun destroy(contact: IContact) + + fun collide() + + val broadPhase: IBroadPhase + var contactList: IContact? + + val contactListIterator: Iterator get() { + return object : Iterator { + private var node = contactList + + override fun hasNext(): Boolean { + return node != null + } + + override fun next(): IContact { + val old = node!! + node = old.next + return old + } + } + } + + val contactCount: Int + var contactFilter: IContactFilter? + var contactListener: IContactListener? +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/Distance.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Distance.kt new file mode 100644 index 00000000..5872d657 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Distance.kt @@ -0,0 +1,72 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.Vector2d + +/** + * A distance proxy is used by the GJK algorithm. + * It encapsulates any shape. + */ +interface IDistanceProxy { + val vertices: List + val radius: Double + + /** + * Get the supporting vertex index in the given direction. + */ + fun getSupport(d: Vector2d): Int + + /** + * Get the supporting vertex in the given direction. + */ + fun getSupportVertex(d: Vector2d): Vector2d +} + +/** + * Used to warm start b2Distance. + * Set count to zero on first call. + */ +data class SimplexCache( + val metric: Double = 0.0, ///< length or area + val count: Int = 0, + val indexA: IntArray = IntArray(0), ///< vertices on shape A + val indexB: IntArray = IntArray(0), ///< vertices on shape B +) + +/** + * Input for b2Distance. + * You have to option to use the shape radii + * in the computation. Even + */ +data class DistanceInput( + var proxyA: IDistanceProxy, + var proxyB: IDistanceProxy, + var transformA: Transform = Transform(), + var transformB: Transform = Transform(), + var useRadii: Boolean = false +) + +/** + * Output for b2Distance. + */ +data class DistanceOutput( + val pointA: Vector2d, ///< closest point on shapeA + val pointB: Vector2d, ///< closest point on shapeB + val distance: Double, + val iterations: Int, ///< number of GJK iterations used + val newCache: SimplexCache +) + +data class ShapeCastInput( + var proxyA: IDistanceProxy, + var proxyB: IDistanceProxy, + var transformA: Transform, + var transformB: Transform, + var translationB: Vector2d, +) + +data class ShapeCastOutput( + var point: Vector2d = Vector2d.ZERO, + var normal: Vector2d = Vector2d.ZERO, + var lambda: Double = 1.0, + var iterations: Int = 0, +) diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/DynamicTree.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/DynamicTree.kt new file mode 100644 index 00000000..6339eddc --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/DynamicTree.kt @@ -0,0 +1,76 @@ +package ru.dbotthepony.kbox2d.api + +import it.unimi.dsi.fastutil.ints.IntArraySet +import ru.dbotthepony.kbox2d.collision.DynamicTree +import ru.dbotthepony.kbox2d.collision.b2_nullNode +import ru.dbotthepony.kstarbound.math.AABB + +private const val DEBUG_CYCLIC_REFERENCES = true + +/// A node in the dynamic tree. The client does not interact with this directly. +data class TreeNode( + val tree: DynamicTree, + /// Enlarged AABB + var aabb: AABB? = null, + + var child1: Int = 0, + var child2: Int = 0, + + // leaf = 0, free node = -1 + var height: Int = 0, + + var moved: Boolean = false, + + var userData: Any? = null, +) { + val isLeaf get() = child1 == b2_nullNode + + // union + private var _union: Int = b2_nullNode + + var parent by this::_union + var next by this::_union + + internal fun validate() { + if (!DEBUG_CYCLIC_REFERENCES) + return + + val seen = IntArraySet() + var parent = parent + + while (parent != b2_nullNode) { + if (!seen.add(parent)) { + throw IllegalStateException("Cycle detected: $seen") + } + + seen.add(parent) + parent = tree.nodes[parent].parent + } + } +} + +interface IDynamicTree : IProxieable, IMovable { + fun wasMoved(proxyID: Int): Boolean + fun clearMoved(proxyID: Int) + + /** + * Validate this tree. For testing. + */ + fun validate() + + /** + * Compute the height of the binary tree in O(N) time. Should not be + * called often. + */ + val height: Int + + /// Get the maximum balance of an node in the tree. The balance is the difference + /// in height of the two children of a node. + val maxBalance: Int + + /// Get the ratio of the sum of the node areas to the root area. + val getAreaRatio: Double + + /// Build an optimal tree. Very expensive. For testing. + fun rebuildBottomUp() +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/Fixture.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Fixture.kt new file mode 100644 index 00000000..38b3c1b8 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Fixture.kt @@ -0,0 +1,181 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d + +sealed interface IFilter { + /** + * The collision category bits. Normally you would just set one bit. + */ + val categoryBits: Long + + /** + * The collision mask bits. This states the categories that this + * shape would accept for collision. + */ + val maskBits: Long + + /** + * Collision groups allow a certain group of objects to never collide (negative) + * or always collide (positive). Zero means no collision group. Non-zero group + * filtering always wins against the mask bits. + */ + val groupIndex: Int +} + +data class Filter( + override var categoryBits: Long = 0x1L, + override var maskBits: Long = 0xFFFFL, + override var groupIndex: Int = 0, +) : IFilter { + fun immutable(): ImmutableFilter { + return ImmutableFilter(categoryBits, maskBits, groupIndex) + } +} + +data class ImmutableFilter( + override val categoryBits: Long, + override val maskBits: Long, + override val groupIndex: Int, +) : IFilter + +data class FixtureDef( + /// The shape, this must be set. The shape will be cloned. + var shape: IShape<*>? = null, + + var friction: Double = 0.2, + + /// The restitution (elasticity) usually in the range [0,1]. + var restitution: Double = 0.0, + + /// Restitution velocity threshold, usually in m/s. Collisions above this + /// speed have restitution applied (will bounce). + var restitutionThreshold: Double = 1.0 * b2_lengthUnitsPerMeter, + + /// The density, usually in kg/m^2. + var density: Double = 0.0, + + /// A sensor shape collects contact information but never generates a collision + /// response. + var isSensor: Boolean = false, + + /// Use this to store application specific fixture data. + var userData: Any? = null, + + val filter: Filter = Filter() +) + +data class FixtureProxy( + var aabb: AABB, + val fixture: IFixture, + val childIndex: Int, +) { + private var setProxyID = false + + var proxyId: Int = e_nullProxy + set(value) { + if (!setProxyID) { + field = value + setProxyID = true + return + } + + throw IllegalStateException("FixtureProxy should be immutable (tried to set value $value, already having $field)") + } +} + +/** + * A fixture is used to attach a shape to a body for collision detection. A fixture + * inherits its transform from its parent. Fixtures hold additional non-geometric data + * such as friction, collision filters, etc. + * Fixtures are created via b2Body::CreateFixture. + * @warning you cannot reuse fixtures. + */ +interface IFixture { + /// Get the type of the child shape. You can use this to down cast to the concrete shape. + /// @return the shape type. + val type: IShape.Type + + /// Get the child shape. You can modify the child shape, however you should not change the + /// number of vertices because this will crash some collision caching mechanisms. + /// Manipulating the shape may lead to non-physical behavior. + val shape: IShape<*> + + /// Set if this fixture is a sensor. + /// Is this fixture a sensor (non-solid)? + /// @return the true if the shape is a sensor. + var isSensor: Boolean + + /// Set the contact filtering data. This will not update contacts until the next time + /// step when either parent body is active and awake. + /// This automatically calls Refilter. + var filter: IFilter + + /// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide. + fun refilter() + + /// Get the parent body of this fixture. This is nullptr if the fixture is not attached. + /// @return the parent body. + val body: ru.dbotthepony.kbox2d.api.IBody? + + /// Get the next fixture in the parent body's fixture list. + /// @return the next shape. + val next: IFixture? + + /// Get the user data that was assigned in the fixture definition. Use this to + /// store your application specific data. + val userData: Any? + + /// Test a point for containment in this fixture. + /// @param p a point in world coordinates. + fun testPoint(p: Vector2d): Boolean { + return shape.testPoint(checkNotNull(body) { "Tried to use detached fixture" }.transform, p) + } + + /// Cast a ray against this shape. + /// @param output the ray-cast results. + /// @param input the ray-cast input parameters. + /// @param childIndex the child shape index (e.g. edge index) + fun rayCast(input: RayCastInput, childIndex: Int): RayCastOutput { + return shape.rayCast(input, checkNotNull(body) { "Tried to use detached fixture" }.transform, childIndex) + } + + /// Get the mass data for this fixture. The mass data is based on the density and + /// the shape. The rotational inertia is about the shape's origin. This operation + /// may be expensive. + fun getMassData(): MassData { + return shape.computeMass(density) + } + + /// Set the density of this fixture. This will _not_ automatically adjust the mass + /// of the body. You must call b2Body::ResetMassData to update the body's mass. + /// Get the density of this fixture. + var density: Double + + /// Get the coefficient of friction. + /// Set the coefficient of friction. This will _not_ change the friction of + /// existing contacts. + var friction: Double + + /// Get the coefficient of restitution. + /// Set the coefficient of restitution. This will _not_ change the restitution of + /// existing contacts. + var restitution: Double + + /// Get the restitution velocity threshold. + /// Set the restitution threshold. This will _not_ change the restitution threshold of + /// existing contacts. + var restitutionThreshold: Double + + /// Get the fixture's AABB. This AABB may be enlarge and/or stale. + /// If you need a more accurate AABB, compute it using the shape and + /// the body transform. + fun getAABB(childIndex: Int): AABB { + return proxies[childIndex].aabb + } + + /// Dump this fixture to the log file. (Log4J) + fun dump(childIndex: Int) { } + + val proxies: List +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/IDebugDraw.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/IDebugDraw.kt new file mode 100644 index 00000000..45655724 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/IDebugDraw.kt @@ -0,0 +1,52 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.util.Color + +/** + * Implement and register this class with a b2World to provide debug drawing of physics + * entities in your game. + */ +interface IDebugDraw { + var drawShapes: Boolean + var drawJoints: Boolean + var drawAABB: Boolean + var drawPairs: Boolean + var drawCenterOfMess: Boolean + + /** + * Draw a closed polygon provided in CCW order. + */ + fun drawPolygon(vertices: List, color: Color) + + /** + * Draw a solid closed polygon provided in CCW order. + */ + fun drawSolidPolygon(vertices: List, 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) +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/IMovable.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/IMovable.kt new file mode 100644 index 00000000..19dd3b0a --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/IMovable.kt @@ -0,0 +1,12 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.Vector2d + +interface IMovable { + /** + * Shift the world origin. Useful for large worlds. + * The shift formula is: position -= newOrigin + * @param newOrigin the new origin with respect to the old origin + */ + fun shiftOrigin(newOrigin: Vector2d) +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/IProxieable.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/IProxieable.kt new file mode 100644 index 00000000..2d0280c1 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/IProxieable.kt @@ -0,0 +1,76 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d + +fun interface ProxyQueryCallback { + fun invoke(nodeId: Int, userData: Any?): Boolean +} + +fun interface ProxyRayCastCallback { + fun invoke(subInput: RayCastInput, nodeId: Int, userData: Any?): Double +} + +sealed interface IProxieable { + /** + * For [IBroadPhase]: + * Create a proxy with an initial AABB. Pairs are not reported until + * UpdatePairs is called. + * + * For [IDynamicTree]: + * Create a proxy. Provide a tight fitting AABB and a userData pointer. + */ + fun createProxy(aabb: AABB, userData: Any?): Int + + /** + * For [IBroadPhase]: + * Destroy a proxy. It is up to the client to remove any pairs. + * + * For [IDynamicTree]: + * Destroy a proxy. This asserts if the id is invalid. + */ + fun destroyProxy(proxyID: Int) + + /** + * For [IBroadPhase]: + * Call MoveProxy as many times as you like, then when you are done + * call UpdatePairs to finalized the proxy pairs (for your time step). + * @return true + * + * For [IDynamicTree]: + * Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, + * then the proxy is removed from the tree and re-inserted. Otherwise + * the function returns immediately. + * @return true if the proxy was re-inserted. + */ + fun moveProxy(proxyID: Int, aabb: AABB, displacement: Vector2d): Boolean + + /** + * Get user data from a proxy. Returns nullptr if the id is invalid. + */ + fun getUserData(proxyID: Int): Any? + + /** + * Get the fat AABB for a proxy. + */ + fun getFatAABB(proxyID: Int): AABB + + /** + * Query an AABB for overlapping proxies. The callback class + * is called for each proxy that overlaps the supplied AABB. + * + * @return Whenever callback returned false + */ + fun query(aabb: AABB, callback: ProxyQueryCallback): Boolean + + /** + * Ray-cast against the proxies in the tree. This relies on the callback + * to perform a exact ray-cast in the case were the proxy contains a shape. + * The callback also performs the any collision filtering. This has performance + * roughly equal to k * log(n), where k is the number of collisions and n is the + * number of proxies in the tree. + * @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + * @param callback a callback class that is called for each proxy that is hit by the ray. + */ + fun rayCast(input: RayCastInput, callback: ProxyRayCastCallback) +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/Joint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Joint.kt new file mode 100644 index 00000000..19238fe4 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Joint.kt @@ -0,0 +1,773 @@ +package ru.dbotthepony.kbox2d.api + +import org.apache.logging.log4j.LogManager +import ru.dbotthepony.kbox2d.dynamics.joint.AbstractJoint +import ru.dbotthepony.kstarbound.math.Vector2d +import kotlin.math.PI + +data class StiffnessResult(val stiffness: Double, val damping: Double) + +/** + * Utility to compute linear stiffness values from frequency and damping ratio + */ +fun b2LinearStiffness( + frequencyHertz: Double, + dampingRatio: Double, + bodyA: IBody?, + bodyB: IBody?, +): StiffnessResult { + val massA = bodyA?.mass ?: 0.0 + val massB = bodyB?.mass ?: 0.0 + val mass: Double + + if (massA > 0.0 && massB > 0.0) { + mass = massA * massB / (massA + massB) + } else if (massA > 0.0) { + mass = massA + } else { + mass = massB + } + + val omega = 2.0 * PI * frequencyHertz + + return StiffnessResult( + stiffness = mass * omega * omega, + damping = 2.0 * mass * dampingRatio * omega + ) +} + +/** + * Utility to compute rotational stiffness values frequency and damping ratio + */ +fun b2AngularStiffness( + frequencyHertz: Double, + dampingRatio: Double, + bodyA: IBody?, + bodyB: IBody?, +): StiffnessResult { + val inertiaA = bodyA?.inertia ?: 0.0 + val inertiaB = bodyB?.inertia ?: 0.0 + val inertia: Double + + if (inertiaA > 0.0 && inertiaB > 0.0) { + inertia = inertiaA * inertiaB / (inertiaA + inertiaB) + } else if (inertiaA > 0.0) { + inertia = inertiaA + } else { + inertia = inertiaB + } + + val omega = 2.0 * PI * frequencyHertz + + return StiffnessResult( + stiffness = inertia * omega * omega, + damping = 2.0 * inertia * dampingRatio * omega + ) +} + +enum class JointType { + REVOLUTE, + PRISMATIC, + DISTANCE, + PULLEY, + MOUSE, + GEAR, + WHEEL, + WELD, + FRICTION, + MOTOR +} + +data class Jacobian( + val linear: Vector2d, + val angularA: Double, + val angularB: Double +) + +/** + * A joint edge is used to connect bodies and joints together + * in a joint graph where each body is a node and each joint + * is an edge. A joint edge belongs to a doubly linked list + * maintained in each attached body. Each joint has two joint + * nodes, one for each attached body. + */ +class JointEdge( + other: IBody?, ///< provides quick access to the other body attached. + val joint: IJoint, ///< the joint + var prev: JointEdge? = null, ///< the previous joint edge in the body's joint list + var next: JointEdge? = null ///< the next joint edge in the body's joint list +) { + val otherNullable: IBody? = other + val other: IBody get() = checkNotNull(otherNullable) { "Other body is not present" } +} + +sealed interface IJointDef { + /** + * The joint type is set automatically for concrete joint types. + */ + val type: JointType + + /** + * The first attached body. + */ + val bodyA: IBody? + + /** + * The second attached body. + */ + val bodyB: IBody? + + /** + * Set this flag to true if the attached bodies should collide. + */ + var collideConnected: Boolean + + /** + * Use this to attach application specific data to your joints. + */ + var userData: Any? +} + +class DistanceJointDef( + b1: IBody, + b2: IBody, + anchor1: Vector2d, + anchor2: Vector2d +) : IJointDef { + override val type: JointType = JointType.DISTANCE + + /** + * The rest length of this joint. Clamped to a stable minimum value. + */ + var length: Double + + /** + * Minimum length. Clamped to a stable minimum value. + */ + var minLength: Double + + /** + * Maximum length. Must be greater than or equal to the minimum length. + */ + var maxLength: Double + + /** + * The linear stiffness in N/m. + */ + val stiffness: Double = 0.0 + + /** + * The linear damping in N*s/m. + */ + val damping: Double = 0.0 + + override var bodyA: IBody = b1 + override var bodyB: IBody = b2 + + override var collideConnected: Boolean = false + override var userData: Any? = null + + /** + * The local anchor point relative to bodyA's origin. + */ + var localAnchorA: Vector2d = bodyA.getLocalPoint(anchor1) + + /** + * The local anchor point relative to bodyB's origin. + */ + var localAnchorB: Vector2d = bodyB.getLocalPoint(anchor2) + + init { + val d = anchor2 - anchor1 + length = b2Max(d.length, b2_linearSlop) + minLength = length + maxLength = length + } +} + +class RevoluteJointDef( + b1: IBody, + b2: IBody, + anchor: Vector2d, +) : IJointDef { + override val type: JointType = JointType.REVOLUTE + override var bodyA: IBody = b1 + override var bodyB: IBody = b2 + override var collideConnected: Boolean = false + override var userData: Any? = null + + /** + * The local anchor point relative to bodyA's origin. + */ + var localAnchorA = bodyA.getLocalPoint(anchor) + + /** + * The local anchor point relative to bodyB's origin. + */ + var localAnchorB = bodyB.getLocalPoint(anchor) + + /** + * The bodyB angle minus bodyA angle in the reference state (radians). + */ + var referenceAngle = bodyB.angle - bodyA.angle + + /** + * A flag to enable joint limits. + */ + var enableLimit = false + + /** + * The lower angle for the joint limit (radians). + */ + var lowerAngle = 0.0 + + /** + * The upper angle for the joint limit (radians). + */ + var upperAngle = 0.0 + + /** + * A flag to enable the joint motor. + */ + var enableMotor = false + + /** + * The desired motor speed. Usually in radians per second. + */ + var motorSpeed = 0.0 + + /** + * The maximum motor torque used to achieve the desired motor speed. + * + * Usually in N-m. + */ + var maxMotorTorque = 0.0 +} + +class PrismaticJointDef( + b1: IBody, + b2: IBody, + anchor: Vector2d, + axis: Vector2d, +) : IJointDef { + override val type: JointType = JointType.PRISMATIC + override var bodyA: IBody = b1 + override var bodyB: IBody = b2 + override var collideConnected: Boolean = false + override var userData: Any? = null + + /** + * The local anchor point relative to bodyA's origin. + */ + var localAnchorA = bodyA.getLocalPoint(anchor) + + /** + * The local anchor point relative to bodyB's origin. + */ + var localAnchorB = bodyB.getLocalPoint(anchor) + + /** + * The local translation unit axis in bodyA. + */ + var localAxisA = bodyA.getLocalVector(axis) + + /** + * The constrained angle between the bodies: bodyB_angle - bodyA_angle. + */ + var referenceAngle = bodyB.angle - bodyA.angle + + /** + * Enable/disable the joint limit. + */ + var enableLimit = false + + /** + * The lower translation limit, usually in meters. + */ + var lowerTranslation = 0.0 + + /** + * The upper translation limit, usually in meters. + */ + var upperTranslation = 0.0 + + /** + * Enable/disable the joint motor. + */ + var enableMotor = false + + /** + * The maximum motor torque, usually in N-m. + */ + var maxMotorForce = 0.0 + + /** + * The desired motor speed in radians per second. + */ + var motorSpeed = 0.0 +} + +/** + * Pulley joint definition. This requires two ground anchors, + * two dynamic body anchor points, and a pulley ratio. + */ +class PulleyJointDef( + b1: IBody, + b2: IBody, + + /** + * The first ground anchor in world coordinates. This point never moves. + */ + var groundAnchorA: Vector2d, + + /** + * The second ground anchor in world coordinates. This point never moves. + */ + var groundAnchorB: Vector2d, + + anchorA: Vector2d, + anchorB: Vector2d, + ratio: Double, +) : IJointDef { + override val type: JointType = JointType.PULLEY + override var bodyA: IBody = b1 + override var bodyB: IBody = b2 + override var collideConnected: Boolean = false + override var userData: Any? = null + + /** + * The pulley ratio, used to simulate a block-and-tackle. + */ + var ratio: Double = ratio + set(value) { + require(value > b2_epsilon) { "Ratio is too small: $value" } + field = value + } + + init { + // KBox2D: trigger sanity check at least once + this.ratio = ratio + } + + /** + * The local anchor point relative to bodyA's origin. + */ + var localAnchorA = bodyA.getLocalPoint(anchorA) + + /** + * The local anchor point relative to bodyB's origin. + */ + var localAnchorB = bodyB.getLocalPoint(anchorB) + + /** + * The a reference length for the segment attached to bodyA. + */ + var lengthA: Double = (anchorA - groundAnchorA).length + + /** + * The b reference length for the segment attached to bodyB. + */ + var lengthB: Double = (anchorB - groundAnchorB).length +} + +/** + * Gear joint definition. This definition requires two existing + * revolute or prismatic joints (any combination will work). + * @warning bodyB on the input joints must both be dynamic + */ +class GearJointDef( + override var bodyA: IBody, + override var bodyB: IBody, + + /** + * The first revolute/prismatic joint attached to the gear joint. + */ + var joint1: AbstractJoint, + + /** + * The second revolute/prismatic joint attached to the gear joint. + */ + var joint2: AbstractJoint, + + /** + * The gear ratio. + * @see ru.dbotthepony.kbox2d.dynamics.joint.GearJoint for explanation. + */ + var ratio: Double, +) : IJointDef { + override val type: JointType = JointType.GEAR + override var collideConnected: Boolean = false + override var userData: Any? = null +} + +/** + * Mouse joint definition. This requires a world target point, + * tuning parameters, and the time step. + * + * KBox2D: In Box2D, Body A is not used and you put meaningless data into it just to not make it crash. + * + * In KBox2D, Body A is also meaningless, but you are not required to put anything it (leave as null). + * + * Putting anything in Body A or leaving it as null has no effect, other than bloating body A joint linked list. + */ +class MouseJointDef( + /** + * The initial world target point. This is assumed + * to coincide with the body anchor initially. + */ + var target: Vector2d, + + override var bodyB: IBody, + override val bodyA: IBody? = null, + + /** + * The maximum constraint force that can be exerted + * to move the candidate body. Usually you will express + * as some multiple of the weight (multiplier * mass * gravity). + */ + var maxForce: Double = 0.0, + var stiffness: Double = 0.0, + var damping: Double = 0.0, +) : IJointDef { + override val type: JointType = JointType.MOUSE + + override var collideConnected: Boolean = false + override var userData: Any? = null + + fun linearStiffness( + frequencyHertz: Double, + dampingRatio: Double, + bodyA: IBody?, + bodyB: IBody? + ): MouseJointDef { + val (stiffness, damping) = b2LinearStiffness(frequencyHertz, dampingRatio, bodyA, bodyB) + this.stiffness = stiffness + this.damping = damping + return this + } + + fun angularStiffness( + frequencyHertz: Double, + dampingRatio: Double, + bodyA: IBody?, + bodyB: IBody? + ): MouseJointDef { + val (stiffness, damping) = b2AngularStiffness(frequencyHertz, dampingRatio, bodyA, bodyB) + this.stiffness = stiffness + this.damping = damping + return this + } +} + +/** + * Wheel joint definition. This requires defining a line of + * motion using an axis and an anchor point. The definition uses local + * anchor points and a local axis so that the initial configuration + * can violate the constraint slightly. The joint translation is zero + * when the local anchor points coincide in world space. Using local + * anchors and a local axis helps when saving and loading a game. + */ +class WheelJointDef( + override var bodyA: IBody, + override var bodyB: IBody, + anchor: Vector2d, + axis: Vector2d, + + /** + * Enable/disable the joint limit. + */ + var enableLimit: Boolean = false, + + /** + * The lower translation limit, usually in meters. + */ + var lowerTranslation: Double = 0.0, + + /** + * The upper translation limit, usually in meters. + */ + var upperTranslation: Double = 0.0, + + /** + * Enable/disable the joint motor. + */ + var enableMotor: Boolean = false, + + /** + * The maximum motor torque, usually in N-m. + */ + var maxMotorTorque: Double = 0.0, + + /** + * The desired motor speed in radians per second. + */ + var motorSpeed: Double = 0.0, + + /** + * Suspension stiffness. Typically in units N/m. + */ + var stiffness: Double = 0.0, + + /** + * Suspension damping. Typically in units of N*s/m. + */ + var damping: Double = 0.0, +) : IJointDef { + override val type: JointType = JointType.WHEEL + override var collideConnected: Boolean = false + override var userData: Any? = null + + /** + * The local anchor point relative to bodyA's origin. + */ + var localAnchorA: Vector2d = bodyA.getLocalPoint(anchor) + + /** + * The local anchor point relative to bodyB's origin. + */ + var localAnchorB: Vector2d = bodyB.getLocalPoint(anchor) + + /** + * The local translation axis in bodyA. + */ + var localAxisA: Vector2d = bodyA.getLocalVector(axis) + + fun linearStiffness( + frequencyHertz: Double, + dampingRatio: Double, + bodyA: IBody = this.bodyA, + bodyB: IBody = this.bodyB + ): WheelJointDef { + val (stiffness, damping) = b2LinearStiffness(frequencyHertz, dampingRatio, bodyA, bodyB) + this.stiffness = stiffness + this.damping = damping + return this + } + + fun angularStiffness( + frequencyHertz: Double, + dampingRatio: Double, + bodyA: IBody = this.bodyA, + bodyB: IBody = this.bodyB + ): WheelJointDef { + val (stiffness, damping) = b2AngularStiffness(frequencyHertz, dampingRatio, bodyA, bodyB) + this.stiffness = stiffness + this.damping = damping + return this + } +} + +/** + * Weld joint definition. You need to specify local anchor points + * where they are attached and the relative body angle. The position + * of the anchor points is important for computing the reaction torque. + */ +class WeldJointDef( + override var bodyA: IBody, + override var bodyB: IBody, + anchor: Vector2d, + + /** + * Suspension stiffness. Typically in units N/m. + */ + var stiffness: Double = 0.0, + + /** + * Suspension damping. Typically in units of N*s/m. + */ + var damping: Double = 0.0, +) : IJointDef { + override val type: JointType = JointType.WELD + override var collideConnected: Boolean = false + override var userData: Any? = null + + /** + * The bodyB angle minus bodyA angle in the reference state (radians). + */ + var referenceAngle: Double = bodyB.angle - bodyA.angle + + /** + * The local anchor point relative to bodyA's origin. + */ + var localAnchorA: Vector2d = bodyA.getLocalPoint(anchor) + + /** + * The local anchor point relative to bodyB's origin. + */ + var localAnchorB: Vector2d = bodyB.getLocalPoint(anchor) + + fun linearStiffness( + frequencyHertz: Double, + dampingRatio: Double, + bodyA: IBody = this.bodyA, + bodyB: IBody = this.bodyB + ): WeldJointDef { + val (stiffness, damping) = b2LinearStiffness(frequencyHertz, dampingRatio, bodyA, bodyB) + this.stiffness = stiffness + this.damping = damping + return this + } + + fun angularStiffness( + frequencyHertz: Double, + dampingRatio: Double, + bodyA: IBody = this.bodyA, + bodyB: IBody = this.bodyB + ): WeldJointDef { + val (stiffness, damping) = b2AngularStiffness(frequencyHertz, dampingRatio, bodyA, bodyB) + this.stiffness = stiffness + this.damping = damping + return this + } +} + +/** + * Friction joint definition. + */ +class FrictionJointDef( + override var bodyA: IBody, + override var bodyB: IBody, + anchor: Vector2d, + + /** + * The maximum friction force in N. + */ + var maxForce: Double = 0.0, + + /** + * The maximum friction torque in N-m. + */ + var maxTorque: Double = 0.0, + + override var collideConnected: Boolean = false, + override var userData: Any? = null +) : IJointDef { + override val type: JointType = JointType.FRICTION + + /** + * The local anchor point relative to bodyA's origin. + */ + var localAnchorA: Vector2d = bodyA.getLocalPoint(anchor) + + /** + * The local anchor point relative to bodyB's origin. + */ + var localAnchorB: Vector2d = bodyB.getLocalPoint(anchor) +} + +class MotorJointDef( + override var bodyA: IBody, + override var bodyB: IBody, + + /** + * The maximum motor force in N. + */ + var maxForce: Double = 1.0, + + /** + * The maximum motor torque in N-m. + */ + var maxTorque: Double = 1.0, + + override var collideConnected: Boolean = false, + override var userData: Any? = null +) : IJointDef { + override val type: JointType = JointType.MOTOR + + /** + * Position of bodyB minus the position of bodyA, in bodyA's frame, in meters. + */ + var linearOffset: Vector2d = bodyA.getLocalPoint(bodyB.position) + + /** + * The bodyB angle minus bodyA angle in radians. + */ + var angularOffset: Double = bodyB.angle - bodyA.angle + + /** + * Position correction factor in the range [0,1]. + */ + var correctionFactor: Double = 0.3 + set(value) { + require(value in 0.0 .. 1.0) { "Invalid correction factor of $value" } + field = value + } +} + +interface IJoint : IMovable { + /** + * Get the type of the concrete joint. + */ + val type: JointType + + /** + * Get the first body attached to this joint. + */ + val bodyA: IBody + + /** + * Get the second body attached to this joint. + */ + val bodyB: IBody + + /** + * Get the anchor point on bodyA in world coordinates. + */ + val anchorA: Vector2d + + /** + * Get the anchor point on bodyB in world coordinates. + */ + val anchorB: Vector2d + + /** + * Get the reaction force on bodyB at the joint anchor in Newtons. + */ + fun getReactionForce(inv_dt: Double): Vector2d + + /** + * Get the reaction torque on bodyB in N*m. + */ + fun getReactionTorque(inv_dt: Double): Double + + /** + * Get the next joint the world joint list. + * + * Kotlin: This is not a list, but, as in C++ impl, a custom + * linked list. + */ + val next: IJoint? + val prev: IJoint? + + fun destroy() { + bodyA.world.destroyJoint(this) + } + + /** + * Short-cut function to determine if either body is enabled. + */ + val isEnabled: Boolean get() = bodyA.isEnabled || bodyB.isEnabled + + /** + * Get collide connected. + * Note: modifying the collide connect flag won't work correctly because + * the flag is only checked when fixture AABBs begin to overlap. + */ + val collideConnected: Boolean + + /** + * Dump this joint to the log file. + */ + fun dump() { LOGGER.warn("Dump is not supported for this join type: $this") } + + /** + * Debug draw this joint + */ + fun draw(draw: IDebugDraw) { } + + var userData: Any? + + companion object { + private val LOGGER = LogManager.getLogger() + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/Math.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Math.kt new file mode 100644 index 00000000..fd344639 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Math.kt @@ -0,0 +1,510 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.* +import kotlin.math.* + +/// "Next Largest Power of 2 +/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm +/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with +/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next +/// largest power of 2. For a 32-bit value:" +fun b2NextPowerOfTwo(x: Int): Int { + var x = x + x = x or (x ushr 1) + x = x or (x ushr 2) + x = x or (x ushr 4) + x = x or (x ushr 8) + x = x or (x ushr 16) + return x + 1 +} + +fun b2IsPowerOfTwo(x: Int): Boolean { + return x > 0 && (x and (x - 1)) == 0 +} + +/// Rotation +class Rotation( + s: Double, + c: Double, +) { + /// Initialize from an angle in radians + constructor(angle: Double) : this(sin(angle), cos(angle)) + + override fun toString(): String { + return "Rotation($s, $c)" + } + + var s: Double = s + set(value) { + if (!value.isFinite()) { + throw IllegalArgumentException("Tried to set illegal non-finite sinus $value") + } + + if (value.isNaN()) { + throw IllegalArgumentException("Tried to set illegal NaN sinus") + } + + field = value + } + + var c: Double = c + set(value) { + if (!value.isFinite()) { + throw IllegalArgumentException("Tried to set illegal non-finite cosine $value") + } + + if (value.isNaN()) { + throw IllegalArgumentException("Tried to set illegal NaN cosine") + } + + field = value + } + + init { + if (!s.isFinite()) { + throw IllegalArgumentException("Sinus is infinite") + } + + if (!c.isFinite()) { + throw IllegalArgumentException("Cosines is infinite") + } + + if (s.isNaN()) { + throw IllegalArgumentException("Sinus is NaN") + } + + if (c.isNaN()) { + throw IllegalArgumentException("Cosines is NaN") + } + } + + /// Set to the identity rotation + fun setIdentity() { + s = 0.0 + c = 1.0 + } + + /// Set using an angle in radians. + fun set(angle: Double) { + s = sin(angle) + c = cos(angle) + } + + /// Get the angle in radians + val angle: Double get() = atan2(s, c) + + /// Get the x-axis + val xAxis: MutableVector2d get() = MutableVector2d(c, s) + + /// Get the u-axis + val yAxis: MutableVector2d get() = MutableVector2d(-s, c) + + /// Multiply two rotations: q * r + operator fun times(other: Rotation): Rotation { + return Rotation( + s = s * other.c + c * other.s, + c = c * other.c - s * other.s, + ) + } + + /// Transpose multiply two rotations: qT * r + fun timesT(other: Rotation): Rotation { + return Rotation( + s = c * other.s - s * other.c, + c = c * other.c + s * other.s, + ) + } + + operator fun times(v: IVector2d<*>): Vector2d { + return Vector2d(c * v.x - s * v.y, s * v.x + c * v.y) + } + + fun timesT(v: IVector2d<*>): Vector2d { + return Vector2d(c * v.x + s * v.y, -s * v.x + c * v.y) + } +} + +/// A transform contains translation and rotation. It is used to represent +/// the position and orientation of rigid frames. +class Transform( + position: Vector2d, + val rotation: Rotation, +) { + var position: Vector2d = position + set(value) { + if (!value.isFinite) { + throw IllegalArgumentException("Tried to set illegal position $value") + } + + field = value + } + + override fun toString(): String { + return "Transform($position, $rotation)" + } + + constructor(position: Vector2d, rotation: Double) : this(position, Rotation(rotation)) + constructor() : this(Vector2d.ZERO, Rotation(0.0)) + + // aliases for box2d C++ code + val q by this::rotation + var p by this::position + + /// Set this to the identity transform. + fun setIdentity() { + position = Vector2d.ZERO + rotation.setIdentity() + } + + /// Set this based on the position and angle. + fun set(position: IVector2d<*>, angle: Double) { + this.position = Vector2d(position.x, position.y) + rotation.set(angle) + } + + operator fun times(other: Transform): Transform { + return Transform( + rotation = rotation * other.rotation, + position = rotation.times(other.position) + position + ) + } + + fun timesT(other: Transform): Transform { + return Transform( + rotation = rotation.timesT(other.rotation), + position = rotation.timesT(other.position - position) + ) + } + + operator fun times(v: IVector2d<*>): Vector2d { + return Vector2d( + x = rotation.c * v.x - rotation.s * v.y + position.x, + y = rotation.s * v.x + rotation.c * v.y + position.y + ) + } + + fun timesT(v: IVector2d<*>): Vector2d { + val px = v.x - position.x + val py = v.y - position.y + + return Vector2d( + x = (rotation.c * px + rotation.s * py), + y = (-rotation.s * px + rotation.c * py) + ) + } +} + +/** This describes the motion of a body/shape for TOI computation. + * Shapes are defined with respect to the body origin, which may + * no coincide with the center of mass. However, to support dynamics + * we must interpolate the center of mass position. + */ +class Sweep { + var localCenter: Vector2d = Vector2d.ZERO ///< local center of mass position + set(value) { + if (!value.isFinite) { + throw IllegalArgumentException("Tried to set illegal local center $value") + } + + field = value + } + + var c0: Vector2d = Vector2d.ZERO ///< center world positions + set(value) { + if (!value.isFinite) { + throw IllegalArgumentException("Tried to set illegal center world position $value") + } + + field = value + } + + var c: Vector2d = Vector2d.ZERO ///< center world positions + set(value) { + if (!value.isFinite) { + throw IllegalArgumentException("Tried to set illegal center world position $value") + } + + field = value + } + + var a0: Double = 0.0 ///< world angles + set(value) { + if (!value.isFinite()) { + throw IllegalArgumentException("Tried to set illegal non finite $value") + } + + if (value.isNaN()) { + throw IllegalArgumentException("Tried to set illegal NaN value") + } + + field = value + } + + var a: Double = 0.0 ///< world angles + set(value) { + if (!value.isFinite()) { + throw IllegalArgumentException("Tried to set illegal non finite $value") + } + + if (value.isNaN()) { + throw IllegalArgumentException("Tried to set illegal NaN value") + } + + field = value + } + + /// Fraction of the current time step in the range [0,1] + /// c0 and a0 are the positions at alpha0. + var alpha0: Double = 0.0 + set(value) { + if (!value.isFinite()) { + throw IllegalArgumentException("Tried to set illegal non finite $value") + } + + if (value.isNaN()) { + throw IllegalArgumentException("Tried to set illegal NaN value") + } + + field = value + } + + /** Get the interpolated transform at a specific time. + * @param transform the output transform + * @param beta is a factor in [0,1], where 0 indicates alpha0. + * https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ + */ + fun getTransform(loadInto: Transform, beta: Double) { + loadInto.position = c0 * (1.0 - beta) + c * beta + val angle = (1.0 - beta) * a0 + beta * a + loadInto.rotation.set(angle) + loadInto.position -= loadInto.position * localCenter + } + + fun getTransform(beta: Double): Transform { + val v = Transform() + getTransform(v, beta) + return v + } + + /// Advance the sweep forward, yielding a new initial state. + /// @param alpha the new initial time. + fun advance(alpha: Double) { + require(alpha < 1.0) { "Bad advance value $alpha" } + + val beta = (alpha - alpha0) / (1.0 - alpha0) + c0 += (c - c0) * beta + a0 += (a - a0) * beta + alpha0 = alpha + } + + /// Normalize the angles. + fun normalize() { + val d = floor(a0 / (PI * 2.0)) * 2.0 * PI + a0 -= d + a -= d + } + + fun copy(): Sweep { + return Sweep().also { + it.localCenter = localCenter + it.c0 = c0 + it.c = c + it.a0 = a0 + it.a = a + it.alpha0 = alpha0 + } + } + + fun load(from: Sweep) { + this.localCenter = from.localCenter + this.c0 = from.c0 + this.c = from.c + this.a0 = from.a0 + this.a = from.a + this.alpha0 = from.alpha0 + } +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Dot(a: Vector2d, b: Vector2d): Double { + return a.dotProduct(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Cross(a: Vector2d, b: Vector2d): Double { + return a.crossProduct(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Cross(a: Vector2d, b: Double): Vector2d { + return a.crossProduct(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Cross(a: Double, b: Vector2d): Vector2d { + return a.crossProduct(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Abs(a: Vector2d): Vector2d { + return a.absoluteVector +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Abs(a: Double): Double { + return a.absoluteValue +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Mul(q: Rotation, v: Vector2d): Vector2d { + return q.times(v) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2MulT(q: Rotation, v: Vector2d): Vector2d { + return q.timesT(v) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Mul(t: Transform, v: Vector2d): Vector2d { + return t.times(v) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2MulT(t: Transform, v: Vector2d): Vector2d { + return t.timesT(v) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Mul22(t: AbstractMatrix3d<*>, v: Vector2d): Vector2d { + return v.times(t) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Mul(t: Transform, v: Transform): Transform { + return t.times(v) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2MulT(t: Transform, v: Transform): Transform { + return t.timesT(v) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Mul(t: AbstractMatrix2d<*>, v: Vector2d): Vector2d { + return v.times(t) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Mul(t: AbstractMatrix3d<*>, v: Vector3d): Vector3d { + return v.times(t) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Min(a: Double, b: Double): Double { + return a.coerceAtMost(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Min(a: Vector2d, b: Vector2d): Vector2d { + return a.minimumPerComponent(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Max(a: Int, b: Int): Int { + return a.coerceAtLeast(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Max(a: Double, b: Double): Double { + return a.coerceAtLeast(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Max(a: Vector2d, b: Vector2d): Vector2d { + return a.maximumPerComponent(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Distance(a: Vector2d, b: Vector2d): Double { + return a.distance(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2DistanceSquared(a: Vector2d, b: Vector2d): Double { + return a.distanceSquared(b) +} + +/** + * Shortcut for faster porting of C++ code + */ +@Suppress("NOTHING_TO_INLINE") +internal inline fun b2Clamp(a: Double, min: Double, max: Double): Double { + return b2Max(min, b2Min(a, max)) +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/Shape.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Shape.kt new file mode 100644 index 00000000..a6ae264c --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/Shape.kt @@ -0,0 +1,60 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d + +data class MassData( + val mass: Double, + val center: Vector2d, + val inertia: Double, +) + +/// A shape is used for collision detection. You can create a shape however you like. +/// Shapes used for simulation in b2World are created automatically when a b2Fixture +/// is created. Shapes may encapsulate a one or more child shapes. +interface IShape> { + enum class Type { + CIRCLE, + EDGE, + POLYGON, + CHAIN, + } + + /// Clone the concrete shape. + fun copy(): S + + /// Get the type of this shape. You can use this to down cast to the concrete shape. + /// @return the shape type. + val type: Type + + /// Get the number of child primitives. + val childCount: Int + + /// Test a point for containment in this shape. This only works for convex shapes. + /// @param xf the shape world transform. + /// @param p a point in world coordinates. + fun testPoint(transform: Transform, p: Vector2d): Boolean { return false } + + /// Cast a ray against a child shape. + /// @param output the ray-cast results. + /// @param input the ray-cast input parameters. + /// @param transform the transform to be applied to the shape. + /// @param childIndex the child shape index + fun rayCast(input: RayCastInput, transform: Transform, childIndex: Int): RayCastOutput + + /// Given a transform, compute the associated axis aligned bounding box for a child shape. + /// @param aabb returns the axis aligned box. + /// @param xf the world transform of the shape. + /// @param childIndex the child shape + fun computeAABB(transform: Transform, childIndex: Int): AABB + + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin. + /// @param massData returns the mass data for this shape. + /// @param density the density in kilograms per meter squared. + fun computeMass(density: Double): MassData + + /// Radius of a shape. For polygonal shapes this must be b2_polygonRadius. There is no support for + /// making rounded polygons. + var radius: Double +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/ShapeDefs.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/ShapeDefs.kt new file mode 100644 index 00000000..5fe626c7 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/ShapeDefs.kt @@ -0,0 +1,2 @@ +package ru.dbotthepony.kbox2d.api + diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/TimeOfImpact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/TimeOfImpact.kt new file mode 100644 index 00000000..c22a065f --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/TimeOfImpact.kt @@ -0,0 +1,28 @@ +package ru.dbotthepony.kbox2d.api + +/** + * Input parameters for b2TimeOfImpact + */ +data class TOIInput( + var proxyA: IDistanceProxy, + var proxyB: IDistanceProxy, + var sweepA: Sweep, + var sweepB: Sweep, + var tMax: Double, // defines sweep interval [0, tMax] +) + +/** + * Output parameters for b2TimeOfImpact. + */ +data class TOIOutput( + val state: State, + val t: Double, +) { + enum class State { + UNKNOWN, + FAILED, + OVERLAPPED, + TOUCHING, + SEPARATED + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/World.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/World.kt new file mode 100644 index 00000000..4379e7e2 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/World.kt @@ -0,0 +1,246 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.client.gl.GLStateTracker +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d + +/** + * The world class manages all physics entities, dynamic simulation, + * and asynchronous queries. The world also contains efficient memory + * management facilities. + */ +interface IB2World : IMovable { + /** + * Register a destruction listener. The listener is owned by you and must + * remain in scope. + */ + var destructionListener: IDestructionListener? + + /** + * Register a contact filter to provide specific control over collision. + * Otherwise the default filter is used (b2_defaultFilter). The listener is + * owned by you and must remain in scope. + */ + var contactFilter: IContactFilter? + + /** + * Register a contact event listener. The listener is owned by you and must + * remain in scope. + */ + var contactListener: IContactListener? + + /** + * Register a routine for debug drawing. The debug draw functions are called + * inside with b2World::DebugDraw method. The debug draw object is owned + * by you and must remain in scope. + */ + var debugDraw: IDebugDraw? + + /** + * Create a rigid body given a definition. No reference to the definition + * is retained. + * @warning This function is locked during callbacks. + */ + fun createBody(bodyDef: ru.dbotthepony.kbox2d.api.BodyDef): ru.dbotthepony.kbox2d.api.IBody + + /** + * Destroy a rigid body given a definition. No reference to the definition + * is retained. This function is locked during callbacks. + * @warning This automatically deletes all associated shapes and joints. + * @warning This function is locked during callbacks. + */ + fun destroyBody(body: ru.dbotthepony.kbox2d.api.IBody) + + /** + * Create a joint to constrain bodies together. No reference to the definition + * is retained. This may cause the connected bodies to cease colliding. + * @warning This function is locked during callbacks. + */ + fun createJoint(jointDef: IJointDef): IJoint + + /** + * Destroy a joint. This may cause the connected bodies to begin colliding. + * @warning This function is locked during callbacks. + */ + fun destroyJoint(joint: IJoint) + + /** + * Take a time step. This performs collision detection, integration, + * and constraint solution. + * @param dt the amount of time to simulate, this should not vary. + * @param velocityIterations for the velocity constraint solver. + * @param positionIterations for the position constraint solver. + */ + fun step(dt: Double, velocityIterations: Int, positionIterations: Int) + + /** + * Manually clear the force buffer on all bodies. By default, forces are cleared automatically + * after each call to Step. The default behavior is modified by calling SetAutoClearForces. + * The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain + * a fixed sized time step under a variable frame-rate. + * When you perform sub-stepping you will disable auto clearing of forces and instead call + * ClearForces after all sub-steps are complete in one pass of your game loop. + * @see SetAutoClearForces + */ + fun clearForces() + + /** + * Call this to draw shapes and other debug draw data. This is intentionally non-const. + */ + fun debugDraw() + + /** + * Query the world for all fixtures that potentially overlap the + * provided AABB. + * @param callback a user implemented callback class. + * @param aabb the query box. + */ + fun queryAABB(aabb: AABB, callback: IQueryCallback) + + /** + * Ray-cast the world for all fixtures in the path of the ray. Your callback + * controls whether you get the closest point, any point, or n-points. + * The ray-cast ignores shapes that contain the starting point. + * @param callback a user implemented callback class. + * @param point1 the ray starting point + * @param point2 the ray ending point + */ + fun rayCast(point1: Vector2d, point2: Vector2d, callback: IRayCastCallback) + + /** + * Get the world body list. With the returned body, use b2Body::GetNext to get + * the next body in the world list. A nullptr body indicates the end of the list. + * @return the head of the world body list. + */ + val bodyList: ru.dbotthepony.kbox2d.api.IBody? + + val bodyListIterator: Iterator get() { + return object : Iterator { + private var node = bodyList + + override fun hasNext(): Boolean { + return node != null + } + + override fun next(): ru.dbotthepony.kbox2d.api.IBody { + val old = node!! + node = old.next + check(node != old) { "Hard loop detected at $old" } + return old + } + } + } + + /** + * Get the world joint list. With the returned joint, use b2Joint::GetNext to get + * the next joint in the world list. A nullptr joint indicates the end of the list. + * @return the head of the world joint list. + */ + val jointList: IJoint? + + val jointListIterator: Iterator get() { + return object : Iterator { + private var node = jointList + + override fun hasNext(): Boolean { + return node != null + } + + override fun next(): IJoint { + val old = node!! + node = old.next + check(node != old) { "Hard loop detected at $old" } + return old + } + } + } + + /** + * Get the world contact list. With the returned contact, use b2Contact::GetNext to get + * the next contact in the world list. A nullptr contact indicates the end of the list. + * @return the head of the world contact list. + * @warning contacts are created and destroyed in the middle of a time step. + * Use b2ContactListener to avoid missing contacts. + */ + val contactList: IContact? get() = contactManager.contactList + + val contactListIterator: Iterator get() { + return object : Iterator { + private var node = contactList + + override fun hasNext(): Boolean { + return node != null + } + + override fun next(): IContact { + val old = node!! + node = old.next + check(node != old) { "Hard loop detected at $old" } + return old + } + } + } + + /** + * Enable/disable sleep. + */ + var allowAutoSleep: Boolean + + /** + * Enable/disable warm starting. For testing. + */ + var warmStarting: Boolean + + /** + * Enable/disable continuous physics. For testing. + */ + var continuousPhysics: Boolean + + /** + * Enable/disable single stepped continuous physics. For testing. + */ + var enableSubStepping: Boolean + + val proxyCount: Int get() = contactManager.broadPhase.proxyCount + val bodyCount: Int + val jointCount: Int + val contactCount: Int get() = contactManager.contactCount + val treeHeight: Int get() = contactManager.broadPhase.treeHeight + val treeBalance: Int get() = contactManager.broadPhase.treeBalance + + /** + * Get the quality metric of the dynamic tree. The smaller the better. + * The minimum is 1. + */ + val treeQuality: Double get() = contactManager.broadPhase.treeQuality + + /** + * Change the global gravity vector. + */ + var gravity: Vector2d + + /** + * Is the world locked (in the middle of a time step). + */ + val isLocked: Boolean + + /** + * Set flag to control automatic clearing of forces after each time step. + */ + var autoClearForces: Boolean + + /** + * Get the current profile. + */ + val profileData: ru.dbotthepony.kbox2d.api.IProfileData + + /** + * Dump the world into the log file. + * @warning this should be called outside of a time step. + */ + fun dump() + + val contactManager: IContactManager + + fun notifyNewContacts() +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/api/WorldCallbacks.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/api/WorldCallbacks.kt new file mode 100644 index 00000000..3386ec31 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/api/WorldCallbacks.kt @@ -0,0 +1,139 @@ +package ru.dbotthepony.kbox2d.api + +import ru.dbotthepony.kstarbound.math.Vector2d + +/** + * Implement this interface to provide collision filtering. In other words, you can implement + * this class if you want finer control over contact creation. + */ +interface IContactFilter { + /** + * Return true if contact calculations should be performed between these two shapes. + * @warning for performance reasons this is only called when the AABBs begin to overlap. + */ + fun shouldCollide(fixtureA: IFixture, fixtureB: IFixture): Boolean +} + +/** + * Joints and fixtures are destroyed when their associated + * body is destroyed. Implement this listener so that you + * may nullify references to these joints and shapes. + */ +interface IDestructionListener { + /** + * Called when any joint is about to be destroyed due + * to the destruction of one of its attached bodies. + */ + fun sayGoodbye(joint: IJoint) + + /** + * Called when any fixture is about to be destroyed due + * to the destruction of its parent body. + */ + fun sayGoodbye(fixture: IFixture) +} + +/** + * Contact impulses for reporting. Impulses are used instead of forces because + * sub-step forces may approach infinity for rigid body collisions. These + * match up one-to-one with the contact points in b2Manifold. + */ +data class ContactImpulse( + val normalImpulses: DoubleArray, + val tangentImpulses: DoubleArray, +) + +/** + * Implement this class to get contact information. You can use these results for + * things like sounds and game logic. You can also get contact results by + * traversing the contact lists after the time step. However, you might miss + * some contacts because continuous physics leads to sub-stepping. + * + * Additionally you may receive multiple callbacks for the same contact in a + * single time step. + * + * You should strive to make your callbacks efficient because there may be + * many callbacks per time step. + * @warning You cannot create/destroy Box2D entities inside these callbacks. + */ +interface IContactListener { + /** + * Called when two fixtures begin to touch. + */ + fun beginContact(contact: IContact) + + /** + * Called when two fixtures cease to touch. + */ + fun endContact(contact: IContact) + + /** + * This is called after a contact is updated. This allows you to inspect a + * contact before it goes to the solver. If you are careful, you can modify the + * contact manifold (e.g. disable contact). + * + * A copy of the old manifold is provided so that you can detect changes. + * + * Note: this is called only for awake bodies. + * + * Note: this is called even when the number of contact points is zero. + * + * Note: this is not called for sensors. + * + * Note: if you set the number of contact points to zero, you will not + * get an EndContact callback. However, you may get a BeginContact callback + * the next step. + */ + fun preSolve(contact: IContact, oldManifold: Manifold) + + /** + * This lets you inspect a contact after the solver is finished. This is useful + * for inspecting impulses. + * + * Note: the contact manifold does not include time of impact impulses, which can be + * arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly + * in a separate data structure. + * + * Note: this is only called for contacts that are touching, solid, and awake. + */ + fun postSolve(contact: IContact, impulse: ContactImpulse) +} + +/** + * Callback class for AABB queries. + * See IWorld#query + */ +fun interface IQueryCallback { + /** + * Called for each fixture found in the query AABB. + * @return false to terminate the query. + */ + fun reportFixture(fixture: IFixture): Boolean +} + +/** + * Callback class for ray casts. + * See IWorld#rayCast + */ +fun interface IRayCastCallback { + /** + * Called for each fixture found in the query. You control how the ray cast + * proceeds by returning a float: + * + * return -1: ignore this fixture and continue + * + * return 0: terminate the ray cast + * + * return fraction: clip the ray to this point + * + * return 1: don't clip the ray and continue + * + * @param fixture the fixture hit by the ray + * @param point the point of initial intersection + * @param normal the normal vector at the point of intersection + * @param fraction the fraction along the ray at the point of intersection + * @return -1 to filter, 0 to terminate, fraction to clip the ray for + * closest hit, 1 to continue + */ + fun reportFixture(fixture: IFixture, point: Vector2d, normal: Vector2d, fraction: Double): Double +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/BroadPhase.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/BroadPhase.kt new file mode 100644 index 00000000..239ab603 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/BroadPhase.kt @@ -0,0 +1,152 @@ +package ru.dbotthepony.kbox2d.collision + +import it.unimi.dsi.fastutil.ints.IntArrayList +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d + +class BroadPhase : IBroadPhase { + private val moveBuffer = IntArrayList() + private val pairBuffer = ArrayList() + private var moveCount = 0 + + private val tree = DynamicTree() + + override var proxyCount: Int = 0 + private set + + override fun touchProxy(proxyID: Int) { + bufferMove(proxyID) + } + + override fun createProxy(aabb: AABB, userData: Any?): Int { + val proxyId = tree.createProxy(aabb, userData) + proxyCount++ + bufferMove(proxyId) + return proxyId + } + + override fun destroyProxy(proxyID: Int) { + unBufferMove(proxyID) + proxyCount-- + tree.destroyProxy(proxyID) + } + + override fun moveProxy(proxyID: Int, aabb: AABB, displacement: Vector2d): Boolean { + if (tree.moveProxy(proxyID, aabb, displacement)) { + bufferMove(proxyID) + return true + } + + return false + } + + private fun bufferMove(proxyId: Int) { + if (moveBuffer.size > moveCount) { + moveBuffer[moveCount] = proxyId + } else { + moveBuffer.add(proxyId) + } + + moveCount++ + } + + private fun unBufferMove(proxyId: Int) { + for (i in 0 until moveCount) { + if (moveBuffer.getInt(i) == proxyId) { + moveBuffer[i] = e_nullProxy + } + } + } + + override fun query(aabb: AABB, callback: ProxyQueryCallback): Boolean { + return tree.query(aabb, callback) + } + + override fun rayCast(input: RayCastInput, callback: ProxyRayCastCallback) { + tree.rayCast(input, callback) + } + + override fun shiftOrigin(newOrigin: Vector2d) { + tree.shiftOrigin(newOrigin) + } + + override fun getUserData(proxyID: Int): Any? { + return tree.getUserData(proxyID) + } + + override fun getFatAABB(proxyID: Int): AABB { + return tree.getFatAABB(proxyID) + } + + override val treeHeight: Int + get() = tree.height + + override val treeBalance: Int + get() = tree.maxBalance + + override val treeQuality: Double + get() = tree.getAreaRatio + + override fun testOverlap(proxyIDA: Int, proxyIDB: Int): Boolean { + return tree.getFatAABB(proxyIDA).intersect(tree.getFatAABB(proxyIDB)) + } + + private var queryProxy: Int = -1 + + private fun queryCallback(proxyId: Int, userData: Any?): Boolean { + if (proxyId == queryProxy) { + return true + } + + val moved = tree.wasMoved(proxyId) + + if (moved && proxyId > queryProxy) { + // Both proxies are moving. Avoid duplicate pairs. + return true + } + + if (proxyId > queryProxy) { + pairBuffer.add(queryProxy to proxyId) + } else { + pairBuffer.add(proxyId to queryProxy) + } + + return true + } + + override fun updatePairs(callback: (Any?, Any?) -> Unit) { + pairBuffer.clear() + + for (i in 0 until moveCount) { + val value = moveBuffer.getInt(i) + + if (value == e_nullProxy) + continue + + val fatAABB = tree.getFatAABB(value) + queryProxy = value + tree.query(fatAABB, this::queryCallback) + } + + // Send pairs to caller + for (primaryPair in pairBuffer) { + val userDataA = tree.getUserData(primaryPair.first) + val userDataB = tree.getUserData(primaryPair.second) + + callback.invoke(userDataA, userDataB) + } + + // Clear move flags + for (i in 0 until moveCount) { + val value = moveBuffer.getInt(i) + + if (value != e_nullProxy) { + tree.clearMoved(value) + } + } + + // Reset move buffer + moveCount = 0 + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/Collision.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/Collision.kt new file mode 100644 index 00000000..a813fc89 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/Collision.kt @@ -0,0 +1,184 @@ +package ru.dbotthepony.kbox2d.collision + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times +import java.util.* +import kotlin.collections.ArrayList + +/// Compute the point states given two manifolds. The states pertain to the transition from manifold1 +/// to manifold2. So state1 is either persist or remove while state2 is either add or persist. +fun b2GetPointStates(state1: Array, state2: Array, 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 = Array(b2_maxManifoldPoints) { Vector2d.ZERO } + private set + + override var separations: DoubleArray = DoubleArray(b2_maxManifoldPoints) + private set + + init { + if (manifold.points.isNotEmpty()) { + when (manifold.type) { + Manifold.Type.CIRCLES -> { + normal = Vector2d.RIGHT + + val pointA = b2Mul(xfA, manifold.localPoint); + val pointB = b2Mul(xfB, manifold.points[0].localPoint); + + if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) { + normal = (pointB - pointA).normalized + } + + val cA = pointA + radiusA * normal + val cB = pointB - radiusB * normal + + points[0] = 0.5 * (cA + cB) + separations[0] = b2Dot(cB - cA, normal) + } + + Manifold.Type.FACE_A -> { + normal = b2Mul(xfA.q, manifold.localNormal) + val planePoint = b2Mul(xfA, manifold.localPoint) + + for (i in manifold.points.indices) { + val clipPoint = b2Mul(xfB, manifold.points[i].localPoint) + val cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal; + val cB = clipPoint - radiusB * normal; + + points[i] = 0.5 * (cA + cB) + separations[i] = b2Dot(cB - cA, normal) + } + } + + Manifold.Type.FACE_B -> { + normal = b2Mul(xfB.q, manifold.localNormal) + val planePoint = b2Mul(xfB, manifold.localPoint) + + for (i in manifold.points.indices) { + val clipPoint = b2Mul(xfA, manifold.points[i].localPoint); + val cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal; + val cA = clipPoint - radiusA * normal; + + points[i] = 0.5 * (cA + cB) + separations[i] = b2Dot(cA - cB, normal) + } + + // Ensure normal points from A to B. + normal = -normal + } + + null -> throw IllegalArgumentException() + } + } + } +} + +// Sutherland-Hodgman clipping. +internal fun b2ClipSegmentToLine( + vIn: Array, + normal: Vector2d, + offset: Double, + vertexIndexA: Int +): Array { + // Start with no output points + val vOut = arrayOfNulls(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 + 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 +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/Distance.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/Distance.kt new file mode 100644 index 00000000..bb34e8f8 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/Distance.kt @@ -0,0 +1,688 @@ +package ru.dbotthepony.kbox2d.collision + +import com.google.common.collect.ImmutableList +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.collision.shapes.ChainShape +import ru.dbotthepony.kbox2d.collision.shapes.CircleShape +import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.crossProduct +import ru.dbotthepony.kstarbound.math.times + +var b2_gjkCalls = 0 + private set + +var b2_gjkIters = 0 + private set + +var b2_gjkMaxIters = 0 + private set + +class DistanceProxy : IDistanceProxy { + override val vertices: List + override val radius: Double + + constructor(shape: IShape<*>, index: Int) { + when (shape.type) { + IShape.Type.CIRCLE -> { + val circle = shape as CircleShape + vertices = listOf(circle.position) + radius = circle.radius + } + + IShape.Type.EDGE -> { + val edge = shape as EdgeShape + vertices = listOf(edge.vertex1, edge.vertex2) + radius = edge.radius + } + + IShape.Type.POLYGON -> { + val polygon = shape as PolygonShape + vertices = polygon.vertices + radius = polygon.radius + } + + IShape.Type.CHAIN -> { + val chain = shape as ChainShape + val faceA = chain.vertices[index] + val faceB: Vector2d + + if (index + 1 < chain.vertices.size) { + faceB = chain.vertices[index + 1] + } else { + faceB = chain.vertices[0] + } + + vertices = listOf(faceA, faceB) + radius = chain.radius + } + } + } + + constructor(vertices: List, radius: Double) { + this.vertices = vertices + this.radius = radius + } + + override fun getSupport(d: Vector2d): Int { + var bestIndex = 0 + var bestValue = vertices[0].dotProduct(d) + + for (i in 1 until vertices.size) { + val value = vertices[i].dotProduct(d) + + if (value > bestValue) { + bestIndex = i + bestValue = value + } + } + + return bestIndex + } + + override fun getSupportVertex(d: Vector2d): Vector2d { + return vertices[getSupport(d)] + } +} + +data class SimplexVertex( + var wA: Vector2d = Vector2d.ZERO, // support point in proxyA + var wB: Vector2d = Vector2d.ZERO, // support point in proxyB + var w: Vector2d = Vector2d.ZERO, // wB - wA + var a: Double = 0.0, // barycentric coordinate for closest point + var indexA: Int = 0, // wA index + var indexB: Int = 0, // wB index +) { + fun load(other: SimplexVertex) { + wA = other.wA + wB = other.wB + w = other.w + a = other.a + indexA = other.indexA + indexB = other.indexB + } +} + +data class WitnessPoints( + val pA: Vector2d, + val pB: Vector2d, +) + +class Simplex() { + var count: Int = 0 + val v1 = SimplexVertex() + val v2 = SimplexVertex() + val v3 = SimplexVertex() + val vertices = arrayListOf(v1, v2, v3) + + constructor( + cache: SimplexCache, + proxyA: IDistanceProxy, + transformA: Transform, + proxyB: IDistanceProxy, + transformB: Transform, + ) : this() { + check(cache.count <= 3) + count = cache.count + + // Copy data from cache. + for (i in 0 until count) { + val v = vertices[i] + v.indexA = cache.indexA[i] + v.indexB = cache.indexB[i] + val wALocal = proxyA.vertices[v.indexA] + val wBLocal = proxyB.vertices[v.indexB] + v.wA = transformA.times(wALocal) + v.wB = transformB.times(wBLocal) + v.w = v.wB - v.wA + } + + // Compute the new simplex metric, if it is substantially different than + // old metric then flush the simplex. + if (count > 1) { + val metric1 = cache.metric + val metric2 = getMetric() + + if (metric2 < 0.5 * metric1 || 2.0 * metric1 < metric2 || metric2 < b2_epsilon) { + // Reset the simplex. + count = 0 + } + } + + // If the cache is empty or invalid ... + if (count == 0) { + val v = v1 + v.indexA = 0 + v.indexB = 0 + val wALocal = proxyA.vertices[0] + val wBLocal = proxyB.vertices[0] + v.wA = transformA.times(wALocal) + v.wB = transformB.times(wBLocal) + v.w = v.wB - v.wA; + v.a = 1.0 + count = 1 + } + } + + fun writeCache(): SimplexCache { + return SimplexCache( + metric = getMetric(), + count = count, + + indexA = IntArray(count).also { + for (i in 0 until count) { + it[i] = vertices[i].indexA + } + }, + + indexB = IntArray(count).also { + for (i in 0 until count) { + it[i] = vertices[i].indexB + } + } + ) + } + + fun getSearchDirection(): Vector2d { + return when (count) { + 1 -> -v1.w + 2 -> { + val e12 = v2.w - v1.w + val sgn = b2Cross(e12, -v1.w) + + if (sgn > 0.0f) { + // Origin is left of e12. + return b2Cross(1.0, e12) + } else { + // Origin is right of e12. + return b2Cross(e12, 1.0) + } + } + else -> throw IllegalStateException(count.toString()) + } + } + + fun getClosestPoint(): Vector2d { + return when (count) { + 1 -> v1.w + 2 -> v1.a * v1.w + v2.a * v2.w + 3 -> Vector2d.ZERO + else -> throw IllegalStateException(count.toString()) + } + } + + fun getWitnessPoints(): WitnessPoints { + return when (count) { + 1 -> WitnessPoints(v1.wA, v1.wB) + 2 -> WitnessPoints( + pA = v1.a * v1.wA + v2.a * v2.wA, + pB = v1.a * v1.wB + v2.a * v2.wB, + ) + 3 -> { + val pA = v1.a * v1.wA + v2.a * v2.wA + v3.a * v3.wA + + WitnessPoints( + pA = pA, + pB = pA + ) + } + else -> throw IllegalStateException(count.toString()) + } + } + + fun getMetric(): Double { + return when (count) { + 1 -> 0.0 + 2 -> b2Distance(v1.w, v2.w) + 3 -> b2Cross(v2.w - v1.w, v3.w - v1.w) + else -> throw IllegalStateException(count.toString()) + } + } + + /** + * Solve a line segment using barycentric coordinates. + * + * p = a1 * w1 + a2 * w2 + * a1 + a2 = 1 + * + * The vector from the origin to the closest point on the line is + * perpendicular to the line. + * e12 = w2 - w1 + * dot(p, e) = 0 + * a1 * dot(w1, e) + a2 * dot(w2, e) = 0 + * + * 2-by-2 linear system + * [1 1 ][a1] = [1] + * [w1.e12 w2.e12][a2] = [0] + * + * Define + * d12_1 = dot(w2, e12) + * d12_2 = -dot(w1, e12) + * d12 = d12_1 + d12_2 + * + * Solution + * a1 = d12_1 / d12 + * a2 = d12_2 / d12 + */ + fun solve2() { + val w1 = v1.w + val w2 = v2.w + val e12 = w2 - w1 + + // w1 region + val d12_2 = -w1.dotProduct(e12) + if (d12_2 <= 0.0) { + // a2 <= 0, so we clamp it to 0 + v1.a = 1.0 + count = 1 + return + } + + // w2 region + val d12_1 = w2.dotProduct(e12) + if (d12_1 <= 0.0) { + // a1 <= 0, so we clamp it to 0 + v2.a = 1.0 + count = 1 + v1.load(v2) // TODO + return + } + + // Must be in e12 region. + val inv_d12 = 1.0f / (d12_1 + d12_2) + v1.a = d12_1 * inv_d12 + v2.a = d12_2 * inv_d12 + count = 2 + } + + /** + * Possible regions: + * - points[2] + * - edge points[0]-points[2] + * - edge points[1]-points[2] + * - inside the triangle + */ + fun solve3() { + val w1 = v1.w + val w2 = v2.w + val w3 = v3.w + + // Edge12 + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // a3 = 0 + val e12 = w2 - w1 + val w1e12 = w1.dotProduct(e12) + val w2e12 = w2.dotProduct(e12) + val d12_1 = w2e12 + val d12_2 = -w1e12 + + // Edge13 + // [1 1 ][a1] = [1] + // [w1.e13 w3.e13][a3] = [0] + // a2 = 0 + val e13 = w3 - w1 + val w1e13 = w1.dotProduct(e13) + val w3e13 = w3.dotProduct(e13) + val d13_1 = w3e13 + val d13_2 = -w1e13 + + // Edge23 + // [1 1 ][a2] = [1] + // [w2.e23 w3.e23][a3] = [0] + // a1 = 0 + val e23 = w3 - w2 + val w2e23 = w2.dotProduct(e23) + val w3e23 = w3.dotProduct(e23) + val d23_1 = w3e23 + val d23_2 = -w2e23 + + // Triangle123 + val n123 = b2Cross(e12, e13) + + val d123_1 = n123 * b2Cross(w2, w3) + val d123_2 = n123 * b2Cross(w3, w1) + val d123_3 = n123 * b2Cross(w1, w2) + + // w1 region + if (d12_2 <= 0.0 && d13_2 <= 0.0) { + v1.a = 1.0 + count = 1 + return + } + + // e12 + if (d12_1 > 0.0 && d12_2 > 0.0 && d123_3 <= 0.0) { + val inv_d12 = 1.0 / (d12_1 + d12_2) + v1.a = d12_1 * inv_d12 + v2.a = d12_2 * inv_d12 + count = 2 + return + } + + // e13 + if (d13_1 > 0.0 && d13_2 > 0.0 && d123_2 <= 0.0) { + val inv_d13 = 1.0 / (d13_1 + d13_2) + v1.a = d13_1 * inv_d13 + v3.a = d13_2 * inv_d13 + count = 2 + v2.load(v3) // TODO + return + } + + // w2 region + if (d12_1 <= 0.0 && d23_2 <= 0.0) { + v2.a = 1.0 + count = 1 + v1.load(v2) // TODO + return + } + + // w3 region + if (d13_1 <= 0.0 && d23_1 <= 0.0) { + v3.a = 1.0 + count = 1 + v1.load(v3) + return + } + + // e23 + if (d23_1 > 0.0 && d23_2 > 0.0 && d123_1 <= 0.0) { + val inv_d23 = 1.0 / (d23_1 + d23_2) + v2.a = d23_1 * inv_d23 + v3.a = d23_2 * inv_d23 + count = 2 + v1.load(v3) + return + } + + // Must be in triangle123 + val inv_d123 = 1.0 / (d123_1 + d123_2 + d123_3) + v1.a = d123_1 * inv_d123 + v2.a = d123_2 * inv_d123 + v3.a = d123_3 * inv_d123 + count = 3 + } +} + +private const val k_maxIters = 20 + +/** + * Compute the closest points between two shapes. Supports any combination of: + * b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output. + * On the first call set b2SimplexCache.count to zero. + */ +fun b2Distance( + cache: SimplexCache, + proxyA: IDistanceProxy, + proxyB: IDistanceProxy, + transformA: Transform, + transformB: Transform, + useRadii: Boolean = false +): DistanceOutput { + // Initialize the simplex. + val simplex = Simplex(cache, proxyA, transformA, proxyB, transformB) + + // Get simplex vertices as an array. + val vertices = simplex.vertices + + // These store the vertices of the last simplex so that we + // can check for duplicates and prevent cycling. + val saveA = IntArray(3) + val saveB = IntArray(3) + var saveCount: Int + + var iter = 0 + while (iter < k_maxIters) { + // Copy simplex so we can identify duplicates. + saveCount = simplex.count + + for (i in 0 until saveCount) { + saveA[i] = vertices[i].indexA + saveB[i] = vertices[i].indexB + } + + when (simplex.count) { + 1 -> {} + 2 -> simplex.solve2() + 3 -> simplex.solve3() + else -> throw IllegalStateException(simplex.count.toString()) + } + + if (simplex.count == 3) { + // If we have 3 points, then the origin is in the corresponding triangle. + break + } + + // Get search direction. + val d = simplex.getSearchDirection() + + // Ensure the search direction is numerically fit. + if (d.lengthSquared < b2_epsilon * b2_epsilon) { + // The origin is probably contained by a line segment + // or triangle. Thus the shapes are overlapped. + + // We can't return zero here even though there may be overlap. + // In case the simplex is a point, segment, or triangle it is difficult + // to determine if the origin is contained in the CSO or very close to it. + break + } + + // Compute a tentative new simplex vertex using support points. + val vertex = vertices[simplex.count] + vertex.indexA = proxyA.getSupport(transformA.q.timesT(-d)) + vertex.wA = transformA.times(proxyA.vertices[vertex.indexA]) + vertex.indexB = proxyB.getSupport(transformB.q.timesT(d)) + vertex.wB = transformB.times(proxyB.vertices[vertex.indexB]) + vertex.w = vertex.wB - vertex.wA + + // Iteration count is equated to the number of support point calls. + iter++ + b2_gjkIters++ + + // Check for duplicate support points. This is the main termination criteria. + var duplicate = false + + for (i in 0 until saveCount) { + if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i]) { + duplicate = true + break + } + } + + // If we found a duplicate support point we must exit to avoid cycling. + if (duplicate) { + break + } + + // New vertex is ok and needed. + simplex.count++ + } + + b2_gjkMaxIters = b2_gjkMaxIters.coerceAtLeast(iter) + + // Prepare output. + var (pointA, pointB) = simplex.getWitnessPoints() + // Cache the simplex. + val newCache = simplex.writeCache() + + var distance = pointA.distance(pointB) + + // Apply radii if requested + if (useRadii) { + if (distance < b2_epsilon) { + // Shapes are too close to safely compute normal + val p = 0.5 * (pointA + pointB) + pointA = p + pointB = p + distance = 0.0 + } else { + // Keep closest points on perimeter even if overlapped, this way + // the points move smoothly. + val rA = proxyA.radius; + val rB = proxyB.radius; + val normal = (pointB - pointA).normalized; + distance = (distance - rA - rB).coerceAtLeast(0.0) + pointA += rA * normal + pointB -= rB * normal + } + } + + return DistanceOutput( + pointA = pointA, + pointB = pointB, + distance = distance, + newCache = newCache, + iterations = iter, + ) +} + +fun b2Distance( + cache: SimplexCache, + input: DistanceInput +): DistanceOutput { + val (proxyA, proxyB, transformA, transformB, useRadii) = input + return b2Distance(cache, proxyA, proxyB, transformA, transformB, useRadii) +} + +private const val gjk_tolerance = 0.5 * b2_linearSlop + +/** + * GJK-raycast + * Algorithm by Gino van den Bergen. + * "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010 + */ +fun b2ShapeCast( + output: ShapeCastOutput, + proxyA: IDistanceProxy, + proxyB: IDistanceProxy, + xfA: Transform, + xfB: Transform, + r: Vector2d, +): Boolean { + output.iterations = 0 + output.lambda = 1.0 + output.normal = Vector2d.ZERO + output.point = Vector2d.ZERO + + val radiusA = proxyA.radius.coerceAtLeast(b2_polygonRadius) + val radiusB = proxyB.radius.coerceAtLeast(b2_polygonRadius) + + val radius = radiusA + radiusB + var n = Vector2d.ZERO + var lambda = 0.0 + + // Initial simplex + val simplex = Simplex() + + // Get simplex vertices as an array. + val vertices = simplex.vertices + + // Get support point in -r direction + var indexA = proxyA.getSupport(xfA.q.timesT(-r)) + var wA = xfA.times(proxyA.vertices[indexA]) + + var indexB = proxyB.getSupport(xfB.q.timesT(r)) + var wB = xfB.times(proxyB.vertices[indexB]) + var v = wA - wB + + // Sigma is the target distance between polygons + val sigma = b2_polygonRadius.coerceAtLeast(radius - b2_polygonRadius) + + // Main iteration loop. + var iter = 0 + + while (iter < k_maxIters && v.length - sigma > gjk_tolerance) { + check(simplex.count < 3) { simplex.count } + + output.iterations++ + + // Support in direction -v (A - B) + indexA = proxyA.getSupport(xfA.q.timesT(-v)) + wA = xfA.times(proxyA.vertices[indexA]) + + indexB = proxyB.getSupport(xfB.q.timesT(v)) + wB = xfB.times(proxyB.vertices[indexB]) + + val p = wA - wB + + // -v is a normal at p + v = v.normalized + + // Intersect ray with plane + val vp = v.dotProduct(p) + val vr = v.dotProduct(r) + + if (vp - sigma > lambda * vr) { + if (vr <= 0.0) { + return false + } + + lambda = (vp - sigma) / vr + + if (lambda >= 1.0) { + return false + } + + n = -v + simplex.count = 0 + } + + // Reverse simplex since it works with B - A. + // Shift by lambda * r because we want the closest point to the current clip point. + // Note that the support point p is not shifted because we want the plane equation + // to be formed in unshifted space. + val vertex = vertices[simplex.count] + vertex.indexA = indexB + vertex.wA = wB + lambda * r + vertex.indexB = indexA + vertex.wB = wA + vertex.w = vertex.wB - vertex.wA + vertex.a = 1.0 + simplex.count++ + + when (simplex.count) { + 1 -> {} + 2 -> simplex.solve2() + 3 -> simplex.solve3() + else -> throw IllegalStateException(simplex.count.toString()) + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if (simplex.count == 3) { + // Overlap + return false + } + + // Get search direction. + v = simplex.getClosestPoint() + + // Iteration count is equated to the number of support point calls. + iter++ + } + + if (iter == 0) { + // Initial overlap + return false + } + + // Prepare output. + val (_, pointA) = simplex.getWitnessPoints() + + if (v.lengthSquared > 0.0) { + n = (-v).normalized + } + + output.point = pointA + radiusA * n + output.normal = n + output.lambda = lambda + output.iterations = iter + + return true +} + +fun b2ShapeCast(output: ShapeCastOutput, input: ShapeCastInput): Boolean { + val (proxyA, proxyB, transformA, transformB, translationB) = input + return b2ShapeCast(output, proxyA, proxyB, transformA, transformB, translationB) +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/DynamicTree.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/DynamicTree.kt new file mode 100644 index 00000000..9bcbe1f8 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/DynamicTree.kt @@ -0,0 +1,715 @@ +package ru.dbotthepony.kbox2d.collision + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d +import kotlin.math.absoluteValue + +const val b2_nullNode = -1 + +class DynamicTree : IDynamicTree { + private var root: Int = b2_nullNode + + private val nodeCapacity get() = nodes.size + private var nodeCount = 0 + private var freeList = 0 + private var insertionCount = 0 + + internal var nodes = Array(16) { TreeNode(this) } + + init { + // Build a linked list for the free list. + for (i in 0 until nodeCapacity - 1) { + nodes[i].next = i + 1 + nodes[i].height = -1 + } + + nodes[nodeCapacity - 1].next = b2_nullNode + nodes[nodeCapacity - 1].height = -1 + } + + private fun allocateNode(): Int { + // Expand the node pool as needed. + if (freeList == b2_nullNode) { + check(nodeCount == nodeCapacity) { "$nodeCount != $nodeCapacity" } + + // The free list is empty. Rebuild a bigger pool. + nodes = Array(nodeCapacity * 2) memcopy@{ + if (it >= nodeCapacity) + return@memcopy TreeNode(this) + else + return@memcopy nodes[it] + } + + // Build a linked list for the free list. The parent + // pointer becomes the "next" pointer. + for (i in nodeCount until nodeCapacity - 1) { + nodes[i].next = i + 1 + nodes[i].height = -1 + } + + nodes[nodeCapacity - 1].next = b2_nullNode + nodes[nodeCapacity - 1].height = -1 + + freeList = nodeCount + } + + // Peel a node off the free list. + val nodeId = freeList + freeList = nodes[nodeId].next + val node = nodes[nodeId] + + node.parent = b2_nullNode + node.child1 = b2_nullNode + node.child2 = b2_nullNode + node.height = 0 + node.userData = null + node.moved = false + + nodeCount++ + return nodeId + } + + private fun freeNode(nodeId: Int) { + require(nodeId in 0 until nodeCapacity) { "$nodeId is out of range (0 to ${nodeCapacity - 1})" } + check(0 < nodeCount) { "We have $nodeCount nodes" } + + val node = nodes[nodeId] + + node.next = freeList + node.height = -1 + // node.aabb = null + // node.userData = null + + freeList = nodeId + nodeCount-- + } + + override fun createProxy(aabb: AABB, userData: Any?): Int { + val proxyId = allocateNode() + + // Fatten the aabb. + val node = nodes[proxyId] + node.aabb = AABB(aabb.mins - R, aabb.maxs + R) + node.userData = userData + node.height = 0 + node.moved = true + + insertLeaf(proxyId) + + return proxyId + } + + override fun destroyProxy(proxyID: Int) { + check(nodes[proxyID].isLeaf) { "Can't chop whole branch" } + + removeLeaf(proxyID) + freeNode(proxyID) + } + + override fun moveProxy(proxyID: Int, aabb: AABB, displacement: Vector2d): Boolean { + check(nodes[proxyID].isLeaf) { "Can't move whole branch" } + + // Extend AABB + val mins = aabb.mins.toMutableVector() - R + val maxs = aabb.maxs.toMutableVector() + R + + // Predict AABB movement + val d = displacement * b2_aabbMultiplier + + if (d.x < 0.0) { + mins.x += d.x + } else { + maxs.x += d.x + } + + if (d.y < 0.0) { + mins.y += d.y + } else { + maxs.y += d.y + } + + val fatAABB = AABB(mins.toVector(), maxs.toVector()) + val treeAABB = checkNotNull(nodes[proxyID].aabb) { "Node at $proxyID has null AABB" } + + if (treeAABB.contains(fatAABB)) { + // The tree AABB still contains the object, but it might be too large. + // Perhaps the object was moving fast but has since gone to sleep. + // The huge AABB is larger than the new fat AABB. + val hugeAABB = AABB( + fatAABB.mins - R * 4.0, + fatAABB.maxs + R * 4.0, + ) + + if (treeAABB.contains(hugeAABB)) { + // The tree AABB contains the object AABB and the tree AABB is + // not too large. No tree update needed. + return false + } + + // Otherwise the tree AABB is huge and needs to be shrunk + } + + removeLeaf(proxyID) + nodes[proxyID].aabb = fatAABB + insertLeaf(proxyID) + nodes[proxyID].moved = true + + return true + } + + override fun getUserData(proxyID: Int): Any? { + return nodes[proxyID].userData + } + + override fun wasMoved(proxyID: Int): Boolean { + return nodes[proxyID].moved + } + + override fun clearMoved(proxyID: Int) { + nodes[proxyID].moved = false + } + + override fun getFatAABB(proxyID: Int): AABB { + return checkNotNull(nodes[proxyID].aabb) { "Tree node has null aabb. This can be either by a bug, or $proxyID is not a valid proxy" } + } + + private fun insertLeaf(leaf: Int) { + insertionCount++ + + if (root == b2_nullNode) { + root = leaf + nodes[leaf].parent = b2_nullNode + return + } + + // Find the best sibling for this node + val leafAABB = checkNotNull(nodes[leaf].aabb) { "Leaf at $leaf has null aabb" } + var index = root + + while (!nodes[index].isLeaf) { + val child1 = nodes[index].child1 + val child2 = nodes[index].child2 + + val area = checkNotNull(nodes[index].aabb) { "Node at $index has null aabb" }.perimeter + val combined = nodes[index].aabb?.combine(leafAABB) ?: throw ConcurrentModificationException() + + val combinedArea = combined.perimeter + + // Cost of creating a new parent for this node and the new leaf + val cost = 2.0 * combinedArea + + // Minimum cost of pushing the leaf further down the tree + val inheritanceCost = 2.0 * (combinedArea - area) + + // Cost of descending into child1 + val cost1: Double + + if (nodes[child1].isLeaf) { + val aabb = leafAABB.combine(checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" }) + cost1 = aabb.perimeter + inheritanceCost + } else { + val aabb = leafAABB.combine(checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" }) + val oldArea = nodes[child1].aabb?.perimeter ?: throw ConcurrentModificationException() + val newArea = aabb.perimeter + cost1 = (newArea - oldArea) + inheritanceCost + } + + // Cost of descending into child2 + val cost2: Double + + if (nodes[child2].isLeaf) { + val aabb = leafAABB.combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" }) + cost2 = aabb.perimeter + inheritanceCost + } else { + val aabb = leafAABB.combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" }) + val oldArea = nodes[child2].aabb?.perimeter ?: throw ConcurrentModificationException() + val newArea = aabb.perimeter + cost2 = (newArea - oldArea) + inheritanceCost + } + + // Descend according to the minimum cost. + if (cost < cost1 && cost < cost2) { + break + } + + // Descend + if (cost1 < cost2) { + index = child1 + } else { + index = child2 + } + } + + val sibling = index + + // Create a new parent. + val oldParent = nodes[sibling].parent + val newParent = allocateNode() + + nodes[newParent].parent = oldParent + nodes[newParent].userData = null + nodes[newParent].aabb = leafAABB.combine(checkNotNull(nodes[sibling].aabb) { "Node at $sibling has null aabb" }) + nodes[newParent].height = nodes[sibling].height + 1 + + if (oldParent != b2_nullNode) { + // The sibling was not the root. + if (nodes[oldParent].child1 == sibling) { + nodes[oldParent].child1 = newParent + } else { + nodes[oldParent].child2 = newParent + } + + nodes[newParent].child1 = sibling + nodes[newParent].child2 = leaf + + nodes[sibling].parent = newParent + nodes[leaf].parent = newParent + } else { + // The sibling was the root. + nodes[newParent].child1 = sibling + nodes[newParent].child2 = leaf + + nodes[sibling].parent = newParent + nodes[leaf].parent = newParent + root = newParent + } + + // Walk back up the tree fixing heights and AABBs + index = nodes[leaf].parent + + while (index != b2_nullNode) { + index = balance(index) + + val child1 = nodes[index].child1 + val child2 = nodes[index].child2 + + check(child1 != b2_nullNode) { "Node at $index is supposed to have child1, but it does not" } + check(child2 != b2_nullNode) { "Node at $index is supposed to have child2, but it does not" } + + nodes[index].height = 1 + nodes[child1].height.coerceAtLeast(nodes[child2].height) + nodes[index].aabb = + checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" } + .combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" }) + + index = nodes[index].parent + } + + // validate() + } + + private fun removeLeaf(leaf: Int) { + if (leaf == root) { + root = b2_nullNode + return + } + + val parent = nodes[leaf].parent + val grandParent = nodes[parent].parent + val sibling: Int + + if (nodes[parent].child1 == leaf) { + sibling = nodes[parent].child2 + } else { + sibling = nodes[parent].child1 + } + + if (grandParent != b2_nullNode) { + // Destroy parent and connect sibling to grandParent. + if (nodes[grandParent].child1 == parent) { + nodes[grandParent].child1 = sibling + } else { + nodes[grandParent].child2 = sibling + } + + nodes[sibling].parent = grandParent + freeNode(parent) + + // Adjust ancestor bounds. + var index = grandParent + + while (index != b2_nullNode) { + index = balance(index) + + val child1 = nodes[index].child1 + val child2 = nodes[index].child2 + + nodes[index].aabb = + checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" } + .combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" }) + + nodes[index].height = 1 + nodes[child1].height.coerceAtLeast(nodes[child2].height) + + index = nodes[index].parent + } + } else { + root = sibling + nodes[sibling].parent = b2_nullNode + freeNode(parent) + } + + // validate() + } + + /** + * Perform a left or right rotation if node A is imbalanced. + * Returns the new root index. + */ + private fun balance(iA: Int): Int { + require(iA != b2_nullNode) { "iA is a null node" } + + val A = nodes[iA] + + if (A.isLeaf || A.height < 2) { + return iA + } + + val iB = A.child1 + val iC = A.child2 + + val B = nodes[iB] + val C = nodes[iC] + + val balance = C.height - B.height + + // Rotate C up + if (balance > 1) { + val iF = C.child1 + val iG = C.child2 + + val F = nodes[iF] + val G = nodes[iG] + + // Swap A and C + C.child1 = iA + C.parent = A.parent + A.parent = iC + + // A's old parent should point to C + if (C.parent != b2_nullNode) { + if (nodes[C.parent].child1 == iA) { + nodes[C.parent].child1 = iC + } else { + check(nodes[C.parent].child2 == iA) { "${nodes[C.parent].child2} != $iA" } + nodes[C.parent].child2 = iC + } + } else { + root = iC + } + + // Rotate + if (F.height > G.height) { + C.child2 = iF + A.child2 = iG + G.parent = iA + A.aabb = checkNotNull(B.aabb) { "Node at $iB has null aabb" }.combine(checkNotNull(G.aabb) { "Node at $iG has null aabb" }) + C.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(F.aabb) { "Node at $iF has null aabb" }) + + A.height = 1 + b2Max(B.height, G.height) + C.height = 1 + b2Max(A.height, F.height) + } else { + C.child2 = iG + A.child2 = iF + F.parent = iA + + A.aabb = checkNotNull(B.aabb) { "Node at $iB has null aabb" }.combine(checkNotNull(F.aabb) { "Node at $iF has null aabb" }) + C.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(G.aabb) { "Node at $iG has null aabb" }) + + A.height = 1 + b2Max(B.height, F.height) + C.height = 1 + b2Max(A.height, G.height) + } + + return iC + } + + // rotate B up + if (balance < -1) { + val iD = B.child1 + val iE = B.child2 + + val D = nodes[iD] + val E = nodes[iE] + + // Swap A and B + B.child1 = iA + B.parent = A.parent + A.parent = iB + + // A's old parent should point to B + if (B.parent != b2_nullNode) { + if (nodes[B.parent].child1 == iA) { + nodes[B.parent].child1 = iB + } else { + check(nodes[B.parent].child2 == iA) { "${nodes[B.parent].child2} != $iA" } + nodes[B.parent].child2 = iB + } + } else { + root = iB + } + + // Rotate + if (D.height > E.height) { + B.child2 = iD + A.child1 = iE + E.parent = iA + + A.aabb = checkNotNull(C.aabb) { "Node at $iC has null aabb" }.combine(checkNotNull(E.aabb) { "Node at $iE has null aabb" }) + B.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(D.aabb) { "Node at $iD has null aabb" }) + + A.height = 1 + b2Max(C.height, E.height) + B.height = 1 + b2Max(A.height, D.height) + } else { + B.child2 = iE + A.child1 = iD + D.parent = iA + + A.aabb = checkNotNull(C.aabb) { "Node at $iC has null aabb" }.combine(checkNotNull(D.aabb) { "Node at $iD has null aabb" }) + B.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(E.aabb) { "Node at $iE has null aabb" }) + + A.height = 1 + b2Max(C.height, D.height) + B.height = 1 + b2Max(A.height, E.height) + } + + return iB + } + + return iA + } + + override val height: Int get() = if (root == b2_nullNode) 0 else nodes[root].height + + override val getAreaRatio: Double get() { + if (root == b2_nullNode) + return 0.0 + + val root = nodes[root] + val rootArea = checkNotNull(root.aabb) { "Node at ${this.root} has null aabb" }.perimeter + + var totalArea = 0.0 + + for ((i, node) in nodes.withIndex()) + if (node.height >= 0) + totalArea += checkNotNull(node.aabb) { "Node at $i has null aabb" }.perimeter + + return totalArea / rootArea + } + + private fun computeHeight(nodeId: Int): Int { + val node = nodes[nodeId] + + if (node.isLeaf) + return 0 + + val height1 = computeHeight(node.child1) + val height2 = computeHeight(node.child2) + return 1 + height1.coerceAtLeast(height2) + } + + private fun computeHeight() = computeHeight(root) + + private fun validateStructure(index: Int) { + if (index == b2_nullNode) + return + + if (index == root) + check(nodes[index].parent == b2_nullNode) + + val node = nodes[index] + + val child1 = node.child1 + val child2 = node.child2 + + if (node.isLeaf) { + check(child1 == b2_nullNode) + check(child2 == b2_nullNode) + check(node.height == 0) + return + } + + check(nodes[child1].parent == index) + check(nodes[child2].parent == index) + + validateStructure(child1) + validateStructure(child2) + } + + private fun validateMetrics(index: Int) { + if (index == b2_nullNode) + return + + val node = nodes[index] + + val child1 = node.child1 + val child2 = node.child2 + + if (node.isLeaf) { + check(child1 == b2_nullNode) + check(child2 == b2_nullNode) + check(node.height == 0) + return + } + + val height1 = nodes[child1].height + val height2 = nodes[child2].height + + val height = 1 + height1.coerceAtLeast(height2) + check(node.height == height1) + + val combined = nodes[child1].aabb!!.combine(nodes[child2].aabb!!) + check(combined == node.aabb) + + validateMetrics(child1) + validateMetrics(child2) + } + + override fun validate() { + validateStructure(root) + validateMetrics(root) + + var freeCount = 0 + var freeIndex = freeList + + while (freeIndex != b2_nullNode) { + freeIndex = nodes[freeIndex].next + freeCount++ + } + + check(height == computeHeight()) { "Height $height does not match checked height ${computeHeight()}" } + check(nodeCount + freeCount == nodeCapacity) + } + + override val maxBalance: Int get() { + var maxBalance = 0 + + for (node in nodes) { + if (node.height <= 1) + continue + + check(!node.isLeaf) + + val child1 = node.child1 + val child2 = node.child2 + val balance = (nodes[child2].height - nodes[child1].height).absoluteValue + maxBalance = balance.coerceAtLeast(maxBalance) + } + + return maxBalance + } + + override fun rebuildBottomUp() { + TODO("Not Yet Implemented") + } + + override fun shiftOrigin(newOrigin: Vector2d) { + // Build array of leaves. Free the rest. + for (node in nodes) { + if (node.aabb != null) { + node.aabb = node.aabb?.minus(newOrigin) ?: throw ConcurrentModificationException() + } + } + } + + override fun query(aabb: AABB, callback: ProxyQueryCallback): Boolean { + val stack = ArrayDeque(256) + stack.add(root) + + while (stack.isNotEmpty()) { + val nodeId = stack.removeLast() + + if (nodeId == b2_nullNode) { + continue + } + + val node = nodes[nodeId] + val nodeAABB = checkNotNull(node.aabb) { "Tree node at $nodeId has null aabb" } + + if (nodeAABB.intersectWeak(aabb)) { + if (node.isLeaf) { + if (!callback.invoke(nodeId, node.userData)) { + return true + } + } else { + stack.add(node.child1) + stack.add(node.child2) + } + } + } + + return false + } + + override fun rayCast(input: RayCastInput, callback: ProxyRayCastCallback) { + val p1 = input.p1 + val p2 = input.p2 + var r = p2 - p1 + val diff = r + require(r.lengthSquared > 0.0) { "Start and end points match: $p1 $p2" } + r = r.normalized + + // v is perpendicular to the segment. + val v = b2Cross(1.0, r) + val abs_v = v.absoluteVector + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + var maxFraction = input.maxFraction + + // Build a bounding box for the segment. + + var t = p1 + diff * maxFraction + + var segmentAABB = AABB( + mins = p1.minimumPerComponent(t), + maxs = p1.maximumPerComponent(t) + ) + + val stack = ArrayDeque(256) + stack.add(root) + + while (stack.isNotEmpty()) { + val nodeId = stack.removeLast() + + if (nodeId == b2_nullNode) { + continue + } + + val node = nodes[nodeId] + val nodeAABB = checkNotNull(node.aabb) { "Tree node at $nodeId has null aabb" } + + if (!nodeAABB.intersectWeak(segmentAABB)) { + continue + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + if (v.dotProduct(p1 - nodeAABB.centre).absoluteValue > abs_v.dotProduct(nodeAABB.extents)) { + continue + } + + if (node.isLeaf) { + val value = callback.invoke(RayCastInput(p1, p2, maxFraction), nodeId, node.userData) + + if (value == 0.0) { + // The client has terminated the ray cast. + return + } else if (value > 0.0) { + // Update segment bounding box. + maxFraction = value + t = p1 + diff * maxFraction + segmentAABB = AABB( + mins = p1.minimumPerComponent(t), + maxs = p1.maximumPerComponent(t) + ) + } + } else { + stack.add(node.child1) + stack.add(node.child2) + } + } + } + + companion object { + private val R = Vector2d(b2_aabbExtension, b2_aabbExtension) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/TimeOfImpact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/TimeOfImpact.kt new file mode 100644 index 00000000..baec4960 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/TimeOfImpact.kt @@ -0,0 +1,407 @@ +package ru.dbotthepony.kbox2d.collision + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times +import kotlin.math.absoluteValue + +var b2_toiCalls = 0 + private set + +var b2_toiRootIters = 0 + private set + +var b2_toiMaxRootIters = 0 + private set + +var b2_toiIters = 0 + private set + +var b2_toiMaxIters = 0 + private set + +var b2_toiTime = 0L + private set + +var b2_toiMaxTime = 0L + private set + +private data class MinSeparationResult( + val indexA: Int, + val indexB: Int, + val separation: Double +) + +private class SeparationFunction( + cache: SimplexCache, + val proxyA: IDistanceProxy, + val proxyB: IDistanceProxy, + val sweepA: Sweep, + val sweepB: Sweep, + t1: Double +) { + enum class Type { + POINTS, + FACE_A, + FACE_B, + } + + val type: Type + val localPoint: Vector2d + val axis: Vector2d + + init { + require(cache.count in 1 .. 2) { cache.count } + + val xfA = sweepA.getTransform(t1) + val xfB = sweepB.getTransform(t1) + + if (cache.count == 1) { + type = Type.POINTS + val localPointA = proxyA.vertices[cache.indexA[0]] + val localPointB = proxyB.vertices[cache.indexB[0]] + val pointA = b2Mul(xfA, localPointA) + val pointB = b2Mul(xfB, localPointB) + axis = pointB - pointA + + localPoint = Vector2d.ZERO + } else if (cache.indexA[0] == cache.indexA[1]) { + // Two points on B and one on A. + type = Type.FACE_B + val localPointB1 = proxyB.vertices[cache.indexB[0]] + val localPointB2 = proxyB.vertices[cache.indexB[1]] + + val axis = b2Cross(localPointB2 - localPointB1, 1.0).normalized + val normal = b2Mul(xfB.q, axis) + + localPoint = 0.5 * (localPointB1 + localPointB2) + val pointB = b2Mul(xfB, localPoint) + + val localPointA = proxyA.vertices[cache.indexA[0]] + val pointA = b2Mul(xfA, localPointA) + + val s = b2Dot(pointA - pointB, normal) + + if (s < 0.0) { + this.axis = -axis + } else { + this.axis = axis + } + } else { + // Two points on A and one or two points on B. + type = Type.FACE_A; + val localPointA1 = proxyA.vertices[cache.indexA[0]] + val localPointA2 = proxyA.vertices[cache.indexA[1]] + + val axis = b2Cross(localPointA2 - localPointA1, 1.0).normalized + val normal = b2Mul(xfA.q, axis) + + localPoint = 0.5 * (localPointA1 + localPointA2) + val pointA = b2Mul(xfA, localPoint) + + val localPointB = proxyB.vertices[cache.indexB[0]] + val pointB = b2Mul(xfB, localPointB) + + val s = b2Dot(pointB - pointA, normal) + + if (s < 0.0) { + this.axis = -axis + } else { + this.axis = axis + } + } + } + + fun findMinSeparation(t: Double): MinSeparationResult { + val xfA = sweepA.getTransform(t) + val xfB = sweepB.getTransform(t) + + when (type) { + Type.POINTS -> { + val axisA = xfA.q.timesT( axis) + val axisB = xfB.q.timesT(-axis) + + val indexA = proxyA.getSupport(axisA) + val indexB = proxyB.getSupport(axisB) + + val localPointA = proxyA.vertices[indexA] + val localPointB = proxyB.vertices[indexB] + + val pointA = xfA.times(localPointA) + val pointB = xfB.times(localPointB) + + return MinSeparationResult( + indexA = indexA, + indexB = indexB, + separation = (pointB - pointA).dotProduct(axis) + ) + } + + Type.FACE_A -> { + val normal = xfA.q.times(axis) + val pointA = xfA.times(localPoint) + + val axisB = xfB.q.timesT(-normal) + + val indexA = -1 + val indexB = proxyB.getSupport(axisB) + + val localPointB = proxyB.vertices[indexB] + val pointB = xfB.times(localPointB) + + val separation = (pointB - pointA).dotProduct(normal) + return MinSeparationResult( + indexA = indexA, + indexB = indexB, + separation = separation + ) + } + + Type.FACE_B -> { + val normal = xfB.q.times(axis) + val pointB = xfB.times(localPoint) + + val axisA = xfA.q.timesT(-normal) + + val indexB = -1 + val indexA = proxyA.getSupport(axisA) + + val localPointA = proxyA.vertices[indexA] + val pointA = xfA.times(localPointA) + + val separation = (pointA - pointB).dotProduct(normal) + return MinSeparationResult( + indexA = indexA, + indexB = indexB, + separation = separation + ) + } + } + } + + fun evaluate(indexA: Int, indexB: Int, t: Double): Double { + val xfA = sweepA.getTransform(t) + val xfB = sweepB.getTransform(t) + + when (type) { + Type.POINTS -> { + val localPointA = proxyA.vertices[indexA] + val localPointB = proxyB.vertices[indexB] + + val pointA = xfA.times(localPointA) + val pointB = xfB.times(localPointB) + + return (pointB - pointA).dotProduct(axis) + } + + Type.FACE_A -> { + val normal = xfA.q.times(axis); + val pointA = xfA.times(localPoint); + + val localPointB = proxyB.vertices[indexB] + val pointB = xfB.times(localPointB) + + return (pointB - pointA).dotProduct(normal) + } + + Type.FACE_B -> { + val normal = xfB.q.times(axis) + val pointB = xfB.times(localPoint) + + val localPointA = proxyA.vertices[indexA] + val pointA = xfA.times(localPointA) + + return (pointA - pointB).dotProduct(normal) + } + } + } +} + +const val k_maxIterations = 20 + +/** + * Compute the upper bound on time before two shapes penetrate. Time is represented as + * a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, + * non-tunneling collisions. If you change the time interval, you should call this function + * again. + * + * Note: use [b2Distance] to compute the contact point and normal at the time of impact. + */ +fun b2TimeOfImpact( + proxyA: IDistanceProxy, + proxyB: IDistanceProxy, + _sweepA: Sweep, + _sweepB: Sweep, + tMax: Double, // defines sweep interval [0, tMax] +): TOIOutput { + var timer = System.nanoTime() + + b2_toiCalls++ + + var state = TOIOutput.State.UNKNOWN + var t = tMax + + // TODO + val sweepA = _sweepA.copy() + val sweepB = _sweepB.copy() + + sweepA.normalize() + sweepB.normalize() + + val totalRadius = proxyA.radius + proxyB.radius + val target = b2_linearSlop.coerceAtLeast(totalRadius - 3.0 * b2_linearSlop) + val tolerance = 0.25 * b2_linearSlop + + check(target > tolerance) { "$target <= $tolerance" } + + var t1 = 0.0 + var iter = 0 + + // Prepare input for distance query. + var cache = SimplexCache() + val distanceInput = DistanceInput( + proxyA = proxyA, + proxyB = proxyB, + useRadii = false + ) + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + while (true) { + val xfA = sweepA.getTransform(t1) + val xfB = sweepB.getTransform(t1) + + // Get the distance between shapes. We can also use the results + // to get a separating axis. + distanceInput.transformA = xfA + distanceInput.transformB = xfB + + val distanceOutput = b2Distance(cache, distanceInput) + cache = distanceOutput.newCache + + // If the shapes are overlapped, we give up on continuous collision. + if (distanceOutput.distance <= 0.0) { + // Failure! + state = TOIOutput.State.OVERLAPPED + t = 0.0 + break + } + + // Initialize the separating axis. + val fcn = SeparationFunction(cache, proxyA, proxyB, sweepA, sweepB, t1) + + // Compute the TOI on the separating axis. We do this by successively + // resolving the deepest point. This loop is bounded by the number of vertices. + var done = false + var t2 = tMax + var pushBackIter = 0 + + while (true) { + // Find the deepest point at t2. Store the witness point indices. + var (indexA, indexB, s2) = fcn.findMinSeparation(t2) + + // Is the final configuration separated? + if (s2 > target + tolerance) { + // Victory! + state = TOIOutput.State.SEPARATED + t = tMax + done = true + break + } + + // Has the separation reached tolerance? + if (s2 > target - tolerance) { + // Advance the sweeps + t1 = t2 + break + } + + // Compute the initial separation of the witness points. + var s1 = fcn.evaluate(indexA, indexB, t1) + + // Check for initial overlap. This might happen if the root finder + // runs out of iterations. + if (s1 < target - tolerance) { + state = TOIOutput.State.FAILED + t = t1 + done = true + break + } + + // Check for touching + if (s1 <= target + tolerance) { + // Victory! t1 should hold the TOI (could be 0.0). + state = TOIOutput.State.TOUCHING + t = t1 + done = true + break + } + + // Compute 1D root of: f(x) - target = 0 + var rootIterCount = 0 + var a1 = t1 + var a2 = t2 + + while (rootIterCount < 50) { + // Use a mix of the secant rule and bisection. + val t: Double + + if (rootIterCount and 1 == 1) { + // Secant rule to improve convergence. + t = a1 + (target - s1) * (a2 - a1) / (s2 - s1) + } else { + // Bisection to guarantee progress. + t = 0.5 * (a1 + a2) + } + + rootIterCount++ + b2_toiRootIters++ + + val s = fcn.evaluate(indexA, indexB, t) + + if ((s - target).absoluteValue < tolerance) { + // t2 holds a tentative value for t1 + t2 = t + break + } + + // Ensure we continue to bracket the root. + if (s > target) { + a1 = t + s1 = s + } else { + a2 = t + s2 = s + } + } + + b2_toiMaxRootIters = b2_toiMaxRootIters.coerceAtLeast(rootIterCount) + pushBackIter++ + + if (pushBackIter == b2_maxPolygonVertices) { + break + } + } + + iter++ + b2_toiIters++ + + if (done) { + break + } + + if (iter == k_maxIterations) { + // Root finder got stuck. Semi-victory. + state = TOIOutput.State.FAILED + t = t1 + break + } + } + + b2_toiMaxIters = b2_toiMaxIters.coerceAtLeast(iter) + val spent = System.nanoTime() - timer + b2_toiMaxTime = b2_toiMaxTime.coerceAtLeast(spent) + b2_toiTime += spent + + return TOIOutput(state, t) +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideCircle.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideCircle.kt new file mode 100644 index 00000000..678941d3 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideCircle.kt @@ -0,0 +1,149 @@ +package ru.dbotthepony.kbox2d.collision.handler + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.b2Dot +import ru.dbotthepony.kbox2d.api.b2Mul +import ru.dbotthepony.kbox2d.collision.shapes.CircleShape +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape +import ru.dbotthepony.kstarbound.math.times + +internal fun b2CollideCircles( + circleA: CircleShape, + xfA: Transform, + circleB: CircleShape, + xfB: Transform +): Manifold { + val pA = b2Mul(xfA, circleA.p) + val pB = b2Mul(xfB, circleB.p) + + val d = pB - pA + val distSqr = b2Dot(d, d) + val rA = circleA.radius + val rB = circleB.radius + val radius = rA + rB + + if (distSqr > radius * radius) { + return Manifold.EMPTY + } + + return Manifold( + type = Manifold.Type.CIRCLES, + localPoint = circleA.p, + points = listOf( + ManifoldPoint( + localPoint = circleB.p, + id = ContactID(key = 0) + ) + ) + ) +} + +internal fun b2CollidePolygonAndCircle( + polygonA: PolygonShape, + xfA: Transform, + circleB: CircleShape, + xfB: Transform +): Manifold { + // Compute circle position in the frame of the polygon. + val c = b2Mul(xfB, circleB.p) + val cLocal = b2MulT(xfA, c) + + // Find the min separating edge. + var normalIndex = 0 + var separation = -Double.MAX_VALUE + val radius = polygonA.radius + circleB.radius + val vertexCount = polygonA.count + val vertices = polygonA.vertices + val normals = polygonA.normals + + for (i in 0 until vertexCount) { + val s = b2Dot(normals[i], cLocal - vertices[i]) + + if (s > radius) { + // Early out. + return Manifold.EMPTY + } + + if (s > separation) { + separation = s + normalIndex = i + } + } + + // Vertices that subtend the incident face. + val vertIndex1 = normalIndex + val vertIndex2 = if (vertIndex1 + 1 < vertexCount) vertIndex1 + 1 else 0 + val v1 = vertices[vertIndex1] + val v2 = vertices[vertIndex2] + + // If the center is inside the polygon ... + if (separation < b2_epsilon) { + return Manifold( + type = Manifold.Type.FACE_A, + localNormal = normals[normalIndex], + localPoint = 0.5 * (v1 + v2), + points = listOf( + ManifoldPoint( + localPoint = circleB.p, + id = ContactID(key = 0) + ) + ) + ) + } + + // Compute barycentric coordinates + val u1 = b2Dot(cLocal - v1, v2 - v1) + val u2 = b2Dot(cLocal - v2, v1 - v2) + + if (u1 <= 0.0f) { + if (b2DistanceSquared(cLocal, v1) > radius * radius) { + return Manifold.EMPTY + } + + return Manifold( + type = Manifold.Type.FACE_A, + localNormal = (cLocal - v1).normalized, + localPoint = v1, + points = listOf( + ManifoldPoint( + localPoint = circleB.p, + id = ContactID(key = 0) + ) + ) + ) + } else if (u2 <= 0.0f) { + if (b2DistanceSquared(cLocal, v2) > radius * radius) { + return Manifold.EMPTY + } + + return Manifold( + type = Manifold.Type.FACE_A, + localNormal = (cLocal - v2).normalized, + localPoint = v2, + points = listOf( + ManifoldPoint( + localPoint = circleB.p, + id = ContactID(key = 0) + ) + ) + ) + } else { + val faceCenter = 0.5 * (v1 + v2) + val s = b2Dot(cLocal - faceCenter, normals[vertIndex1]) + if (s > radius) { + return Manifold.EMPTY + } + + return Manifold( + type = Manifold.Type.FACE_A, + localNormal = normals[vertIndex1], + localPoint = faceCenter, + points = listOf( + ManifoldPoint( + localPoint = circleB.p, + id = ContactID(key = 0) + ) + ) + ) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideEdge.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideEdge.kt new file mode 100644 index 00000000..cc3b8e52 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollideEdge.kt @@ -0,0 +1,469 @@ +package ru.dbotthepony.kbox2d.collision.handler + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.b2Mul +import ru.dbotthepony.kbox2d.api.b2MulT +import ru.dbotthepony.kbox2d.collision.b2ClipSegmentToLine +import ru.dbotthepony.kbox2d.collision.shapes.CircleShape +import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times +import java.util.* +import kotlin.collections.ArrayList + +internal fun b2CollideEdgeAndCircle( + edgeA: EdgeShape, + xfA: Transform, + circleB: CircleShape, + xfB: Transform +): Manifold { + // Compute circle in frame of edge + val Q = b2MulT(xfA, b2Mul(xfB, circleB.p)) + + val A = edgeA.vertex1 + val B = edgeA.vertex2 + val e = B - A + + // Normal points to the right for a CCW winding + var n = Vector2d(e.y, -e.x) + val offset = b2Dot(n, Q - A) + + if (edgeA.oneSided && offset < 0.0) { + return Manifold.EMPTY + } + + // Barycentric coordinates + val u = b2Dot(e, B - Q) + val v = b2Dot(e, Q - A) + + val radius = edgeA.radius + circleB.radius + val cf = ContactFeature( + indexB = 0, + typeB = ContactFeature.Type.VERTEX + ) + + // Region A + if (v <= 0.0) { + val P = A + val d = Q - P + val dd = b2Dot(d, d) + if (dd > radius * radius) { + return Manifold.EMPTY + } + + // Is there an edge connected to A? + if (edgeA.oneSided) { + val A1 = edgeA.vertex0 + val B1 = A + val e1 = B1 - A1 + val u1 = b2Dot(e1, B1 - Q) + + // Is the circle in Region AB of the previous edge? + if (u1 > 0.0) { + return Manifold.EMPTY + } + } + + cf.indexA = 0 + cf.typeA = ContactFeature.Type.VERTEX + + return Manifold( + type = Manifold.Type.CIRCLES, + localPoint = P, + points = listOf( + ManifoldPoint( + id = ContactID(key = 0, cf = cf), + localPoint = circleB.p + ) + ) + ) + } + + // Region B + if (u <= 0.0) { + val P = B + val d = Q - P + val dd = b2Dot(d, d) + if (dd > radius * radius) { + return Manifold.EMPTY + } + + // Is there an edge connected to B? + if (edgeA.oneSided) { + val B2 = edgeA.vertex3 + val A2 = B + val e2 = B2 - A2 + val v2 = b2Dot(e2, Q - A2) + + // Is the circle in Region AB of the next edge? + if (v2 > 0.0) { + return Manifold.EMPTY + } + } + + cf.indexA = 1 + cf.typeA = ContactFeature.Type.VERTEX + + return Manifold( + type = Manifold.Type.CIRCLES, + localPoint = P, + points = listOf( + ManifoldPoint( + id = ContactID(key = 0, cf = cf), + localPoint = circleB.p + ) + ) + ) + } + + // Region AB + val den = b2Dot(e, e) + check(den > 0.0) { den } + val P = (1.0 / den) * (u * A + v * B) + val d = Q - P + val dd = b2Dot(d, d) + if (dd > radius * radius) { + return Manifold.EMPTY + } + + if (offset < 0.0) { + n = Vector2d(-n.x, -n.y) + } + + n = n.normalized + + cf.indexA = 0 + cf.typeA = ContactFeature.Type.FACE + + return Manifold( + type = Manifold.Type.FACE_A, + localNormal = n, + localPoint = A, + points = listOf( + ManifoldPoint( + id = ContactID(key = 0, cf = cf), + localPoint = circleB.p + ) + ) + ) +} + +// This structure is used to keep track of the best separating axis. +private data class EPAxis( + var normal: Vector2d = Vector2d.ZERO, + var type: Type = Type.UNKNOWN, + var index: Int = -1, + var separation: Double = -Double.MAX_VALUE +) { + enum class Type { + UNKNOWN, + EDGE_A, + EDGE_B + } +} + +// This holds polygon B expressed in frame A. +private data class TempPolygon( + val vertices: ArrayList = ArrayList(), + val normals: ArrayList = 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(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)) +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollidePolygon.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollidePolygon.kt new file mode 100644 index 00000000..45f5aab4 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/handler/CollidePolygon.kt @@ -0,0 +1,250 @@ +package ru.dbotthepony.kbox2d.collision.handler + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.b2Dot +import ru.dbotthepony.kbox2d.api.b2Mul +import ru.dbotthepony.kbox2d.api.b2MulT +import ru.dbotthepony.kbox2d.collision.b2ClipSegmentToLine +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape +import ru.dbotthepony.kstarbound.math.times +import java.util.* +import kotlin.collections.ArrayList + +internal data class FindMaxSeparationResult( + val edgeIndex: Int, + val maxSeparation: Double +) + +// Find the max separation between poly1 and poly2 using edge normals from poly1. +internal fun b2FindMaxSeparation( + poly1: PolygonShape, + xf1: Transform, + poly2: PolygonShape, + xf2: Transform +): FindMaxSeparationResult { + val count1 = poly1.vertices.size + val count2 = poly2.vertices.size + val n1s = poly1.normals + val v1s = poly1.vertices + val v2s = poly2.vertices + + var bestIndex = 0 + var maxSeparation = -Double.MAX_VALUE + + val xf = b2MulT(xf2, xf1) + + for (i in 0 until count1) { + // Get poly1 normal in frame2. + val n = b2Mul(xf.q, n1s[i]) + val v1 = b2Mul(xf, v1s[i]) + + // Find deepest point for normal i. + var si = Double.MAX_VALUE + for (j in 0 until count2) { + val sij = b2Dot(n, v2s[j] - v1) + + if (sij < si) { + si = sij + } + } + + if (si > maxSeparation) { + maxSeparation = si + bestIndex = i + } + } + + return FindMaxSeparationResult( + edgeIndex = bestIndex, + maxSeparation = maxSeparation + ) +} + +internal fun b2FindIncidentEdge( + poly1: PolygonShape, + xf1: Transform, + edge1: Int, + poly2: PolygonShape, + xf2: Transform +): Array { + 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() + + 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 + ) +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/ChainShape.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/ChainShape.kt new file mode 100644 index 00000000..1060c58b --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/ChainShape.kt @@ -0,0 +1,191 @@ +package ru.dbotthepony.kbox2d.collision.shapes + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d + +/** + * A chain shape is a free form sequence of line segments. + * The chain has one-sided collision, with the surface normal pointing to the right of the edge. + * This provides a counter-clockwise winding like the polygon shape. + * Connectivity information is used to create smooth collisions. + * @warning the chain will not collide properly if there are self-intersections. + */ +class ChainShape : IShape { + 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) { + for (i in 1 until vertices.size) { + val v1 = vertices[i - 1] + val v2 = vertices[i] + + v1.isFiniteOrThrow { "Vertex at ${i - 1} is invalid" } + v2.isFiniteOrThrow { "Vertex at $i is invalid" } + + require(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop) { + "Vertices are too close together, at indices ${i - 1} ($v1) and $i ($v2)" + } + } + } + + /** + * Create a loop. This automatically adjusts connectivity. + * @param vertices an array of vertices, these are copied + * @param count the vertex count + */ + fun createLoop(vertices: List) { + // 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, 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() + 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 = arrayOfNulls(0) + private var rayEdgeCache: Array = arrayOfNulls(0) + + /** + * Get a child edge. + */ + fun getChildEdge(index: Int): EdgeShape { + val getEdge = edgeCache[index] + + if (getEdge != null) { + return getEdge + } + + val edge = EdgeShape() + + edge.vertex1 = vertices[index] + edge.vertex2 = vertices[index + 1] + edge.oneSided = true + + if (index > 0) { + edge.vertex0 = vertices[index - 1] + } else { + edge.vertex0 = prevVertex ?: throw NullPointerException("prevVertex is null") + } + + if (index < vertices.size - 2) { + edge.vertex3 = vertices[index + 2] + } else { + edge.vertex3 = nextVertex ?: throw NullPointerException("nextVertex is null") + } + + edgeCache[index] = edge + return edge + } + + override fun rayCast(input: RayCastInput, transform: Transform, childIndex: Int): RayCastOutput { + val getEdge = rayEdgeCache[childIndex] + + if (getEdge != null) { + return getEdge.rayCast(input, transform, 0) + } + + val edgeShape = EdgeShape() + + val i1 = childIndex + var i2 = childIndex + 1 + + if (i2 == vertices.size) { + i2 = 0 + } + + edgeShape.vertex1 = vertices[i1] + edgeShape.vertex2 = vertices[i2] + + rayEdgeCache[childIndex] = edgeShape + return edgeShape.rayCast(input, transform, 0) + } + + override fun computeAABB(transform: Transform, childIndex: Int): AABB { + val i1 = childIndex + var i2 = childIndex + 1 + + if (i2 == vertices.size) { + i2 = 0 + } + + val v1 = b2Mul(transform, vertices[i1]) + val v2 = b2Mul(transform, vertices[i2]) + + val lower = b2Min(v1, v2) + val upper = b2Max(v1, v2) + + val r = Vector2d(radius, radius) + + return AABB( + mins = lower - r, + maxs = upper + r + ) + } + + override fun computeMass(density: Double): MassData { + return MASS + } + + override var radius: Double = b2_polygonRadius + set(value) { + if (value == b2_polygonRadius) { + field = value + } + + throw IllegalArgumentException("") + } + + companion object { + private val MASS = MassData(mass = 0.0, center = Vector2d.ZERO, inertia = 0.0) + } +} \ No newline at end of file diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/CircleShape.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/CircleShape.kt new file mode 100644 index 00000000..69d6e70b --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/CircleShape.kt @@ -0,0 +1,79 @@ +package ru.dbotthepony.kbox2d.collision.shapes + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times +import kotlin.math.PI +import kotlin.math.sqrt + +class CircleShape( + override var radius: Double = 0.0, + var position: Vector2d = Vector2d.ZERO, +) : IShape { + var p by this::position + + override fun copy(): CircleShape { + return CircleShape().also { + it.position = this.position + it.radius = this.radius + } + } + + override val type: IShape.Type = IShape.Type.CIRCLE + override val childCount: Int = 1 + + override fun testPoint(transform: Transform, p: Vector2d): Boolean { + val center = transform.p + b2Mul(transform.q, position) + val d = p - center + return b2Dot(d, d) <= radius * radius + } + + override fun rayCast(input: RayCastInput, transform: Transform, childIndex: Int): RayCastOutput { + val position = transform.p + b2Mul(transform.q, position) + val s = input.p1 - position + val b = b2Dot(s, s) - radius * radius + + // Solve quadratic equation. + // Solve quadratic equation. + val r = input.p2 - input.p1 + val c = b2Dot(s, r) + val rr = b2Dot(r, r) + val sigma = c * c - rr * b + + // Check for negative discriminant and short segment. + if (sigma < 0.0 || rr < b2_epsilon) { + return RayCastOutput.MISS + } + + // Find the point of intersection of the line with the circle. + var a = -(c + sqrt(sigma)) + + // Is the intersection point on the segment? + if (a in 0.0 .. input.maxFraction * rr) { + a /= rr + return RayCastOutput(hit = true, fraction = a, normal = (s + a * r).normalized) + } + + return RayCastOutput.MISS + } + + override fun computeAABB(transform: Transform, childIndex: Int): AABB { + val p = transform.p + b2Mul(transform.q, position) + + return AABB( + mins = Vector2d(p.x - radius, p.y - radius), + maxs = Vector2d(p.x + radius, p.y + radius) + ) + } + + override fun computeMass(density: Double): MassData { + val mass = density * PI * radius * radius + + return MassData( + mass = mass, + center = position, + inertia = mass * (0.5 * radius * radius + b2Dot(position, position)) + ) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/EdgeShape.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/EdgeShape.kt new file mode 100644 index 00000000..516b744c --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/EdgeShape.kt @@ -0,0 +1,188 @@ +package ru.dbotthepony.kbox2d.collision.shapes + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times + +/** + * A line segment (edge) shape. These can be connected in chains or loops + * to other edge shapes. Edges created independently are two-sided and do + * no provide smooth movement across junctions. + */ +class EdgeShape : IShape { + override fun copy(): EdgeShape { + return EdgeShape().also { + it.oneSided = oneSided + it.vertex0 = vertex0 + it.vertex1 = vertex1 + it.vertex2 = vertex2 + it.vertex3 = vertex3 + } + } + + override val type: IShape.Type = IShape.Type.EDGE + override val childCount: Int = 1 + + override fun rayCast(input: RayCastInput, transform: Transform, childIndex: Int): RayCastOutput { + // Put the ray into the edge's frame of reference. + val p1 = b2MulT(transform.q, input.p1 - transform.p) + val p2 = b2MulT(transform.q, input.p2 - transform.p) + val d = p2 - p1 + + val v1 = vertex1 + val v2 = vertex2 + val e = v2 - v1 + + // Normal points to the right, looking from v1 at v2 + val normal = Vector2d(e.y, -e.x).normalized + + // q = p1 + t * d + // dot(normal, q - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + val numerator = b2Dot(normal, v1 - p1) + + if (oneSided && numerator > 0.0) { + return RayCastOutput.MISS + } + + val denominator = b2Dot(normal, d) + + if (denominator == 0.0) { + return RayCastOutput.MISS + } + + val t = numerator / denominator + + if (t < 0.0 || input.maxFraction < t) { + return RayCastOutput.MISS + } + + val q = p1 + t * d + + // q = v1 + s * r + // s = dot(q - v1, r) / dot(r, r) + val r = v2 - v1 + val rr = b2Dot(r, r) + + if (rr == 0.0) { + return RayCastOutput.MISS + } + + val s = b2Dot(q - v1, r) / rr + + if (s < 0.0 || 1.0 < s) { + return RayCastOutput.MISS + } + + val outputNormal: Vector2d + + if (numerator > 0.0) { + outputNormal = -b2Mul(transform.q, normal) + } else { + outputNormal = b2Mul(transform.q, normal) + } + + return RayCastOutput( + hit = true, + normal = outputNormal, + fraction = t + ) + } + + override fun computeAABB(transform: Transform, childIndex: Int): AABB { + val v1 = b2Mul(transform, vertex1) + val v2 = b2Mul(transform, vertex2) + + val lower = b2Min(v1, v2) + val upper = b2Max(v1, v2) + + val r = Vector2d(radius, radius) + + return AABB( + mins = lower - r, + maxs = upper + r + ) + } + + override fun computeMass(density: Double): MassData { + return MassData( + center = 0.5 * (vertex1 + vertex2), + mass = 0.0, + inertia = 0.0 + ) + } + + override var radius: Double = b2_polygonRadius + set(value) { + if (value == b2_polygonRadius) { + field = value + return + } + + throw UnsupportedOperationException("For polygonal shapes this must be b2_polygonRadius") + } + + /** + * Set this as a part of a sequence. Vertex [v0] precedes the edge and vertex [v3] + * follows. These extra vertices are used to provide smooth movement + * across junctions. This also makes the collision one-sided. The edge + * normal points to the right looking from [v1] to [v2]. + */ + fun setOneSided(v0: Vector2d, v1: Vector2d, v2: Vector2d, v3: Vector2d) { + oneSided = true + vertex0 = v0 + vertex1 = v1 + vertex2 = v2 + vertex3 = v3 + } + + /** + * Set this as an isolated edge. Collision is two-sided. + */ + fun setTwoSided(v1: Vector2d, v2: Vector2d) { + oneSided = false + vertex1 = v1 + vertex2 = v2 + } + + /// These are the edge vertices + var vertex1 = Vector2d.ZERO + set(value) { + if (!value.isFinite) { + throw IllegalArgumentException("Tried to set illegal vertex $value") + } + + field = value + } + + var vertex2 = Vector2d.ZERO + set(value) { + if (!value.isFinite) { + throw IllegalArgumentException("Tried to set illegal vertex $value") + } + + field = value + } + + /// Optional adjacent vertices. These are used for smooth collision. + var vertex0 = Vector2d.ZERO + set(value) { + if (!value.isFinite) { + throw IllegalArgumentException("Tried to set illegal vertex $value") + } + + field = value + } + + var vertex3 = Vector2d.ZERO + set(value) { + if (!value.isFinite) { + throw IllegalArgumentException("Tried to set illegal vertex $value") + } + + field = value + } + + var oneSided = false +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/PolygonShape.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/PolygonShape.kt new file mode 100644 index 00000000..864002bf --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/collision/shapes/PolygonShape.kt @@ -0,0 +1,449 @@ +package ru.dbotthepony.kbox2d.collision.shapes + +import it.unimi.dsi.fastutil.ints.IntArrayList +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times + +private const val inv3 = 1.0 / 3.0 + +private fun computeCentroid(vs: List): Vector2d { + 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 { + 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) { + 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() + + for ((i, v) in vertices.withIndex()) { + var unique = true + + for ((j, other) in ps.withIndex()) { + if (v.distanceSquared(other) < ((0.5 * b2_linearSlop) * (0.5 * b2_linearSlop))) { + unique = false + break + } + } + + if (unique) { + ps.add(v) + } + } + + check(ps.size >= 3) { "Polygon is degenerate" } + + // Create the convex hull using the Gift wrapping algorithm + // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm + + // Find the right most point on the hull + var i0 = 0 + var x0 = ps[0].x + + for (i in 1 until ps.size) { + val x = ps[i].x + + if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) { + i0 = i + x0 = x + } + } + + val hull = IntArrayList() + var ih = i0 + + while (true) { + hull.add(ih) + + var ie = 0 + for (j in 1 until ps.size) { + if (ie == ih) { + ie = j + continue + } + + val r = ps[ie] - ps[ih] + val v = ps[j] - ps[ih] + + val c = b2Cross(r, v) + + if (c < 0.0) { + ie = j + } + + // Collinearity check + if (c == 0.0 && v.lengthSquared > r.lengthSquared) { + ie = j + } + } + + ih = ie + + if (ie == i0) { + break + } + } + + check(hull.size >= 3) { "Polygon is degenerate" } + + // Copy vertices. + for (i in hull.indices) { + this.vertices.add(ps[hull.getInt(i)]) + } + + // Compute normals. Ensure the edges have non-zero length. + for (i in hull.indices) { + val i2 = if (i + 1 < hull.size) i + 1 else 0 + val edge = this.vertices[i2] - this.vertices[i] + check(edge.lengthSquared > b2_epsilon * b2_epsilon) + this.normals.add(b2Cross(edge, 1.0).normalized) + } + + // Compute the polygon centroid. + centroid = computeCentroid(this.vertices) + } + + /** + * Build vertices to represent an axis-aligned box centered on the local origin. + * @param hx the half-width. + * @param hy the half-height. + */ + fun setAsBox(hx: Double, hy: Double) { + vertices.clear() + normals.clear() + + vertices.add(Vector2d(-hx, -hy)) + vertices.add(Vector2d(hx, -hy)) + vertices.add(Vector2d(hx, hy)) + vertices.add(Vector2d(-hx, hy)) + + normals.add(Vector2d.DOWN) + normals.add(Vector2d.RIGHT) + normals.add(Vector2d.UP) + normals.add(Vector2d.LEFT) + + centroid = Vector2d.ZERO + } + + /** + * Build vertices to represent an oriented box. + * @param hx the half-width. + * @param hy the half-height. + * @param center the center of the box in local coordinates. + * @param angle the rotation of the box in local coordinates. + */ + fun setAsBox(hx: Double, hy: Double, center: Vector2d, angle: Double) { + vertices.clear() + normals.clear() + + vertices.add(Vector2d(-hx, -hy)) + vertices.add(Vector2d(hx, -hy)) + vertices.add(Vector2d(hx, hy)) + vertices.add(Vector2d(-hx, hy)) + + normals.add(Vector2d.DOWN) + normals.add(Vector2d.RIGHT) + normals.add(Vector2d.UP) + normals.add(Vector2d.LEFT) + + centroid = center + + val xf = Transform(center, angle) + + // Transform vertices and normals. + for (i in 0 until 4) { + vertices[i] = b2Mul(xf, vertices[i]) + normals[i] = b2Mul(xf.q, normals[i]) + } + } + + override fun testPoint(transform: Transform, p: Vector2d): Boolean { + val pLocal = b2MulT(transform.q, p - transform.p) + + for (i in 0 until vertices.size) { + val dot = b2Dot(normals[i], pLocal - vertices[i]) + + if (dot > 0.0) { + return false + } + } + + return true + } + + override val type: IShape.Type = IShape.Type.POLYGON + override val childCount: Int = 1 + + override fun rayCast(input: RayCastInput, transform: Transform, childIndex: Int): RayCastOutput { + // Put the ray into the polygon's frame of reference. + val p1 = b2MulT(transform.q, input.p1 - transform.p) + val p2 = b2MulT(transform.q, input.p2 - transform.p) + val d = p2 - p1 + + var lower = 0.0 + var upper = input.maxFraction + + var index = -1 + + for (i in 0 until vertices.size) { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + + val numerator = b2Dot(normals[i], vertices[i] - p1) + val denominator = b2Dot(normals[i], d) + + if (denominator == 0.0) { + if (numerator < 0.0) { + return RayCastOutput.MISS + } + } else { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + + if (denominator < 0.0 && numerator < lower * denominator) { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator + index = i + } else if (denominator > 0.0 && numerator < upper * denominator) { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator + } + } + + // The use of epsilon here causes the assert on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + //if (upper < lower - b2_epsilon) + + if (upper < lower) { + return RayCastOutput.MISS + } + } + + check(lower in 0.0 .. input.maxFraction) { "$lower <=> ${input.maxFraction}!" } + + if (index >= 0) { + return RayCastOutput( + hit = true, + fraction = lower, + normal = b2Mul(transform.q, normals[index]) + ) + } + + return RayCastOutput.MISS + } + + override fun computeAABB(transform: Transform, childIndex: Int): AABB { + var lower = b2Mul(transform, vertices[0]) + var upper = lower + + for (i in 1 until vertices.size) { + val v = b2Mul(transform, vertices[i]) + lower = b2Min(lower, v) + upper = b2Max(upper, v) + } + + val r = Vector2d(radius, radius) + + return AABB( + mins = lower - r, + maxs = upper + r + ) + } + + override fun computeMass(density: Double): MassData { + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.x = (1/mass) * rho * int(x * dA) + // centroid.y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + check(vertices.size >= 3) { vertices.size } + + var center = Vector2d.ZERO + var area = 0.0 + var I = 0.0 + + // Get a reference point for forming triangles. + // Use the first vertex to reduce round-off errors. + var s = vertices[0] + + for (i in 0 until vertices.size) { + // Triangle vertices. + val e1 = vertices[i] - s; + val e2 = if (i + 1 < vertices.size) vertices[i+1] - s else vertices[0] - s + + val D = b2Cross(e1, e2) + + val triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + center += triangleArea * inv3 * (e1 + e2); + + val ex1 = e1.x + val ey1 = e1.y + val ex2 = e2.x + val ey2 = e2.y + + val intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; + val inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; + + I += (0.25f * inv3 * D) * (intx2 + inty2); + } + + check(area > b2_epsilon) { "Area is too small: $area" } + + center *= 1.0 / area + val center2 = center + s + val mass = density * area + + return MassData( + // Total mass + mass = mass, + // Center of mass + center = center2, + // Inertia tensor relative to the local origin (point s). + // Shift to center of mass then to original body origin. + inertia = density * I + mass * (b2Dot(center2, center2) - b2Dot(center, center)), + ) + } + + /** + * Validate convexity. This is a very time consuming operation. + * @returns true if valid + */ + fun validate(doThrow: Boolean = false): Boolean { + for (i in 0 until vertices.size) { + val i1 = i + val i2 = if (i < vertices.size - 1) i1 + 1 else 0 + val p = vertices[i1] + val e = vertices[i2] - p + + if (!p.isFinite) { + if (doThrow) { + throw IllegalStateException("Vertex at $i1 is not finite") + } + + return false + } + + if (!e.isFinite) { + if (doThrow) { + throw IllegalStateException("Vertex at $i2 is not finite") + } + + return false + } + + for (j in 0 until vertices.size) { + if (j == i1 || j == i2) { + continue + } + + val v = vertices[j] - p + val c = b2Cross(e, v) + + if (c < 0.0) { + if (doThrow) { + throw IllegalStateException("Vertex at $j form concave shape") + } + + return false + } + } + } + + return true + } + + internal var centroid = Vector2d.ZERO + internal val vertices = ArrayList() + internal val normals = ArrayList() + + 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") + } +} \ No newline at end of file diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/B2World.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/B2World.kt new file mode 100644 index 00000000..e3594d07 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/B2World.kt @@ -0,0 +1,916 @@ +package ru.dbotthepony.kbox2d.dynamics + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.collision.DistanceProxy +import ru.dbotthepony.kbox2d.collision.b2TimeOfImpact +import ru.dbotthepony.kbox2d.collision.shapes.ChainShape +import ru.dbotthepony.kbox2d.collision.shapes.CircleShape +import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape +import ru.dbotthepony.kbox2d.dynamics.contact.AbstractContact +import ru.dbotthepony.kbox2d.dynamics.internal.Island +import ru.dbotthepony.kbox2d.dynamics.joint.AbstractJoint +import ru.dbotthepony.kstarbound.math.AABB +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times +import ru.dbotthepony.kstarbound.util.Color + +class B2World(override var gravity: Vector2d) : IB2World { + override var bodyCount: Int = 0 + private set + override var jointCount: Int = 0 + private set + + override val contactManager: IContactManager = ContactManager() + + override var destructionListener: IDestructionListener? = null + override var contactFilter: IContactFilter? by contactManager::contactFilter + override var contactListener: IContactListener? by contactManager::contactListener + + override var debugDraw: IDebugDraw? = null + + override var bodyList: IBody? = null + private set + override var jointList: IJoint? = null + private set + + override var warmStarting: Boolean = true + override var continuousPhysics: Boolean = true + override var enableSubStepping: Boolean = false + + override var allowAutoSleep: Boolean = true + set(value) { + if (value == field) + return + + field = value + + if (!value) { + for (body in bodyListIterator) { + body.isAwake = true + } + } + } + + override var isLocked: Boolean = false + private set + + override var autoClearForces: Boolean = true + + private var stepComplete = true + private var newContacts = false + + private val profile = ProfileData() + + override fun notifyNewContacts() { + newContacts = true + } + + override fun createBody(bodyDef: BodyDef): IBody { + if (isLocked) + throw ConcurrentModificationException() + + val body = Body(bodyDef, this) + + body.next = bodyList + (bodyList as Body?)?.prev = body + bodyList = body + bodyCount++ + + return body + } + + override fun destroyBody(body: IBody) { + if (isLocked) + throw ConcurrentModificationException() + + check(body.world == this) { "$body does not belong to $this" } + check(bodyCount > 0) { "I have ${bodyCount} bodies, can't remove one" } + + // Delete the attached joints. + for (jointEdge in body.jointIterator) { + destructionListener?.sayGoodbye(jointEdge.joint) + destroyJoint(jointEdge.joint) + } + + // Delete the attached contacts. + for (contactEdge in body.contactEdgeIterator) { + contactManager.destroy(contactEdge.contact) + } + + // Delete the attached fixtures. This destroys broad-phase proxies. + for (fixture in body.fixtureIterator) { + destructionListener?.sayGoodbye(fixture) + (fixture as Fixture).destroyProxies(contactManager.broadPhase) + } + + // Remove world body list. + val prev = body.prev as Body? + val next = body.next as Body? + + prev?.next = next + next?.prev = prev + + if (body == bodyList) { + bodyList = next ?: prev + } + + bodyCount-- + (body as Body).unlink() + } + + override fun createJoint(jointDef: IJointDef): AbstractJoint { + if (isLocked) + throw ConcurrentModificationException() + + val joint = AbstractJoint.create(jointDef.type, jointDef) + + // Connect to the world list. + joint.next = jointList + (jointList as AbstractJoint?)?.prev = joint + jointList = joint + jointCount++ + + if (joint.hasTwoBodies) { + val bodyA = joint.bodyA + val bodyB = joint.bodyB + + // If the joint prevents collisions, then flag any contacts for filtering. + if (!joint.collideConnected) { + for (edge in bodyB.contactEdgeIterator) { + if (edge.other == bodyA) { + edge.contact.flagForFiltering() + } + } + } + } + + // Note: creating a joint doesn't wake the bodies. + + return joint + } + + override fun destroyJoint(joint: IJoint) { + if (isLocked) + throw ConcurrentModificationException() + + check(jointCount > 0) { "No joints tracked to remove" } + joint as AbstractJoint + require((joint.nullableBodyA?.world == this || joint.nullableBodyA == null) && (joint.nullableBodyB?.world == this || joint.nullableBodyB == null)) { "$joint does not belong to $this" } + + if (!joint.isValid) { + throw IllegalStateException("Joint $joint is already destroyed") + } + + // Remove from the doubly linked list. + this.run { + val prev = joint.prev as AbstractJoint? + val next = joint.next as AbstractJoint? + + prev?.next = next + next?.prev = prev + + if (joint == this.jointList) { + this.jointList = next ?: prev + } + } + + // Disconnect from island graph. + val bodyA = joint.nullableBodyA + val bodyB = joint.nullableBodyB + + // Wake up connected bodies. + bodyA?.isAwake = true + bodyB?.isAwake = true + + // Remove from body 1. + this.run { + val edgeA = joint.edgeA + + val prev = edgeA.prev + val next = edgeA.next + + prev?.next = next + next?.prev = prev + + if (bodyA?.jointList == edgeA) { + bodyA.jointList = next ?: prev + } + } + + // Remove from body 2 + this.run { + val edgeB = joint.edgeB + + val prev = edgeB.prev + val next = edgeB.next + + prev?.next = next + next?.prev = prev + + if (bodyB?.jointList == edgeB) { + bodyB.jointList = next ?: prev + } + } + + jointCount-- + joint.unlink() + + if (!joint.collideConnected && bodyB != null && bodyA != null) { + for (edge in bodyB.contactEdgeIterator) { + if (edge.other == bodyA) { + edge.contact.flagForFiltering() + } + } + } + } + + private val _profile = ProfileData() + + /** + * Find islands, integrate and solve constraints, solve position constraints + */ + internal fun solve(step: B2TimeStep) { + _profile.solveInit = 0L + _profile.solveVelocity = 0L + _profile.solvePosition = 0L + + // Size the island for the worst case. + // TODO: Kotlin: or do we size it?? + val island = Island(listener = contactListener) + + // Clear all the island flags. + for (body in bodyListIterator) { + body as Body + body.isOnIsland = false + } + + for (contact in contactListIterator) { + contact as AbstractContact + contact.isOnIsland = false + } + + for (joint in jointListIterator) { + joint as AbstractJoint + check(joint.isValid) { "$joint is no longer valid, but present in linked list" } + joint.isOnIsland = false + } + + // Build and simulate all awake islands. + for (seed in bodyListIterator) { + seed as Body + + if (seed.type == BodyType.STATIC || !seed.isAwake || !seed.isEnabled || seed.isOnIsland) { + continue + } + + // Reset island and stack. + island.clear() + val stack = ArrayDeque(32) + stack.add(seed) + seed.isOnIsland = true + + // Perform a depth first search (DFS) on the constraint graph. + while (stack.isNotEmpty()) { + // Grab the next body off the stack and add it to the island. + val body = stack.removeLast() + check(body.isEnabled) + island.add(body) + + // To keep islands as small as possible, we don't + // propagate islands across static bodies. + if (body.type == BodyType.STATIC) + continue + + // Make sure the body is awake (without resetting sleep timer). + body.flags = BodyFlags.AWAKE.or(body.flags) + + // Search all contacts connected to this body. + for (ce in body.contactEdgeIterator) { + val contact = ce.contact as AbstractContact + + // Has this contact already been added to an island? + if (contact.isOnIsland) + continue + + // Is this contact solid and touching? + if (!contact.isEnabled || !contact.isTouching) + continue + + // Skip sensors. + if (contact.fixtureA.isSensor || contact.fixtureB.isSensor) + continue + + island.add(contact) + contact.isOnIsland = true + + val other = ce.other as Body + + // Was the other body already added to this island? + if (!other.isOnIsland) { + stack.add(other) + other.isOnIsland = true + } + } + + // Search all joints connect to this body. + for (je in body.jointIterator) { + val joint = je.joint as AbstractJoint + check(joint.isValid) { "$joint is no longer valid, but present in linked list of $body" } + + if (joint.isOnIsland) + continue + + val other = je.otherNullable as Body? + + // Don't simulate joints connected to disabled bodies. + if (other != null && !other.isEnabled) + continue + + island.add(joint) + joint.isOnIsland = true + + if (other != null && !other.isOnIsland) { + stack.add(other) + other.isOnIsland = true + } + } + } + + val profile = ProfileData() + island.solve(profile, step, gravity, allowAutoSleep) + this.profile.solveInit += profile.solveInit + this.profile.solveVelocity += profile.solveVelocity + this.profile.solvePosition += profile.solvePosition + this.profile.integratePositions += profile.integratePositions + + // Post solve cleanup. + for (body in island.bodiesAccess) { + // Allow static bodies to participate in other islands. + if (body.type == BodyType.STATIC) { + body.isOnIsland = false + } + } + } + + val timer = System.nanoTime() + + // Synchronize fixtures, check for out of range bodies. + for (body in bodyListIterator) { + body as Body + + // If a body was not in an island then it did not move. + if (!body.isOnIsland || body.type == BodyType.STATIC) { + continue + } + + // Update fixtures (for broad-phase). + body.synchronizeFixtures() + } + + // Look for new contacts. + contactManager.findNewContacts() + profile.broadphase = System.nanoTime() - timer + } + + /** + * Find TOI contacts and solve them. + */ + fun solveTOI(step: B2TimeStep) { + val island = Island(listener = contactListener) + + if (stepComplete) { + for (body in bodyListIterator) { + body as Body + body.isOnIsland = false + body.sweep.alpha0 = 0.0 + } + + for (c in contactManager.contactListIterator) { + // Invalidate TOI + c as AbstractContact + c.isOnIsland = false + c.toiFlag = false + c.toiCount = 0 + c.toi = 1.0 + } + } + + // Find TOI events and solve them. + while (true) { + var minContact: AbstractContact? = null + var minAlpha = 1.0 + + for (c in contactManager.contactListIterator) { + if (!c.isEnabled) { + continue + } + + c as AbstractContact + + if (c.toiCount > b2_maxSubSteps) { + continue + } + + var alpha = 1.0 + + if (c.toiFlag) { + // This contact has a valid cached TOI. + alpha = c.toi + } else { + val fA = c.fixtureA + val fB = c.fixtureB + + // Is there a sensor? + if (fA.isSensor || fB.isSensor) { + continue + } + + val bA = fA.body as Body + val bB = fB.body as Body + + val typeA = bA.type + val typeB = bB.type + + check(typeA == BodyType.DYNAMIC || typeB == BodyType.DYNAMIC) + + val activeA = bA.isAwake && typeA != BodyType.STATIC + val activeB = bB.isAwake && typeB != BodyType.STATIC + + // Is at least one body active (awake and dynamic or kinematic)? + if (!activeA && !activeB) { + continue + } + + val collideA = bA.isBullet || typeA != BodyType.DYNAMIC + val collideB = bB.isBullet || typeB != BodyType.DYNAMIC + + // Are these two non-bullet dynamic bodies? + if (!collideA && !collideB) { + continue + } + + // Compute the TOI for this contact. + // Put the sweeps onto the same time interval. + var alpha0 = bA.sweep.alpha0 + + if (bA.sweep.alpha0 < bB.sweep.alpha0) { + alpha0 = bB.sweep.alpha0 + bA.sweep.advance(alpha0) + } else if (bA.sweep.alpha0 > bB.sweep.alpha0) { + alpha0 = bA.sweep.alpha0 + bB.sweep.advance(alpha0) + } + + check(alpha0 < 1.0) { alpha0 } + + val indexA = c.childIndexA + val indexB = c.childIndexB + + // Compute the time of impact in interval [0, minTOI] + val output = b2TimeOfImpact( + proxyA = DistanceProxy(fA.shape, indexA), + proxyB = DistanceProxy(fB.shape, indexB), + _sweepA = bA.sweep, + _sweepB = bB.sweep, + tMax = 1.0 + ) + + // Beta is the fraction of the remaining portion of the . + val beta = output.t + + if (output.state == TOIOutput.State.TOUCHING) { + alpha = 1.0.coerceAtMost(alpha0 + (1.0f - alpha0) * beta) + } else { + alpha = 1.0 + } + + c.toi = alpha + c.toiFlag = true + } + + if (alpha < minAlpha) { + // This is the minimum TOI found so far. + minContact = c + minAlpha = alpha + } + } + + if (minContact == null || 1.0 - 10.0 * b2_epsilon < minAlpha) { + // No more TOI events. Done! + stepComplete = true + break + } + + // Advance the bodies to the TOI. + val fA = minContact.fixtureA + val fB = minContact.fixtureB + val bA = fA.body as Body + val bB = fB.body as Body + + val backup1 = bA.sweep.copy() + val backup2 = bB.sweep.copy() + + bA.advance(minAlpha) + bB.advance(minAlpha) + + // The TOI contact likely has some new contact points. + minContact.update(contactManager.contactListener) + minContact.toiFlag = false + minContact.toiCount++ + + // Is the contact solid? + if (!minContact.isEnabled || !minContact.isTouching) { + // Restore the sweeps. + minContact.isEnabled = false + bA.sweep.load(backup1) + bB.sweep.load(backup2) + bA.synchronizeTransform() + bB.synchronizeTransform() + continue + } + + bA.isAwake = true + bB.isAwake = true + + // Build the island + island.clear() + island.add(bA) + island.add(bB) + island.add(minContact) + + bA.isOnIsland = true + bB.isOnIsland = true + minContact.isOnIsland = true + + // Get contacts on bodyA and bodyB. + val bodies = arrayOf(bA, bB) + for (i in 0 .. 1) { + val body = bodies[i] + + if (body.type == BodyType.DYNAMIC) { + for (ce in body.contactEdgeIterator) { + val contact = ce.contact as AbstractContact + + // Has this contact already been added to the island? + if (contact.isOnIsland) { + continue + } + + // Only add static, kinematic, or bullet bodies. + val other = ce.other + + if (other.type == BodyType.DYNAMIC && !body.isBullet && !other.isBullet) { + continue + } + + // Skip sensors. + if (contact.fixtureA.isSensor || contact.fixtureB.isSensor) { + continue + } + + other as Body + + // Tentatively advance the body to the TOI. + val backup = other.sweep + if (!other.isOnIsland) { + other.advance(minAlpha) + } + + // Update the contact points + contact.update(contactManager.contactListener) + + // Was the contact disabled by the user? + if (!contact.isEnabled) { + other.sweep.load(backup) + other.synchronizeTransform() + continue + } + + // Are there contact points? + if (!contact.isTouching) { + other.sweep.load(backup) + other.synchronizeTransform() + continue + } + + // Add the contact to the island + contact.isOnIsland = true + island.add(contact) + + // Has the other body already been added to the island? + if (other.isOnIsland) { + continue + } + + // Add the other body to the island. + other.isOnIsland = true + + if (other.type != BodyType.STATIC) { + other.isAwake = true + } + + island.add(other) + } + } + } + + val subStepDT = (1.0 - minAlpha) * step.dt + + val subStep = B2TimeStep( + dt = subStepDT, + inv_dt = 1.0 / subStepDT, + dtRatio = 1.0, + positionIterations = 20, + velocityIterations = step.velocityIterations, + warmStarting = false, + ) + + island.solveTOI(subStep, bA.islandIndex, bB.islandIndex) + + // Reset island flags and synchronize broad-phase proxies. + for (body in island.bodiesAccess) { + body.isOnIsland = false + if (body.type != BodyType.DYNAMIC) { + continue + } + + body.synchronizeFixtures() + + // Invalidate all contact TOIs on this displaced body. + for (ce in body.contactEdgeIterator) { + val contact = ce.contact as AbstractContact + contact.toiFlag = false + contact.isOnIsland = false + } + } + + // Commit fixture proxy movements to the broad-phase so that new contacts are created. + // Also, some contacts can be destroyed. + contactManager.findNewContacts() + + if (enableSubStepping) { + stepComplete = false + break + } + } + } + + private var m_inv_dt0 = 0.0 + + override fun step(dt: Double, velocityIterations: Int, positionIterations: Int) { + var stepTimer = System.nanoTime() + + // If new fixtures were added, we need to find the new contacts. + if (newContacts) { + contactManager.findNewContacts() + newContacts = false + } + + isLocked = true + + try { + val inv_dt: Double + + if (dt > 0.0) { + inv_dt = 1.0 / dt + } else { + inv_dt = 0.0 + } + + val step = B2TimeStep( + dt = dt, + inv_dt = inv_dt, + dtRatio = m_inv_dt0 * dt, + warmStarting = warmStarting, + velocityIterations = velocityIterations, + positionIterations = positionIterations + ) + + // Update contacts. This is where some contacts are destroyed. + this.run { + val timer = System.nanoTime() + this.contactManager.collide() + this.profile.collide = System.nanoTime() - timer + } + + // Integrate velocities, solve velocity constraints, and integrate positions. + if (stepComplete && dt > 0.0) { + val timer = System.nanoTime() + solve(step) + profile.solve = System.nanoTime() - timer + } + + // Handle TOI events. + if (continuousPhysics && dt > 0.0) { + val timer = System.nanoTime() + solveTOI(step) + profile.solveTOI = System.nanoTime() - timer + } + + if (dt > 0.0) { + m_inv_dt0 = step.inv_dt + } + + if (autoClearForces) { + clearForces() + } + } catch(err: Throwable) { + throw RuntimeException("Caught an exception simulating physics world", err) + } finally { + isLocked = false + } + + profile.step = System.nanoTime() - stepTimer + } + + override fun clearForces() { + for (body in bodyListIterator) { + body as Body + body.force = Vector2d.ZERO + body.torque = 0.0 + } + } + + override fun queryAABB(aabb: AABB, callback: IQueryCallback) { + contactManager.broadPhase.query(aabb) { nodeId, userData -> callback.reportFixture((userData as FixtureProxy).fixture) } + } + + override fun rayCast(point1: Vector2d, point2: Vector2d, callback: IRayCastCallback) { + val input = RayCastInput(point1, point2, 1.0) + + contactManager.broadPhase.rayCast(input, object : ProxyRayCastCallback { + override fun invoke(subInput: RayCastInput, nodeId: Int, userData: Any?): Double { + val proxy = userData as FixtureProxy + val fixture = proxy.fixture + val index = proxy.childIndex + val output = fixture.rayCast(subInput, index) + + if (output.hit) { + val point = (1.0 - output.fraction) * subInput.p1 + output.fraction * subInput.p2 + return callback.reportFixture(fixture, point, output.normal, output.fraction) + } + + return subInput.maxFraction + } + }) + } + + private fun drawShape(fixture: IFixture, xf: Transform, color: Color) { + when (fixture.type) { + IShape.Type.CIRCLE -> { + val circle = fixture.shape as CircleShape + val center = b2Mul(xf, circle.p) + val radius = circle.radius + val axis = b2Mul(xf.q, Vector2d.RIGHT) + + debugDraw?.drawSolidCircle(center, radius, axis, color) + } + + IShape.Type.EDGE -> { + val edge = fixture.shape as EdgeShape + val v1 = b2Mul(xf, edge.vertex1) + val v2 = b2Mul(xf, edge.vertex2) + + debugDraw?.drawSegment(v1, v2, color) + + if (!edge.oneSided) { + debugDraw?.drawPoint(v1, 4.0, color) + debugDraw?.drawPoint(v2, 4.0, color) + } + } + + IShape.Type.POLYGON -> { + val poly = fixture.shape as PolygonShape + val vertices = poly.vertices.map { b2Mul(xf, it) } + debugDraw?.drawSolidPolygon(vertices, color) + } + + IShape.Type.CHAIN -> { + val chain = fixture.shape as ChainShape + var v1 = b2Mul(xf, chain.vertices[0]) + + for (i in 1 until chain.vertices.size) { + val v2 = b2Mul(xf, chain.vertices[i]) + debugDraw?.drawSegment(v1, v2, color) + v1 = v2 + } + } + } + } + + override fun debugDraw() { + val debugDraw = debugDraw ?: return + + if (debugDraw.drawShapes) { + for (body in bodyListIterator) { + val xf = body.transform + + for (f in body.fixtureIterator) { + if (body.type == BodyType.DYNAMIC && body.mass == 0.0) { + // Bad body + drawShape(f, xf, BAD_BODY_COLOR) + } else if (!body.isEnabled) { + drawShape(f, xf, DISABLED_BODY_COLOR) + } else if (body.type == BodyType.STATIC) { + drawShape(f, xf, STATIC_BODY_COLOR) + } else if (body.type == BodyType.KINEMATIC) { + drawShape(f, xf, KINEMATIC_BODY_COLOR) + } else if (!body.isAwake) { + drawShape(f, xf, SLEEPING_BODY_COLOR) + } else { + drawShape(f, xf, NORMAL_BODY_COLOR) + } + } + } + } + + if (debugDraw.drawJoints) { + for (joint in jointListIterator) { + joint.draw(debugDraw) + } + } + + if (debugDraw.drawPairs) { + for (c in contactManager.contactListIterator) { + val fixtureA = c.fixtureA + val fixtureB = c.fixtureB + val indexA = c.childIndexA + val indexB = c.childIndexB + val cA = fixtureA.getAABB(indexA).centre + val cB = fixtureB.getAABB(indexB).centre + + debugDraw.drawSegment(cA, cB, PAIR_COLOR) + } + } + + if (debugDraw.drawAABB) { + for (body in bodyListIterator) { + if (!body.isEnabled) { + continue + } + + for (f in body.fixtureIterator) { + for (proxy in f.proxies) { + val aabb = contactManager.broadPhase.getFatAABB(proxy.proxyId) + + debugDraw.drawPolygon(listOf( + aabb.A, aabb.B, aabb.C, aabb.D + ), AABB_COLOR + ) + } + } + } + } + + if (debugDraw.drawCenterOfMess) { + for (body in bodyListIterator) { + val xf = Transform(body.transform.position, body.transform.rotation.angle) + xf.p = body.worldCenter + debugDraw.drawTransform(xf) + } + } + } + + companion object { + private val BAD_BODY_COLOR = Color(1f, 0f, 0f) + private val DISABLED_BODY_COLOR = Color(0.5f, 0.5f, 0.3f) + private val STATIC_BODY_COLOR = Color(0.5f, 0.9f, 0.5f) + private val KINEMATIC_BODY_COLOR = Color(0.5f, 0.5f, 0.9f) + private val SLEEPING_BODY_COLOR = Color(0.6f, 0.6f, 0.6f) + private val NORMAL_BODY_COLOR = Color(0.9f, 0.7f, 0.7f) + private val PAIR_COLOR = Color(0.3f, 0.9f, 0.9f) + private val AABB_COLOR = Color(0.9f, 0.3f, 0.9f) + } + + override fun shiftOrigin(newOrigin: Vector2d) { + if (isLocked) + throw ConcurrentModificationException() + + isLocked = true + + try { + for (body in bodyListIterator) { + body as Body + body.transform.p -= newOrigin + body.sweep.c0 -= newOrigin + body.sweep.c -= newOrigin + } + + for (joint in jointListIterator) { + joint.shiftOrigin(newOrigin) + } + + contactManager.broadPhase.shiftOrigin(newOrigin) + } finally { + isLocked = false + } + } + + override val profileData: IProfileData + get() = profile.snapshot() + + override fun dump() { + TODO("Not yet implemented") + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/Body.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/Body.kt new file mode 100644 index 00000000..e82d44fc --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/Body.kt @@ -0,0 +1,622 @@ +package ru.dbotthepony.kbox2d.dynamics + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kstarbound.math.Vector2d + +open class Body(def: BodyDef, world: IB2World) : IBody { + private var _world: IB2World? = world + + final override val world: IB2World + get() = _world ?: throw IllegalStateException("Tried to use removed body") + + internal var flags: Int = 0 + internal var isOnIsland = false + internal var islandIndex: Int = 0 + + internal fun unlink() { + _world = null + } + + init { + def.validate() + + if (def.bullet) { + flags = BodyFlags.BULLET.or(flags) + } + + if (def.fixedRotation) { + flags = BodyFlags.FIXED_ROTATION.or(flags) + } + + if (def.allowSleep) { + flags = BodyFlags.AUTO_SLEEP.or(flags) + } + + if (def.awake && def.type != BodyType.STATIC) { + flags = BodyFlags.AWAKE.or(flags) + } + + if (def.enabled) { + flags = BodyFlags.ENABLED.or(flags) + } + } + + internal val sweep: Sweep = Sweep() + + final override val transform: Transform = Transform(def.position, def.angle) + val xf by this::transform + + override val position: Vector2d + get() = transform.position + override val angle: Double + get() = sweep.a + + override val userData: Any? = def.userData + + internal var sleepTime: Double = 0.0 + internal var torque: Double = 0.0 + internal var force: Vector2d = Vector2d.ZERO + + override var linearVelocity: Vector2d = def.linearVelocity + set(value) { + if (type == BodyType.STATIC) + return + + if (value.dotProduct(value) > 0.0) + isAwake = true + + field = value + } + + override var angularVelocity: Double = def.angularVelocity + set(value) { + if (type == BodyType.STATIC) + return + + if (value * value > 0.0) + isAwake = true + + field = value + } + + override var fixtureList: Fixture? = null + protected set + + protected var fixtureCount: Int = 0 + + override var jointList: JointEdge? = null + internal set + override var contactEdge: ContactEdge? = null + internal set + + override var next: IBody? = null + internal set + override var prev: IBody? = null + internal set + + override var linearDamping: Double = def.linearDamping + override var angularDamping: Double = def.angularDamping + override var gravityScale: Double = def.gravityScale + + override var mass: Double = 0.0 + protected set + + internal var invMass: Double = 0.0 + + /** + * Rotational inertia about the center of mass. + */ + internal var rotInertia: Double = 0.0 + + /** + * Rotational inertia about the center of mass. + */ + internal var rotInertiaInv: Double = 0.0 + + internal var I by this::rotInertia + internal var invI by this::rotInertiaInv + + override var isBullet: Boolean + get() = BodyFlags.BULLET.isit(flags) + set(value) { flags = BodyFlags.BULLET.update(flags, value) } + + override var isAwake: Boolean + get() = BodyFlags.AWAKE.isit(flags) + set(value) { + if (type == BodyType.STATIC || BodyFlags.AWAKE.isit(flags) == value) + return + + if (value) { + flags = flags or BodyFlags.AWAKE.bitmask + sleepTime = 0.0 + } else { + flags = flags and BodyFlags.AWAKE.bitmask.inv() + sleepTime = 0.0 + linearVelocity = Vector2d.ZERO + angularVelocity = 0.0 + force = Vector2d.ZERO + torque = 0.0 + } + } + + override var isEnabled: Boolean + get() = BodyFlags.ENABLED.isit(flags) + set(value) { + if (world.isLocked) + throw ConcurrentModificationException() + + if (value == isEnabled) + return + + if (value) { + flags = BodyFlags.ENABLED.not(flags) + + // Create all proxies. + val broadPhase = world.contactManager.broadPhase + + for (fixture in fixtureIterator) { + (fixture as Fixture).createProxies(broadPhase, transform) + } + + world.notifyNewContacts() + } else { + flags = BodyFlags.ENABLED.or(flags) + + // Destroy all proxies. + val broadPhase = world.contactManager.broadPhase + + for (fixture in fixtureIterator) { + (fixture as Fixture).destroyProxies(broadPhase) + } + + // Destroy the attached contacts. + for (edge in contactEdgeIterator) { + world.contactManager.destroy(edge.contact) + } + + contactEdge = null + } + } + + override var isFixedRotation: Boolean + get() = BodyFlags.FIXED_ROTATION.isit(flags) + set(value) { + if (value == isFixedRotation) + return + + flags = BodyFlags.FIXED_ROTATION.update(flags, value) + + angularVelocity = 0.0 + resetMassData() + } + + override var allowAutoSleep: Boolean + get() = BodyFlags.AUTO_SLEEP.isit(flags) + set(value) { + flags = BodyFlags.AUTO_SLEEP.update(flags, value) + + if (!value) { + isAwake = true + } + } + + override var type: BodyType = def.type + set(value) { + if (world.isLocked) + throw ConcurrentModificationException() + + field = value + + resetMassData() + + if (value == BodyType.STATIC) { + linearVelocity = Vector2d.ZERO + angularVelocity = 0.0 + sweep.a0 = sweep.a + sweep.c0 = sweep.c + flags = BodyFlags.AWAKE.not(flags) + synchronizeFixtures() + } + + isAwake = true + + force = Vector2d.ZERO + torque = 0.0 + + var edge = contactEdge + + while (edge != null) { + // TODO: проверить, что делает destroy + world.contactManager.destroy(edge.contact) + edge = edge.next + } + + contactEdge = null + + val broadPhase = world.contactManager.broadPhase + var f: IFixture? = fixtureList + + while (f != null) { + for (proxy in f.proxies) { + broadPhase.touchProxy(proxy.proxyId) + } + + f = f.next + } + } + + override fun createFixture(def: FixtureDef): IFixture { + if (world.isLocked) + throw ConcurrentModificationException() + + val fixture = Fixture(this, def) + + if (isEnabled) { + fixture.createProxies(world.contactManager.broadPhase, transform) + } + + fixture.next = fixtureList + fixtureList = fixture + fixtureCount++ + + // Adjust mass properties if needed. + if (fixture.density > 0.0) { + resetMassData() + } + + // Let the world know we have a new fixture. This will cause new contacts + // to be created at the beginning of the next time step. + world.notifyNewContacts() + + return fixture + } + + override fun destroyFixture(fixture: IFixture) { + if (world.isLocked) + throw ConcurrentModificationException() + + require(fixture.body == this) { "$fixture does not belong to $this (belongs to ${fixture.body})" } + check(fixtureCount > 0) { "Having no tracked fixtures, but $fixture belongs to us" } + + var node: IFixture? = fixtureList + var found = false + var previous: IFixture? = null + + while (node != null) { + if (node == fixture) { + // TODO: Это должно работать + (previous as Fixture?)?.next = node.next + found = true + break + } else { + previous = node + node = node.next + } + } + + check(found) { "Can't find $fixture in linked list of fixtures" } + + val density = node!!.density + + // Destroy any contacts associated with the fixture. + var edge = contactEdge + + while (edge != null) { + val contact = edge.contact + edge = edge.next + + val fixtureA = contact.fixtureA + val fixtureB = contact.fixtureB + + if (fixtureA == fixture || fixtureB == fixture) { + // This destroys the contact and removes it from + // this body's contact list. + world.contactManager.destroy(contact) + } + } + + fixture as Fixture + + if (isEnabled) { + fixture.destroyProxies(world.contactManager.broadPhase) + } + + fixture.unlink() + fixtureCount-- + + // Reset the mass data + if (density > 0.0) { + resetMassData() + } + } + + override fun resetMassData() { + // Compute mass data from shapes. Each shape has its own density. + mass = 0.0 + invMass = 0.0 + rotInertia = 0.0 + rotInertiaInv = 0.0 + sweep.localCenter = Vector2d.ZERO + + if (type == BodyType.STATIC || type == BodyType.KINEMATIC) { + sweep.c0 = transform.position + sweep.c = transform.position + sweep.a0 = sweep.a + return + } + + check(type == BodyType.DYNAMIC) + + // Accumulate mass over all fixtures. + var localCenter = Vector2d.ZERO + + for (fixture in fixtureIterator) { + if (fixture.density == 0.0) { + continue + } + + val massData = fixture.getMassData() + mass += massData.mass + localCenter += massData.center * massData.mass + rotInertia += massData.inertia + } + + // Compute center of mass. + if (mass > 0.0) { + invMass = 1.0 / mass + localCenter *= invMass + } + + if (rotInertia > 0.0 && !isFixedRotation) { + // Center the inertia about the center of mass. + rotInertia -= mass * localCenter.dotProduct(localCenter) + check(rotInertia > 0.0) + rotInertiaInv = 1.0 / rotInertia + } else { + rotInertia = 0.0 + rotInertiaInv = 0.0 + } + + // Move center of mass. + val oldCenter = sweep.c + sweep.localCenter = localCenter + sweep.c = transform.times(localCenter) + sweep.c0 = sweep.c + + // Update center of mass velocity. + linearVelocity += b2Cross(angularVelocity, sweep.c - oldCenter) + } + + override var massData: MassData + get() = MassData( + mass = mass, + inertia = inertia, + center = sweep.localCenter + ) + set(value) { + if (world.isLocked) + throw ConcurrentModificationException() + + if (type != BodyType.DYNAMIC) + return + + invMass = 0.0 + rotInertia = 0.0 + rotInertiaInv = 0.0 + + mass = value.mass + + if (mass <= 0.0) { + mass = 1.0 + } + + invMass = 1.0 / mass + + if (value.inertia > 0.0 && !isFixedRotation) { + rotInertia = value.inertia - mass * value.center.dotProduct(value.center) + check(rotInertia > 0.0) + rotInertiaInv = 1.0 / rotInertia + } + + // Move center of mass. + val oldCenter = sweep.c + sweep.localCenter = value.center + sweep.c = transform.times(sweep.localCenter) + sweep.c0 = sweep.c + + // Update center of mass velocity. + linearVelocity += b2Cross(angularVelocity, sweep.c - oldCenter) + } + + internal fun shouldCollide(other: IBody): Boolean { + // At least one body should be dynamic. + if (type != BodyType.DYNAMIC && other.type != BodyType.DYNAMIC) + return false + + // Does a joint prevent collision? + var joint = jointList + + while (joint != null) { + if (joint.otherNullable == other && !joint.joint.collideConnected) { + return false + } + + joint = joint.next + } + + return true + } + + init { + sweep.c0 = transform.position + sweep.c = transform.position + sweep.a0 = def.angle + sweep.a = def.angle + } + + override fun setTransform(position: Vector2d, angle: Double) { + if (world.isLocked) + throw ConcurrentModificationException() + + transform.rotation.set(angle) + transform.position = position + + sweep.c = transform.times(sweep.localCenter) + sweep.a = angle + + sweep.c0 = sweep.c + sweep.a0 = angle + + val broadPhase = world.contactManager.broadPhase + + for (fixture in fixtureIterator) { + (fixture as Fixture?)?.synchronize(broadPhase, transform, transform) + } + + // Check for new contacts the next step + world.notifyNewContacts() + } + + override val inertia: Double + get() = rotInertia + mass * sweep.localCenter.dotProduct(sweep.localCenter) + + override val localCenter: Vector2d + get() = sweep.localCenter + + override val worldCenter: Vector2d + get() = sweep.c + + override fun getWorldPoint(localPoint: Vector2d): Vector2d { + return transform.times(localPoint) + } + + override fun getWorldVector(localPoint: Vector2d): Vector2d { + return transform.rotation.times(localPoint) + } + + override fun getLocalPoint(worldPoint: Vector2d): Vector2d { + return transform.timesT(worldPoint) + } + + override fun getLocalVector(worldVector: Vector2d): Vector2d { + return transform.rotation.timesT(worldVector) + } + + override fun getLinearVelocityFromWorldPoint(worldPoint: Vector2d): Vector2d { + return linearVelocity + b2Cross(angularVelocity, worldPoint - sweep.c) + } + + override fun getLinearVelocityFromLocalPoint(localPoint: Vector2d): Vector2d { + return getLinearVelocityFromWorldPoint(getWorldPoint(localPoint)) + } + + override fun applyForce(force: Vector2d, point: Vector2d, wake: Boolean) { + if (type != BodyType.DYNAMIC) + return + + if (wake && !isAwake) + isAwake = true + + // Don't accumulate a force if the body is sleeping. + if (isAwake) { + this.force += force + this.torque += b2Cross(point - sweep.c, force) + } + } + + override fun applyForceToCenter(force: Vector2d, wake: Boolean) { + if (type != BodyType.DYNAMIC) + return + + if (wake && !isAwake) + isAwake = true + + // Don't accumulate a force if the body is sleeping. + if (isAwake) { + this.force += force + } + } + + override fun applyTorque(torque: Double, wake: Boolean) { + if (type != BodyType.DYNAMIC) + return + + if (wake && !isAwake) + isAwake = true + + if (isAwake) { + this.torque += torque + } + } + + override fun applyLinearImpulse(impulse: Vector2d, point: Vector2d, wake: Boolean) { + if (type != BodyType.DYNAMIC) + return + + if (wake && !isAwake) + isAwake = true + + if (isAwake) { + linearVelocity += impulse * invMass + angularVelocity += rotInertiaInv * b2Cross(point - sweep.c, impulse) + } + } + + override fun applyLinearImpulseToCenter(impulse: Vector2d, wake: Boolean) { + if (type != BodyType.DYNAMIC) + return + + if (wake && !isAwake) + isAwake = true + + if (isAwake) { + linearVelocity += impulse * invMass + } + } + + override fun applyAngularImpulse(impulse: Double, wake: Boolean) { + if (type != BodyType.DYNAMIC) + return + + if (wake && !isAwake) + isAwake = true + + if (isAwake) { + angularVelocity += impulse * rotInertiaInv + } + } + + internal fun synchronizeTransform() { + transform.rotation.set(sweep.a) + transform.position = sweep.c - transform.rotation.times(sweep.localCenter) + } + + internal fun advance(alpha: Double) { + sweep.advance(alpha) + sweep.c = sweep.c0 + sweep.a = sweep.a0 + synchronizeTransform() + } + + internal fun synchronizeFixtures() { + val broadPhase = world.contactManager.broadPhase + + if (isAwake) { + val transform1 = Transform() + transform1.rotation.set(sweep.a0) + transform1.position = sweep.c0 - transform1.rotation.times(sweep.localCenter) + + for (fixture in fixtureIterator) { + (fixture as Fixture).synchronize(broadPhase, transform1, transform) + } + } else { + for (fixture in fixtureIterator) { + (fixture as Fixture).synchronize(broadPhase, transform, transform) + } + } + } + + override fun dump() { + TODO("Not yet implemented") + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/ContactManager.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/ContactManager.kt new file mode 100644 index 00000000..9476f018 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/ContactManager.kt @@ -0,0 +1,193 @@ +package ru.dbotthepony.kbox2d.dynamics + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.collision.BroadPhase +import ru.dbotthepony.kbox2d.dynamics.contact.AbstractContact + +class ContactManager : IContactManager { + override val broadPhase: IBroadPhase = BroadPhase() + override var contactList: IContact? = null + override var contactCount: Int = 0 + private set + + override var contactFilter: IContactFilter? = null + override var contactListener: IContactListener? = null + + override fun destroy(contact: IContact) { + contact as AbstractContact + + val fixtureA = contact.fixtureA + val fixtureB = contact.fixtureB + val bodyA = fixtureA.body as Body + val bodyB = fixtureB.body as Body + + if (contact.isTouching) { + contactListener?.endContact(contact) + } + + // Remove from the world. + run { + val prev = contact.prev as AbstractContact? + val next = contact.next as AbstractContact? + + prev?.next = next + next?.prev = prev + + if (contactList == contact) { + contactList = next ?: prev + } + } + + // Remove from body 1 + run { + val prev = contact.nodeA.prev + val next = contact.nodeA.next + + prev?.next = next + next?.prev = prev + + if (contact.nodeA == bodyA.contactEdge) { + bodyA.contactEdge = next ?: prev + } + } + + // Remove from body 2 + run { + val prev = contact.nodeB.prev + val next = contact.nodeB.next + + prev?.next = next + next?.prev = prev + + if (contact.nodeB == bodyB.contactEdge) { + bodyB.contactEdge = next ?: prev + } + } + + contactCount-- + } + + /** + * This is the top level collision call for the time step. Here + * all the narrow phase collision is processed for the world + * contact list. + */ + override fun collide() { + // Update awake contacts. + + for (c in contactListIterator) { + c as AbstractContact + val fixtureA = c.fixtureA + val fixtureB = c.fixtureB + val indexA = c.childIndexA + val indexB = c.childIndexB + val bodyA = fixtureA.body as Body + val bodyB = fixtureB.body as Body + + // Is this contact flagged for filtering? + if (c.isFlaggedForFiltering) { + // Should these bodies collide? + if (!bodyB.shouldCollide(bodyA)) { + destroy(c) + continue + } + + // Check user filtering. + if (contactFilter?.shouldCollide(fixtureA, fixtureB) == false) { + destroy(c) + continue + } + + // Clear the filtering flag. + c.isFlaggedForFiltering = false + } + + val activeA = bodyA.isAwake && bodyA.type != ru.dbotthepony.kbox2d.api.BodyType.STATIC + val activeB = bodyB.isAwake && bodyB.type != ru.dbotthepony.kbox2d.api.BodyType.STATIC + + // At least one body must be awake and it must be dynamic or kinematic. + if (!activeA && !activeB) { + continue + } + + val proxyIdA = fixtureA.proxies[indexA].proxyId + val proxyIdB = fixtureB.proxies[indexB].proxyId + val overlap = broadPhase.testOverlap(proxyIdA, proxyIdB) + + if (!overlap) { + destroy(c) + continue + } + + c.update(contactListener) + } + } + + override fun findNewContacts() { + broadPhase.updatePairs(this::addPair) + } + + override fun addPair(proxyUserDataA: Any?, proxyUserDataB: Any?) { + val proxyA = proxyUserDataA as FixtureProxy + val proxyB = proxyUserDataB as FixtureProxy + + val fixtureA = proxyA.fixture + val fixtureB = proxyB.fixture + + val indexA = proxyA.childIndex + val indexB = proxyB.childIndex + + val bodyA = fixtureA.body as Body + val bodyB = fixtureB.body as Body + + // Are the fixtures on the same body? + if (bodyA === bodyB) { + return + } + + // TODO_ERIN use a hash table to remove a potential bottleneck when both + // bodies have a lot of contacts. + // Does a contact already exist? + for (edge in bodyB.contactEdgeIterator) { + if (edge.other === bodyA) { + val fA = edge.contact.fixtureA + val fB = edge.contact.fixtureB + val iA = edge.contact.childIndexA + val iB = edge.contact.childIndexB + + if (fA === fixtureA && fB === fixtureB && iA == indexA && iB == indexB) { + // A contact already exists. + return + } + + if (fA === fixtureB && fB === fixtureA && iA == indexB && iB == indexA) { + // A contact already exists. + return + } + } + } + + // Does a joint override collision? Is at least one body dynamic? + if (!bodyB.shouldCollide(bodyA)) { + return + } + + // Check user filtering. + if (contactFilter?.shouldCollide(fixtureA, fixtureB) == false) { + return + } + + // Call the factory. + val c = AbstractContact.create(fixtureA, indexA, fixtureB, indexB) + + // Contact creation may swap fixtures. + // Insert into the world. + c.next = contactList + (contactList as AbstractContact?)?.prev = c + contactList = c + + // Connect to island graph. + // (connection is done in AbstractContact initializer) + contactCount++ + } +} \ No newline at end of file diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/Fixture.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/Fixture.kt new file mode 100644 index 00000000..44896678 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/Fixture.kt @@ -0,0 +1,147 @@ +package ru.dbotthepony.kbox2d.dynamics + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.collision.shapes.ChainShape +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape +import java.util.* +import kotlin.collections.ArrayList + +open class Fixture( + body: ru.dbotthepony.kbox2d.api.IBody, + def: FixtureDef +) : IFixture { + final override var body: ru.dbotthepony.kbox2d.api.IBody? = body + protected set + + final override var next: IFixture? = null + + override val userData: Any? = def.userData + override var friction: Double = def.friction + override var restitution: Double = def.restitution + override var restitutionThreshold: Double = def.restitutionThreshold + + override val type: IShape.Type get() = shape.type + + override var isSensor: Boolean = def.isSensor + set(value) { + if (field != value) { + checkNotNull(body) { "Already destroyed" }.isAwake = true + field = value + } + } + + final override val shape: IShape<*> = requireNotNull(def.shape) { "null shape provided" }.copy() + + init { + if (shape is PolygonShape) { + shape.validate(true) + } + + if (shape.childCount < 0) { + throw IllegalArgumentException("Shape $shape has ${shape.childCount} children") + } + } + + protected val internalProxies = ArrayList(shape.childCount) + final override val proxies: List = Collections.unmodifiableList(internalProxies) + + override var density: Double = def.density + set(value) { + require(value.isFinite()) { "Infinite density" } + require(!value.isNaN()) { "NaN density" } + require(value >= 0.0) { "Negative density of $value" } + field = value + } + + override var filter: IFilter = def.filter.immutable() + set(value) { + if (value is ImmutableFilter) + field = value + else + field = (value as Filter).immutable() + + refilter() + } + + fun destroy() { + checkNotNull(body) { "Already destroyed" }.destroyFixture(this) + } + + internal fun unlink() { + check(body != null) { "Already destroyed" } + check(internalProxies.isEmpty()) { "Still having proxies" } + body = null + } + + /** + * These support body activation/deactivation. + */ + internal fun createProxies(broadPhase: IBroadPhase, xf: Transform) { + check(body != null) { "Already destroyed" } + check(internalProxies.isEmpty()) { "Already having proxies" } + + for (i in 0 until shape.childCount) { + val aabb = shape.computeAABB(xf, i) + + val proxy = FixtureProxy( + fixture = this, + childIndex = i, + aabb = aabb, + ) + + proxy.proxyId = broadPhase.createProxy(aabb, proxy) + internalProxies.add(proxy) + } + } + + /** + * These support body activation/deactivation. + */ + internal fun destroyProxies(broadPhase: IBroadPhase) { + check(body != null) { "Already destroyed" } + + // Destroy proxies in the broad-phase. + for (proxy in internalProxies) { + broadPhase.destroyProxy(proxy.proxyId) + } + + internalProxies.clear() + } + + internal fun synchronize(broadPhase: IBroadPhase, transform1: Transform, transform2: Transform) { + check(body != null) { "Already destroyed" } + + for (proxy in internalProxies) { + val aabb1 = shape.computeAABB(transform1, proxy.childIndex) + val aabb2 = shape.computeAABB(transform2, proxy.childIndex) + + proxy.aabb = aabb1.combine(aabb2) + val displacement = aabb2.centre - aabb1.centre + broadPhase.moveProxy(proxy.proxyId, proxy.aabb, displacement) + } + } + + override fun refilter() { + val body = body + check(body != null) { "Already destroyed" } + var edge = body.contactEdge + + while (edge != null) { + val contact = edge.contact + val fixtureA = contact.fixtureA + val fixtureB = contact.fixtureB + + if (fixtureA == this || fixtureB == this) + contact.flagForFiltering() + + edge = edge.next + } + + val world = body.world + val broadPhase = world.contactManager.broadPhase + + for (proxy in internalProxies) { + broadPhase.touchProxy(proxy.proxyId) + } + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/AbstractContact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/AbstractContact.kt new file mode 100644 index 00000000..5d776daa --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/AbstractContact.kt @@ -0,0 +1,215 @@ +package ru.dbotthepony.kbox2d.dynamics.contact + +import it.unimi.dsi.fastutil.objects.Object2ObjectArrayMap +import it.unimi.dsi.fastutil.objects.Object2ObjectFunction +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.collision.WorldManifold +import ru.dbotthepony.kbox2d.collision.b2TestOverlap +import ru.dbotthepony.kbox2d.dynamics.Body + +fun interface ContactFactory { + fun factorize(fixtureA: IFixture, childIndexA: Int, fixtureB: IFixture, childIndexB: Int): AbstractContact +} + +sealed class AbstractContact( + final override val fixtureA: IFixture, + final override val childIndexA: Int, + final override val fixtureB: IFixture, + final override val childIndexB: Int, +) : IContact { + internal var flags: Int = ContactFlags.ENABLED.bitmask + internal var isOnIsland = false + internal var toiFlag = false + + final override var manifold: Manifold = Manifold() + private set + + override var next: IContact? = null + internal set + + override var prev: IContact? = null + internal set + + internal var isFlaggedForFiltering: Boolean + get() = ContactFlags.FILTER.isit(flags) + set(value) { flags = ContactFlags.FILTER.update(flags, value) } + + internal val nodeA: ContactEdge = ContactEdge(contact = this, other = fixtureB.body!!) + internal val nodeB: ContactEdge = ContactEdge(contact = this, other = fixtureA.body!!) + + init { + val bodyA = fixtureA.body as Body + val bodyB = fixtureB.body as Body + + nodeA.next = bodyA.contactEdge + nodeB.next = bodyB.contactEdge + + bodyA.contactEdge?.prev = nodeA + bodyB.contactEdge?.prev = nodeB + + bodyA.contactEdge = nodeA + bodyB.contactEdge = nodeB + } + + override var friction: Double = b2MixFriction(fixtureA.friction, fixtureB.friction) + override var restitution: Double = b2MixRestitution(fixtureA.restitution, fixtureB.restitution) + override var restitutionThreshold: Double = b2MixRestitutionThreshold(fixtureA.restitutionThreshold, fixtureB.restitutionThreshold) + override var tangentSpeed: Double = 0.0 + + internal var toiCount: Int = 0 + internal var toi: Double = 0.0 + + override val worldManifold: IWorldManifold get() { + val bodyA = checkNotNull(fixtureA.body) { "FixtureA has no body attached" } + val bodyB = checkNotNull(fixtureB.body) { "FixtureB has no body attached" } + + val shapeA = fixtureA.shape + val shapeB = fixtureB.shape + + return WorldManifold(manifold, bodyA.transform, shapeA.radius, bodyB.transform, shapeB.radius) + } + + override var isTouching: Boolean + get() = (flags and ContactFlags.TOUCHING.bitmask) == ContactFlags.TOUCHING.bitmask + internal set(value) { flags = ContactFlags.TOUCHING.update(flags, value) } + + override var isEnabled: Boolean + get() = (flags and ContactFlags.ENABLED.bitmask) == ContactFlags.ENABLED.bitmask + set(value) { flags = ContactFlags.ENABLED.update(flags, value) } + + override fun flagForFiltering() { + flags = flags or ContactFlags.FILTER.bitmask + } + + /** + * Awakens fixtures this contact belongs to if neither of them are sensors + */ + fun destroy() { + if (manifold.points.isNotEmpty() && !fixtureA.isSensor && !fixtureB.isSensor) { + fixtureA.body!!.isAwake = true + fixtureB.body!!.isAwake = true + } + } + + /** + * Update the contact manifold and touching status. + * Note: do not assume the fixture AABBs are overlapping or are valid. + */ + internal fun update(contactListener: IContactListener?) { + // Re-enable this contact. + flags = ContactFlags.ENABLED.or(flags) + + val oldManifold = manifold + + val touching: Boolean + val wasTouching = isTouching + + val sensor = fixtureA.isSensor || fixtureB.isSensor + + val bodyA = fixtureA.body as Body + val bodyB = fixtureB.body as Body + + val xfA = bodyA.transform + val xfB = bodyB.transform + + // Is this contact a sensor? + if (sensor) { + val shapeA = fixtureA.shape + val shapeB = fixtureB.shape + + touching = b2TestOverlap(shapeA, childIndexA, shapeB, childIndexB, xfA, xfB) + // Sensors don't generate manifolds. + manifold = Manifold.EMPTY + } else { + manifold = evaluate(xfA, xfB) + touching = manifold.points.isNotEmpty() + + // Match old contact ids to new contact ids and copy the + // stored impulses to warm start the solver. + for (mp2 in manifold.points) { + mp2.normalImpulse = 0.0 + mp2.tangentImpulse = 0.0 + val id2 = mp2.id + + for (mp1 in oldManifold.points) { + if (mp1.id.key == id2.key) { + mp2.normalImpulse = mp1.normalImpulse + mp2.tangentImpulse = mp1.tangentImpulse + break + } + } + } + + if (touching != wasTouching) { + bodyA.isAwake = true + bodyB.isAwake = true + } + } + + this.isTouching = touching + + if (!wasTouching && touching) { + contactListener?.beginContact(this) + } else if (wasTouching && !touching) { + contactListener?.endContact(this) + } + + if (!sensor && touching) { + contactListener?.preSolve(this, oldManifold) + } + } + + companion object { + private val registry = + Object2ObjectArrayMap>() + + internal fun register(type1: IShape.Type, type2: IShape.Type, factory: ContactFactory) { + registry.computeIfAbsent(type1, Object2ObjectFunction { Object2ObjectArrayMap() }) + .put(type2, factory) + } + + internal fun create( + fixtureA: IFixture, + indexA: Int, + fixtureB: IFixture, + indexB: Int, + ): AbstractContact { + val type1 = fixtureA.type + val type2 = fixtureB.type + + return registry[type1]?.get(type2)?.factorize(fixtureA, indexA, fixtureB, indexB) + ?: registry[type2]?.get(type1)?.factorize(fixtureB, indexB, fixtureA, indexA) + ?: throw IllegalArgumentException("No collision handler for between $type1 and $type2") + } + + init { + register(IShape.Type.POLYGON, IShape.Type.POLYGON) { fixtureA: IFixture, _: Int, fixtureB: IFixture, _: Int -> + return@register PolygonContact(fixtureA, fixtureB) + } + + register(IShape.Type.POLYGON, IShape.Type.CIRCLE) { fixtureA: IFixture, _: Int, fixtureB: IFixture, _: Int -> + return@register PolygonCircleContact(fixtureA, fixtureB) + } + + register(IShape.Type.CIRCLE, IShape.Type.CIRCLE) { fixtureA: IFixture, _: Int, fixtureB: IFixture, _: Int -> + return@register CircleContact(fixtureA, fixtureB) + } + + register(IShape.Type.EDGE, IShape.Type.CIRCLE) { fixtureA: IFixture, _: Int, fixtureB: IFixture, _: Int -> + return@register EdgeCircleContact(fixtureA, fixtureB) + } + + register(IShape.Type.EDGE, IShape.Type.POLYGON) { fixtureA: IFixture, _: Int, fixtureB: IFixture, _: Int -> + return@register EdgePolygonContact(fixtureA, fixtureB) + } + + register(IShape.Type.CHAIN, IShape.Type.POLYGON) { fixtureA: IFixture, indexA: Int, fixtureB: IFixture, indexB: Int -> + return@register ChainPolygonContact(fixtureA, indexA, fixtureB, indexB) + } + + register(IShape.Type.CHAIN, IShape.Type.CIRCLE) { fixtureA: IFixture, indexA: Int, fixtureB: IFixture, indexB: Int -> + return@register ChainCircleContact(fixtureA, indexA, fixtureB, indexB) + } + } + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainCircleContact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainCircleContact.kt new file mode 100644 index 00000000..c20c20c8 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainCircleContact.kt @@ -0,0 +1,28 @@ +package ru.dbotthepony.kbox2d.dynamics.contact + +import ru.dbotthepony.kbox2d.api.IFixture +import ru.dbotthepony.kbox2d.api.IShape +import ru.dbotthepony.kbox2d.api.Manifold +import ru.dbotthepony.kbox2d.api.Transform +import ru.dbotthepony.kbox2d.collision.handler.b2CollideEdgeAndCircle +import ru.dbotthepony.kbox2d.collision.shapes.ChainShape +import ru.dbotthepony.kbox2d.collision.shapes.CircleShape + +class ChainCircleContact( + fixtureA: IFixture, + childIndexA: Int, + fixtureB: IFixture, + childIndexB: Int, +) : AbstractContact(fixtureA, childIndexA, fixtureB, childIndexB) { + init { + require(fixtureA.type == IShape.Type.CHAIN) { "Fixture A is of type ${fixtureA.type}" } + require(fixtureB.type == IShape.Type.CIRCLE) { "Fixture B is of type ${fixtureB.type}" } + } + + override fun evaluate(xfA: Transform, xfB: Transform): Manifold { + val chain = fixtureA.shape as ChainShape + val edge = chain.getChildEdge(childIndexA) + + return b2CollideEdgeAndCircle(edge, xfA, fixtureB.shape as CircleShape, xfB) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainPolygonContact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainPolygonContact.kt new file mode 100644 index 00000000..0c27a4a4 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/ChainPolygonContact.kt @@ -0,0 +1,28 @@ +package ru.dbotthepony.kbox2d.dynamics.contact + +import ru.dbotthepony.kbox2d.api.IFixture +import ru.dbotthepony.kbox2d.api.IShape +import ru.dbotthepony.kbox2d.api.Manifold +import ru.dbotthepony.kbox2d.api.Transform +import ru.dbotthepony.kbox2d.collision.handler.b2CollideEdgeAndPolygon +import ru.dbotthepony.kbox2d.collision.shapes.ChainShape +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape + +class ChainPolygonContact( + fixtureA: IFixture, + childIndexA: Int, + fixtureB: IFixture, + childIndexB: Int, +) : AbstractContact(fixtureA, childIndexA, fixtureB, childIndexB) { + init { + require(fixtureA.type == IShape.Type.CHAIN) { "Fixture A is of type ${fixtureA.type}" } + require(fixtureB.type == IShape.Type.POLYGON) { "Fixture B is of type ${fixtureB.type}" } + } + + override fun evaluate(xfA: Transform, xfB: Transform): Manifold { + val chain = fixtureA.shape as ChainShape + val edge = chain.getChildEdge(childIndexA) + + return b2CollideEdgeAndPolygon(edge, xfA, fixtureB.shape as PolygonShape, xfB) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/CircleContact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/CircleContact.kt new file mode 100644 index 00000000..1e7b5fb9 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/CircleContact.kt @@ -0,0 +1,22 @@ +package ru.dbotthepony.kbox2d.dynamics.contact + +import ru.dbotthepony.kbox2d.api.IFixture +import ru.dbotthepony.kbox2d.api.IShape +import ru.dbotthepony.kbox2d.api.Manifold +import ru.dbotthepony.kbox2d.api.Transform +import ru.dbotthepony.kbox2d.collision.handler.b2CollideCircles +import ru.dbotthepony.kbox2d.collision.shapes.CircleShape + +class CircleContact( + fixtureA: IFixture, + fixtureB: IFixture, +) : AbstractContact(fixtureA, 0, fixtureB, 0) { + init { + require(fixtureA.type == IShape.Type.CIRCLE) { "Fixture A is of type ${fixtureA.type}" } + require(fixtureB.type == IShape.Type.CIRCLE) { "Fixture B is of type ${fixtureB.type}" } + } + + override fun evaluate(xfA: Transform, xfB: Transform): Manifold { + return b2CollideCircles(fixtureA.shape as CircleShape, xfA, fixtureB.shape as CircleShape, xfB) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgeCircleContact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgeCircleContact.kt new file mode 100644 index 00000000..a64164d2 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgeCircleContact.kt @@ -0,0 +1,23 @@ +package ru.dbotthepony.kbox2d.dynamics.contact + +import ru.dbotthepony.kbox2d.api.IFixture +import ru.dbotthepony.kbox2d.api.IShape +import ru.dbotthepony.kbox2d.api.Manifold +import ru.dbotthepony.kbox2d.api.Transform +import ru.dbotthepony.kbox2d.collision.handler.b2CollideEdgeAndCircle +import ru.dbotthepony.kbox2d.collision.shapes.CircleShape +import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape + +class EdgeCircleContact( + fixtureA: IFixture, + fixtureB: IFixture, +) : AbstractContact(fixtureA, 0, fixtureB, 0) { + init { + require(fixtureA.type == IShape.Type.EDGE) { "Fixture A is of type ${fixtureA.type}, expected EDGE" } + require(fixtureB.type == IShape.Type.CIRCLE) { "Fixture B is of type ${fixtureB.type}, expected CIRCLE" } + } + + override fun evaluate(xfA: Transform, xfB: Transform): Manifold { + return b2CollideEdgeAndCircle(fixtureA.shape as EdgeShape, xfA, fixtureB.shape as CircleShape, xfB) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgePolygonContact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgePolygonContact.kt new file mode 100644 index 00000000..1dc03ecd --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/EdgePolygonContact.kt @@ -0,0 +1,23 @@ +package ru.dbotthepony.kbox2d.dynamics.contact + +import ru.dbotthepony.kbox2d.api.IFixture +import ru.dbotthepony.kbox2d.api.IShape +import ru.dbotthepony.kbox2d.api.Manifold +import ru.dbotthepony.kbox2d.api.Transform +import ru.dbotthepony.kbox2d.collision.handler.b2CollideEdgeAndPolygon +import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape + +class EdgePolygonContact( + fixtureA: IFixture, + fixtureB: IFixture, +) : AbstractContact(fixtureA, 0, fixtureB, 0) { + init { + require(fixtureA.type == IShape.Type.EDGE) { "Fixture A is of type ${fixtureA.type}, expected EDGE" } + require(fixtureB.type == IShape.Type.POLYGON) { "Fixture B is of type ${fixtureB.type}, expected POLYGON" } + } + + override fun evaluate(xfA: Transform, xfB: Transform): Manifold { + return b2CollideEdgeAndPolygon(fixtureA.shape as EdgeShape, xfA, fixtureB.shape as PolygonShape, xfB) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonCircleContact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonCircleContact.kt new file mode 100644 index 00000000..41598f90 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonCircleContact.kt @@ -0,0 +1,23 @@ +package ru.dbotthepony.kbox2d.dynamics.contact + +import ru.dbotthepony.kbox2d.api.IFixture +import ru.dbotthepony.kbox2d.api.IShape +import ru.dbotthepony.kbox2d.api.Manifold +import ru.dbotthepony.kbox2d.api.Transform +import ru.dbotthepony.kbox2d.collision.handler.b2CollidePolygonAndCircle +import ru.dbotthepony.kbox2d.collision.shapes.CircleShape +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape + +class PolygonCircleContact( + fixtureA: IFixture, + fixtureB: IFixture, +) : AbstractContact(fixtureA, 0, fixtureB, 0) { + init { + require(fixtureA.type == IShape.Type.POLYGON) { "Fixture A is of type ${fixtureA.type}, expected POLYGON" } + require(fixtureB.type == IShape.Type.CIRCLE) { "Fixture B is of type ${fixtureB.type}, expected CIRCLE" } + } + + override fun evaluate(xfA: Transform, xfB: Transform): Manifold { + return b2CollidePolygonAndCircle(fixtureA.shape as PolygonShape, xfA, fixtureB.shape as CircleShape, xfB) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonContact.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonContact.kt new file mode 100644 index 00000000..4636b0b0 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/contact/PolygonContact.kt @@ -0,0 +1,26 @@ +package ru.dbotthepony.kbox2d.dynamics.contact + +import ru.dbotthepony.kbox2d.api.IFixture +import ru.dbotthepony.kbox2d.api.IShape +import ru.dbotthepony.kbox2d.api.Manifold +import ru.dbotthepony.kbox2d.api.Transform +import ru.dbotthepony.kbox2d.collision.handler.b2CollidePolygons +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape + +class PolygonContact( + fixtureA: IFixture, + fixtureB: IFixture, +) : AbstractContact(fixtureA, 0, fixtureB, 0) { + init { + require(fixtureA.type == IShape.Type.POLYGON) { "Fixture A has type of ${fixtureA.type}" } + require(fixtureB.type == IShape.Type.POLYGON) { "Fixture B has type of ${fixtureB.type}" } + } + + override fun evaluate(xfA: Transform, xfB: Transform): Manifold { + return b2CollidePolygons( + fixtureA.shape as PolygonShape, + xfA, + fixtureB.shape as PolygonShape, + xfB) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/ContactSolver.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/ContactSolver.kt new file mode 100644 index 00000000..beb242bb --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/ContactSolver.kt @@ -0,0 +1,820 @@ +package ru.dbotthepony.kbox2d.dynamics.internal + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.collision.WorldManifold +import ru.dbotthepony.kbox2d.dynamics.contact.AbstractContact +import ru.dbotthepony.kbox2d.dynamics.Body +import ru.dbotthepony.kstarbound.math.MutableMatrix2d +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times +import java.lang.IllegalArgumentException +import kotlin.math.absoluteValue + +private const val g_blockSolve = true + +// Ensure a reasonable condition number. +private const val k_maxConditionNumber = 1000.0 + +private const val k_errorTol = 1E-3 + +private const val B2_DEBUG_SOLVER = false + +internal data class VelocityCostantPoint( + var rA: Vector2d = Vector2d.ZERO, + var rB: Vector2d = Vector2d.ZERO, + var normalImpulse: Double = 0.0, + var tangentImpulse: Double = 0.0, + var normalMass: Double = 0.0, + var tangentMass: Double = 0.0, + var velocityBias: Double = 0.0, +) + +internal data class ContactVelocityConstraint( + var points: Array, + 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, + val positions: List, + val velocities: List, +) + +internal data class ContactPositionConstraint( + val localPoints: Array, + 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, + val velocities: List, + val contacts: List, +) { + constructor(def: ContactSolverDef) : this( + def.step, + def.positions, + def.velocities, + def.contacts, + ) + + val positionConstraints = ArrayList() + val velocityConstraints = ArrayList() + + init { + // Initialize position independent portions of the constraints. + for ((i, contact) in contacts.withIndex()) { + val fixtureA = contact.fixtureA + val fixtureB = contact.fixtureB + val shapeA = fixtureA.shape + val shapeB = fixtureB.shape + val radiusA = shapeA.radius + val radiusB = shapeB.radius + val bodyA = fixtureA.body as Body + val bodyB = fixtureB.body as Body + val manifold = contact.manifold + + check(manifold.points.isNotEmpty()) { "Manifold points at $i are empty" } + + val vc = ContactVelocityConstraint( + friction = contact.friction, + restitution = contact.restitution, + threshold = contact.restitutionThreshold, + tangentSpeed = contact.tangentSpeed, + indexA = bodyA.islandIndex, + indexB = bodyB.islandIndex, + invMassA = bodyA.invMass, + invMassB = bodyB.invMass, + invIA = bodyA.rotInertiaInv, + invIB = bodyB.rotInertiaInv, + contactIndex = i, + K = MutableMatrix2d(m00 = 0.0, m11 = 0.0), + normalMass = MutableMatrix2d(m00 = 0.0, m11 = 0.0), + normal = Vector2d.ZERO, + points = Array(manifold.points.size) { VelocityCostantPoint() } + ) + + velocityConstraints.add(vc) + + val pc = ContactPositionConstraint( + indexA = bodyA.islandIndex, + indexB = bodyB.islandIndex, + invMassA = bodyA.invMass, + invMassB = bodyB.invMass, + localCenterA = bodyA.sweep.localCenter, + localCenterB = bodyB.sweep.localCenter, + invIA = bodyA.rotInertiaInv, + invIB = bodyB.rotInertiaInv, + localNormal = manifold.localNormal, + localPoint = manifold.localPoint, + localPoints = Array(manifold.points.size) { Vector2d.ZERO }, + radiusA = radiusA, + radiusB = radiusB, + type = manifold.type, + ) + + positionConstraints.add(pc) + + for (j in manifold.points.indices) { + val cp = manifold.points[j] + val vcp = vc.points[j] + + if (step.warmStarting) { + vcp.normalImpulse = cp.normalImpulse * step.dtRatio + vcp.tangentImpulse = cp.tangentImpulse * step.dtRatio + } + + pc.localPoints[j] = cp.localPoint + } + } + } + + /** + * Initialize position dependent portions of the velocity constraints. + */ + fun initializeVelocityConstraints() { + for (i in contacts.indices) { + val vc = velocityConstraints[i] + val pc = positionConstraints[i] + + val radiusA = pc.radiusA + val radiusB = pc.radiusB + val manifold = contacts[vc.contactIndex].manifold + + val indexA = vc.indexA + val indexB = vc.indexB + + val mA = vc.invMassA + val mB = vc.invMassB + val iA = vc.invIA + val iB = vc.invIB + + val localCenterA = pc.localCenterA + val localCenterB = pc.localCenterB + + val cA = positions[indexA].c + val aA = positions[indexA].a + val vA = velocities[indexA].v + val wA = velocities[indexA].w + + val cB = positions[indexB].c + val aB = positions[indexB].a + val vB = velocities[indexB].v + val wB = velocities[indexB].w + + check(manifold.points.isNotEmpty()) { "Manifold at $i is empty" } + + val xfA = Transform() + val xfB = Transform() + + xfA.rotation.set(aA) + xfB.rotation.set(aB) + + xfA.position = cA - b2Mul(xfA.q, localCenterA) + xfB.position = cB - b2Mul(xfB.q, localCenterB) + + val worldManifold = WorldManifold(manifold, xfA, radiusA, xfB, radiusB) + + vc.normal = worldManifold.normal + + for ((j, vcp) in vc.points.withIndex()) { + vcp.rA = worldManifold.points[j] - cA + vcp.rB = worldManifold.points[j] - cB + + val rnA = b2Cross(vcp.rA, vc.normal) + val rnB = b2Cross(vcp.rB, vc.normal) + + val kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB + + vcp.normalMass = if (kNormal > 0.0) 1.0 / kNormal else 0.0 + + val tangent = b2Cross(vc.normal, 1.0) + + val rtA = b2Cross(vcp.rA, tangent) + val rtB = b2Cross(vcp.rB, tangent) + + val kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB + + vcp.tangentMass = if (kTangent > 0.0) 1.0 / kTangent else 0.0 + + // Setup a velocity bias for restitution. + // vcp.velocityBias = 0.0 + val vRel = b2Dot(vc.normal, vB + b2Cross(wB, vcp.rB) - vA - b2Cross(wA, vcp.rA)) + + if (vRel < -vc.threshold) { + vcp.velocityBias = -vc.restitution * vRel + } + } + + // If we have two points, then prepare the block solver. + if (vc.points.size == 2 && g_blockSolve) { + val vcp1 = vc.points[0] + val vcp2 = vc.points[1] + + val rn1A = b2Cross(vcp1.rA, vc.normal) + val rn1B = b2Cross(vcp1.rB, vc.normal) + val rn2A = b2Cross(vcp2.rA, vc.normal) + val rn2B = b2Cross(vcp2.rB, vc.normal) + + val k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B + val k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B + val k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B + + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { + // K is safe to invert. + // k11 k12 + // k12 k22 + // + // vc->K.ex.Set(k11, k12); + // vc->K.ey.Set(k12, k22); + + vc.K.m00 = k11 + vc.K.m10 = k12 + vc.K.m01 = k12 + vc.K.m11 = k22 + vc.normalMass = vc.K.getInverse().asMutableMatrix() + } else { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + vc.points = Array(1) { vc.points[it] } + } + } + } + } + + /** + * Warm start. + */ + fun warmStart() { + for (vc in velocityConstraints) { + val indexA = vc.indexA + val indexB = vc.indexB + val mA = vc.invMassA + val iA = vc.invIA + val mB = vc.invMassB + val iB = vc.invIB + + var vA = velocities[indexA].v + var wA = velocities[indexA].w + var vB = velocities[indexB].v + var wB = velocities[indexB].w + + val normal = vc.normal + val tangent = b2Cross(normal, 1.0) + + for (vcp in vc.points) { + val P = normal * vcp.normalImpulse + tangent * vcp.tangentImpulse + wA -= iA * b2Cross(vcp.rA, P) + vA -= P * mA + wB += iB * b2Cross(vcp.rB, P) + vB += P * mB + } + + velocities[indexA].v = vA + velocities[indexA].w = wA + velocities[indexB].v = vB + velocities[indexB].w = wB + } + } + + fun solveVelocityConstraints() { + for (vc in velocityConstraints) { + val indexA = vc.indexA + val indexB = vc.indexB + val mA = vc.invMassA + val iA = vc.invIA + val mB = vc.invMassB + val iB = vc.invIB + + var vA = velocities[indexA].v + var wA = velocities[indexA].w + var vB = velocities[indexB].v + var wB = velocities[indexB].w + + val normal = vc.normal + val tangent = b2Cross(normal, 1.0) + val friction = vc.friction + + check(vc.points.size == 1 || vc.points.size == 2) { "Unexpected points amount: ${vc.points.size}" } + + // Solve tangent constraints first because non-penetration is more important + // than friction. + for (vcp in vc.points) { + // Relative velocity at contact + val dv = vB + vB + b2Cross(wB, vcp.rB) - vA - b2Cross(wA, vcp.rA) + + // Compute tangent force + val vt = b2Dot(dv, tangent) - vc.tangentSpeed + var lambda = vcp.tangentMass * (-vt) + + // b2Clamp the accumulated force + val maxFriction = friction * vcp.normalImpulse + val newImpulse = b2Clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction) + + lambda = newImpulse - vcp.tangentImpulse + vcp.tangentImpulse = newImpulse + + // Apply contact impulse + val P = lambda * tangent + + vA -= mA * P + wA -= iA * b2Cross(vcp.rA, P) + + vB += mB * P + wB += iB * b2Cross(vcp.rB, P) + } + + // Solve normal constraints + if (vc.points.size == 1 || !g_blockSolve) { + for (vcp in vc.points) { + // Relative velocity at contact + val dv = vB + b2Cross(wB, vcp.rB) - vA - b2Cross(wA, vcp.rA) + + // Compute normal impulse + val vn = b2Dot(dv, normal) + var lambda = -vcp.normalMass * (vn - vcp.velocityBias) + + // b2Clamp the accumulated impulse + val newImpulse = b2Max(vcp.normalImpulse + lambda, 0.0) + lambda = newImpulse - vcp.normalImpulse + vcp.normalImpulse = newImpulse + + // Apply contact impulse + val P = lambda * normal + vA -= mA * P + wA -= iA * b2Cross(vcp.rA, P) + + vB += mB * P + wB += iB * b2Cross(vcp.rB, P) + } + } else { + // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). + // Build the mini LCP for this contact patch + // + // vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 + // + // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) + // b = vn0 - velocityBias + // + // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i + // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases + // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid + // solution that satisfies the problem is chosen. + // + // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires + // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). + // + // Substitute: + // + // x = a + d + // + // a := old total impulse + // x := new total impulse + // d := incremental impulse + // + // For the current iteration we extend the formula for the incremental impulse + // to compute the new total impulse: + // + // vn = A * d + b + // = A * (x - a) + b + // = A * x + b - A * a + // = A * x + b' + // b' = b - A * a; + + val cp1 = vc.points[0] + val cp2 = vc.points[1] + + val a = Vector2d(cp1.normalImpulse, cp2.normalImpulse) + + check(a.x >= 0.0 && a.y >= 0.0) { a } + + // Relative velocity at contact + var dv1 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA) + var dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA) + + // Compute normal velocity + var vn1 = b2Dot(dv1, normal) + var vn2 = b2Dot(dv2, normal) + + var b = Vector2d( + x = vn1 - cp1.velocityBias, + y = vn2 - cp2.velocityBias, + ) + + // Compute b' + b -= b2Mul(vc.K, a) + + // for (;;) + run { + // + // Case 1: vn = 0 + // + // 0 = A * x + b' + // + // Solve for x: + // + // x = - inv(A) * b' + // + var x = -b2Mul(vc.normalMass, b) + + if (x.x >= 0.0 && x.y >= 0.0) { + // Get the incremental impulse + val d = x - a + + // Apply incremental impulse + val P1 = normal * d.x + val P2 = normal * d.y + + vA -= mA * (P1 + P2) + wA -= iA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2)) + + vB += mB * (P1 + P2) + wB += iB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2)) + + // Accumulate + cp1.normalImpulse = x.x + cp2.normalImpulse = x.y + + // Postconditions + if (B2_DEBUG_SOLVER) { + dv1 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA) + dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA) + + // Compute normal velocity + vn1 = dv1.dotProduct(normal) + vn2 = dv2.dotProduct(normal) + + check((vn1 - cp1.velocityBias).absoluteValue < k_errorTol) { (vn1 - cp1.velocityBias).absoluteValue } + check((vn2 - cp2.velocityBias).absoluteValue < k_errorTol) { (vn2 - cp2.velocityBias).absoluteValue } + } + + return@run + } + + // + // Case 2: vn1 = 0 and x2 = 0 + // + // 0 = a11 * x1 + a12 * 0 + b1' + // vn2 = a21 * x1 + a22 * 0 + b2' + // + x = Vector2d(x = -cp1.normalMass * b.x) + + vn1 = 0.0 + // vn2 = vc->K.ex.y * x.x + b.y; + vn2 = vc.K.m10 * x.x + b.y + + if (x.x >= 0.0 && vn2 >= 0.0) { + // Get the incremental impulse + val d = x - a + + // Apply incremental impulse + val P1 = d.x * normal + val P2 = d.y * normal + + vA -= mA * (P1 + P2) + wA -= iA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2)) + + vB += mB * (P1 + P2) + wB += iB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2)) + + // Accumulate + cp1.normalImpulse = x.x + cp2.normalImpulse = x.y + + if (B2_DEBUG_SOLVER) { + // Postconditions + dv1 = vB + b2Cross(wB, cp1.rB) - vA - b2Cross(wA, cp1.rA) + + // Compute normal velocity + vn1 = b2Dot(dv1, normal) + + check((vn1 - cp1.velocityBias).absoluteValue < k_errorTol) { (vn1 - cp1.velocityBias).absoluteValue } + } + + return@run + } + + // + // Case 3: vn2 = 0 and x1 = 0 + // + // vn1 = a11 * 0 + a12 * x2 + b1' + // 0 = a21 * 0 + a22 * x2 + b2' + // + x = Vector2d(y = -cp2.normalMass * b.y) + vn1 = vc.K.m01 * x.y + b.x + vn2 = 0.0 + + if (x.y >= 0.0 && vn1 >= 0.0) { + // Resubstitute for the incremental impulse + val d = x - a + + // Apply incremental impulse + val P1 = d.x * normal + val P2 = d.y * normal + + vA -= mA * (P1 + P2) + wA -= iA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2)) + + vB += mB * (P1 + P2) + wB += iB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2)) + + // Accumulate + cp1.normalImpulse = x.x + cp2.normalImpulse = x.y + + if (B2_DEBUG_SOLVER) { + // Postconditions + dv2 = vB + b2Cross(wB, cp2.rB) - vA - b2Cross(wA, cp2.rA) + + // Compute normal velocity + vn2 = b2Dot(dv2, normal) + + check((vn2 - cp2.velocityBias).absoluteValue < k_errorTol) { (vn2 - cp2.velocityBias).absoluteValue } + } + + return@run + } + + // + // Case 4: x1 = 0 and x2 = 0 + // + // vn1 = b1 + // vn2 = b2; + x = Vector2d.ZERO + vn1 = b.x + vn2 = b.y + + if (vn1 >= 0.0 && vn2 >= 0.0) { + // Resubstitute for the incremental impulse + val d = x - a + + // Apply incremental impulse + val P1 = d.x * normal + val P2 = d.y * normal + + vA -= mA * (P1 + P2) + wA -= iA * (b2Cross(cp1.rA, P1) + b2Cross(cp2.rA, P2)) + + vB += mB * (P1 + P2) + wB += iB * (b2Cross(cp1.rB, P1) + b2Cross(cp2.rB, P2)) + + // Accumulate + cp1.normalImpulse = x.x + cp2.normalImpulse = x.y + + return@run + } + + // No solution, give up. This is hit sometimes, but it doesn't seem to matter. + } + } + + velocities[indexA].v = vA + velocities[indexA].w = wA + velocities[indexB].v = vB + velocities[indexB].w = wB + } + } + + fun storeImpulses() { + for (vc in velocityConstraints) { + val manifold = contacts[vc.contactIndex].manifold + + for ((j, point) in vc.points.withIndex()) { + manifold.points[j].normalImpulse = point.normalImpulse + manifold.points[j].tangentImpulse = point.tangentImpulse + } + } + } + + /** + * Sequential solver. + */ + fun solvePositionConstraints(): Boolean { + var minSeparation = 0.0 + + for (pc in positionConstraints) { + val indexA = pc.indexA + val indexB = pc.indexB + val localCenterA = pc.localCenterA + val mA = pc.invMassA + val iA = pc.invIA + val localCenterB = pc.localCenterB + val mB = pc.invMassB + val iB = pc.invIB + + var cA = positions[indexA].c + var aA = positions[indexA].a + + var cB = positions[indexB].c + var aB = positions[indexB].a + + // Solve normal constraints + for (j in 0 until pc.localPoints.size) { + val xfA = Transform() + val xfB = Transform() + + xfA.q.set(aA) + xfB.q.set(aB) + + xfA.p = cA - b2Mul(xfA.q, localCenterA) + xfB.p = cB - b2Mul(xfB.q, localCenterB) + + val (normal, point, separation) = PositionSolverManifold(pc, xfA, xfB, j) + + val rA = point - cA + val rB = point - cB + + // Track max constraint error. + minSeparation = b2Min(minSeparation, separation) + + // Prevent large corrections and allow slop. + val C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0) + + // Compute the effective mass. + val rnA = b2Cross(rA, normal) + val rnB = b2Cross(rB, normal) + val K = mA + mB + iA * rnA * rnA + iB * rnB * rnB + + // Compute normal impulse + val impulse = if (K > 0.0) -C / K else 0.0 + + val P = impulse * normal + + cA -= mA * P + aA -= iA * b2Cross(rA, P) + + cB += mB * P + aB += iB * b2Cross(rB, P) + } + + positions[indexA].c = cA + positions[indexA].a = aA + + positions[indexB].c = cB + positions[indexB].a = aB + } + + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -3.0f * b2_linearSlop + } + + /** + * Sequential position solver for position constraints. + */ + fun solveTOIPositionConstraints(toiIndexA: Int, toiIndexB: Int): Boolean { + var minSeparation = 0.0 + + for (pc in positionConstraints) { + val indexA = pc.indexA + val indexB = pc.indexB + val localCenterA = pc.localCenterA + val localCenterB = pc.localCenterB + + var mA = 0.0 + var iA = 0.0 + + if (indexA == toiIndexA || indexA == toiIndexB) { + mA = pc.invMassA + iA = pc.invIA + } + + var mB = 0.0 + var iB = 0.0 + + if (indexB == toiIndexA || indexB == toiIndexB) { + mB = pc.invMassB + iB = pc.invIB + } + + var cA = positions[indexA].c + var aA = positions[indexA].a + + var cB = positions[indexB].c + var aB = positions[indexB].a + + // Solve normal constraints + for (j in 0 until pc.localPoints.size) { + val xfA = Transform() + val xfB = Transform() + + xfA.q.set(aA) + xfB.q.set(aB) + + xfA.p = cA - b2Mul(xfA.q, localCenterA) + xfB.p = cB - b2Mul(xfB.q, localCenterB) + + val (normal, point, separation) = PositionSolverManifold(pc, xfA, xfB, j) + + val rA = point - cA + val rB = point - cB + + // Track max constraint error. + minSeparation = b2Min(minSeparation, separation) + + // Prevent large corrections and allow slop. + val C = b2Clamp(b2_toiBaumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0) + + // Compute the effective mass. + val rnA = b2Cross(rA, normal) + val rnB = b2Cross(rB, normal) + val K = mA + mB + iA * rnA * rnA + iB * rnB * rnB + + // Compute normal impulse + val impulse = if (K > 0.0) -C / K else 0.0 + + val P = impulse * normal + + cA -= mA * P + aA -= iA * b2Cross(rA, P) + + cB += mB * P + aB += iB * b2Cross(rB, P) + } + + positions[indexA].c = cA + positions[indexA].a = aA + + positions[indexB].c = cB + positions[indexB].a = aB + } + + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -1.5f * b2_linearSlop + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/Island.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/Island.kt new file mode 100644 index 00000000..4c0c6369 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/internal/Island.kt @@ -0,0 +1,480 @@ +package ru.dbotthepony.kbox2d.dynamics.internal + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.collision.DistanceProxy +import ru.dbotthepony.kbox2d.collision.b2Distance +import ru.dbotthepony.kbox2d.dynamics.contact.AbstractContact +import ru.dbotthepony.kbox2d.dynamics.joint.AbstractJoint +import ru.dbotthepony.kbox2d.dynamics.Body +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times +import java.util.* +import kotlin.collections.ArrayList +import kotlin.math.absoluteValue + +private const val linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance +private const val angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance + +private const val checkPositions = false + +/* +Position Correction Notes +========================= +I tried the several algorithms for position correction of the 2D revolute joint. +I looked at these systems: +- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s. +- suspension bridge with 30 1m long planks of length 1m. +- multi-link chain with 30 1m long links. + +Here are the algorithms: + +Baumgarte - A fraction of the position error is added to the velocity error. There is no +separate position solver. + +Pseudo Velocities - After the velocity solver and position integration, +the position error, Jacobian, and effective mass are recomputed. Then +the velocity constraints are solved with pseudo velocities and a fraction +of the position error is added to the pseudo velocity error. The pseudo +velocities are initialized to zero and there is no warm-starting. After +the position solver, the pseudo velocities are added to the positions. +This is also called the First Order World method or the Position LCP method. + +Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the +position error is re-computed for each constraint and the positions are updated +after the constraint is solved. The radius vectors (aka Jacobians) are +re-computed too (otherwise the algorithm has horrible instability). The pseudo +velocity states are not needed because they are effectively zero at the beginning +of each iteration. Since we have the current position error, we allow the +iterations to terminate early if the error becomes smaller than b2_linearSlop. + +Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed +each time a constraint is solved. + +Here are the results: +Baumgarte - this is the cheapest algorithm but it has some stability problems, +especially with the bridge. The chain links separate easily close to the root +and they jitter as they struggle to pull together. This is one of the most common +methods in the field. The big drawback is that the position correction artificially +affects the momentum, thus leading to instabilities and false bounce. I used a +bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller +factor makes joints and contacts more spongy. + +Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is +stable. However, joints still separate with large angular velocities. Drag the +simple pendulum in a circle quickly and the joint will separate. The chain separates +easily and does not recover. I used a bias factor of 0.2. A larger value lead to +the bridge collapsing when a heavy cube drops on it. + +Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo +Velocities, but in other ways it is worse. The bridge and chain are much more +stable, but the simple pendulum goes unstable at high angular velocities. + +Full NGS - stable in all tests. The joints display good stiffness. The bridge +still sags, but this is better than infinite forces. + +Recommendations +Pseudo Velocities are not really worthwhile because the bridge and chain cannot +recover from joint separation. In other cases the benefit over Baumgarte is small. + +Modified NGS is not a robust method for the revolute joint due to the violent +instability seen in the simple pendulum. Perhaps it is viable with other constraint +types, especially scalar constraints where the effective mass is a scalar. + +This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities +and is very fast. I don't think we can escape Baumgarte, especially in highly +demanding cases where high constraint fidelity is not needed. + +Full NGS is robust and easy on the eyes. I recommend this as an option for +higher fidelity simulation and certainly for suspension bridges and long chains. +Full NGS might be a good choice for ragdolls, especially motorized ragdolls where +joint separation can be problematic. The number of NGS iterations can be reduced +for better performance without harming robustness much. + +Each joint in a can be handled differently in the position solver. So I recommend +a system where the user can select the algorithm on a per joint basis. I would +probably default to the slower Full NGS and let the user select the faster +Baumgarte method in performance critical scenarios. +*/ + +/* +Cache Performance + +The Box2D solvers are dominated by cache misses. Data structures are designed +to increase the number of cache hits. Much of misses are due to random access +to body data. The constraint structures are iterated over linearly, which leads +to few cache misses. + +The bodies are not accessed during iteration. Instead read only data, such as +the mass values are stored with the constraints. The mutable data are the constraint +impulses and the bodies velocities/positions. The impulses are held inside the +constraint structures. The body velocities/positions are held in compact, temporary +arrays to increase the number of cache hits. Linear and angular velocity are +stored in a single array since multiple arrays lead to multiple misses. +*/ + +/* +2D Rotation + +R = [cos(theta) -sin(theta)] + [sin(theta) cos(theta) ] + +thetaDot = omega + +Let q1 = cos(theta), q2 = sin(theta). +R = [q1 -q2] + [q2 q1] + +q1Dot = -thetaDot * q2 +q2Dot = thetaDot * q1 + +q1_new = q1_old - dt * w * q2 +q2_new = q2_old + dt * w * q1 +then normalize. + +This might be faster than computing sin+cos. +However, we can compute sin+cos of the same angle fast. +*/ + +/** + * This is an internal class. + */ +internal class Island( + initialBodyCapacity: Int = 0, + initialContactCapacity: Int = 0, + initialJointCapacity: Int = 0, + val listener: IContactListener? = null +) { + private val bodies = ArrayList(initialBodyCapacity) + private val contacts = ArrayList(initialContactCapacity) + private val joints = ArrayList(initialJointCapacity) + + private val velocities = ArrayList(initialBodyCapacity) + private val positions = ArrayList(initialBodyCapacity) + + val bodiesAccess: List = Collections.unmodifiableList(bodies) + + fun clear() { + bodies.clear() + contacts.clear() + joints.clear() + + velocities.clear() + positions.clear() + } + + fun add(body: Body) { + body.islandIndex = bodies.size + bodies.add(body) + velocities.add(ru.dbotthepony.kbox2d.api.B2Velocity()) + positions.add(ru.dbotthepony.kbox2d.api.B2Position()) + } + + fun add(contact: AbstractContact) { + contacts.add(contact) + } + + fun add(joint: AbstractJoint) { + joints.add(joint) + } + + fun solve(profile: ru.dbotthepony.kbox2d.api.ProfileData, step: ru.dbotthepony.kbox2d.api.B2TimeStep, gravity: Vector2d, allowSleep: Boolean) { + val h = step.dt + + // Integrate velocities and apply damping. Initialize the body state. + for ((i, body) in bodies.withIndex()) { + val c = body.sweep.c + val a = body.sweep.a + var v = body.linearVelocity + var w = body.angularVelocity + + // Store positions for continuous collision. + body.sweep.c0 = body.sweep.c + body.sweep.a0 = body.sweep.a + + if (body.type == ru.dbotthepony.kbox2d.api.BodyType.DYNAMIC) { + // Integrate velocities. + v += (gravity * body.gravityScale * body.mass + body.force) * body.invMass * h + w += h * body.rotInertiaInv * body.torque + + // Apply damping. + // ODE: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Pade approximation: + // v2 = v1 * 1 / (1 + c * dt) + v *= 1.0 / (1.0 + h * body.linearDamping) + w *= 1.0 / (1.0 + h * body.angularDamping) + } + + positions[i].c = c + positions[i].a = a + velocities[i].v = v + velocities[i].w = w + } + + var timer = System.nanoTime() + + // Solver data + val solverData = ru.dbotthepony.kbox2d.api.B2SolverData( + step = step, + positions = positions, + velocities = velocities + ) + + // Initialize velocity constraints. + val contactSolver = ContactSolver( + step = step, + contacts = contacts, + positions = positions, + velocities = velocities, + ) + + contactSolver.initializeVelocityConstraints() + + if (step.warmStarting) { + contactSolver.warmStart() + } + + for (joint in joints) { + joint.initVelocityConstraints(solverData) + } + + profile.solveInit = System.nanoTime() - timer + timer = System.nanoTime() + + // Solve velocity constraints + for (i in 0 until step.velocityIterations) { + for (joint in joints) { + joint.solveVelocityConstraints(solverData) + } + + contactSolver.solveVelocityConstraints() + } + + // Store impulses for warm starting + contactSolver.storeImpulses() + profile.solveVelocity = System.nanoTime() - timer + timer = System.nanoTime() + + // Integrate positions + for (i in bodies.indices) { + var c = positions[i].c + var a = positions[i].a + var v = velocities[i].v + var w = velocities[i].w + + // Check for large velocities + val translation = h * v + if (translation.dotProduct(translation) > b2_maxTranslationSquared) { + v *= b2_maxTranslation / translation.length + } + + val rotation = h * w + if (rotation * rotation > b2_maxRotationSquared) { + w *= b2_maxRotation / rotation.absoluteValue + } + + // Integrate + c += h * v + a += h * w + + positions[i].c = c + positions[i].a = a + velocities[i].v = v + velocities[i].w = w + } + + profile.integratePositions = System.nanoTime() - timer + timer = System.nanoTime() + + // Solve position constraints + var positionSolved = false + + for (i in 0 until step.positionIterations) { + val contactsOkay = contactSolver.solvePositionConstraints() + var jointsOkay = true + + for (joint in joints) { + val jointOkay = joint.solvePositionConstraints(solverData) + jointsOkay = jointsOkay && jointOkay + } + + if (contactsOkay && jointsOkay) { + // Exit early if the position errors are small. + positionSolved = true + break + } + } + + // Copy state buffers back to the bodies + for ((i, body) in bodies.withIndex()) { + body.sweep.c = positions[i].c + body.sweep.a = positions[i].a + body.linearVelocity = velocities[i].v + body.angularVelocity = velocities[i].w + body.synchronizeTransform() + } + + profile.solvePosition = System.nanoTime() - timer + + report(contactSolver.velocityConstraints) + + if (allowSleep) { + var minSleepTime = Double.MAX_VALUE + + for (body in bodies) { + if (body.type == ru.dbotthepony.kbox2d.api.BodyType.STATIC) + continue + + if ( + !body.allowAutoSleep || + body.angularVelocity * body.angularVelocity > angTolSqr || + body.linearVelocity.dotProduct(body.linearVelocity) > linTolSqr + ) { + body.sleepTime = 0.0 + minSleepTime = 0.0 + } else { + body.sleepTime += h + minSleepTime = minSleepTime.coerceAtMost(body.sleepTime) + } + } + + if (minSleepTime >= b2_timeToSleep && positionSolved) { + for (body in bodies) { + body.isAwake = false + } + } + } + } + + fun solveTOI(subStep: ru.dbotthepony.kbox2d.api.B2TimeStep, toiIndexA: Int, toiIndexB: Int) { + check(toiIndexA < bodies.size) { "$toiIndexA >= ${bodies.size}" } + check(toiIndexB < bodies.size) { "$toiIndexB >= ${bodies.size}" } + + // Initialize the body state. + for ((i, body) in bodies.withIndex()) { + positions[i].c = body.sweep.c + positions[i].a = body.sweep.a + velocities[i].v = body.linearVelocity + velocities[i].w = body.angularVelocity + } + + val contactSolver = ContactSolver( + contacts = contacts, + step = subStep, + positions = positions, + velocities = velocities, + ) + + // Solve position constraints. + for (i in 0 until subStep.positionIterations) { + val contactsOkay = contactSolver.solveTOIPositionConstraints(toiIndexA, toiIndexB) + + if (contactsOkay) { + break + } + } + + // Is the new position really safe? + if (checkPositions) { + for (c in contacts) { + val fA = c.fixtureA + val fB = c.fixtureB + + val bA = fA.body!! + val bB = fB.body!! + + val indexA = c.childIndexA + val indexB = c.childIndexB + + val cache = SimplexCache() + + val output = b2Distance( + cache = cache, + proxyA = DistanceProxy(fA.shape, indexA), + proxyB = DistanceProxy(fB.shape, indexB), + transformA = bA.transform, + transformB = bB.transform, + ) + + if (output.distance == 0.0 || output.newCache.count == 3) { + // cache.count += 0; + // doesn't make much sense + } + } + } + + // Leap of faith to new safe state. + bodies[toiIndexA].sweep.c0 = positions[toiIndexA].c + bodies[toiIndexA].sweep.a0 = positions[toiIndexA].a + bodies[toiIndexB].sweep.c0 = positions[toiIndexB].c + bodies[toiIndexB].sweep.a0 = positions[toiIndexB].a + + // No warm starting is needed for TOI events because warm + // starting impulses were applied in the discrete solver. + contactSolver.initializeVelocityConstraints() + + // Solve velocity constraints. + for (i in 0 until subStep.velocityIterations) { + contactSolver.solveVelocityConstraints() + } + + // Don't store the TOI contact forces for warm starting + // because they can be quite large. + + val h = subStep.dt + + // Integrate positions + for ((i, body) in bodies.withIndex()) { + var c = positions[i].c + var a = positions[i].a + var v = velocities[i].v + var w = velocities[i].w + + // Check for large velocities + val translation = h * v + if (translation.dotProduct(translation) > b2_maxTranslationSquared) { + v *= b2_maxTranslation / translation.length + } + + val rotation = h * w + if (rotation * rotation > b2_maxRotationSquared) { + w *= b2_maxRotation / rotation.absoluteValue + } + + // Integrate + c += h * v + a += h * w + + positions[i].c = c + positions[i].a = a + velocities[i].v = v + velocities[i].w = w + + // Sync bodies + body.sweep.c = c + body.sweep.a = a + body.linearVelocity = v + body.angularVelocity = w + body.synchronizeTransform() + } + + report(contactSolver.velocityConstraints) + } + + fun report(constraints: List) { + 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) + } + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/AbstractJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/AbstractJoint.kt new file mode 100644 index 00000000..929417be --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/AbstractJoint.kt @@ -0,0 +1,144 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import it.unimi.dsi.fastutil.objects.Object2ObjectArrayMap +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.dynamics.Body +import ru.dbotthepony.kstarbound.math.Vector2d +import kotlin.math.PI + +fun interface JointFactory { + fun factorize(jointDef: IJointDef): AbstractJoint +} + +sealed class AbstractJoint(def: IJointDef) : IJoint { + init { + require(def.bodyA != def.bodyB) { "Tried to create join on same body" } + } + + internal var isOnIsland = false + + protected var index: Int = 0 + + override val collideConnected: Boolean = def.collideConnected + override val type: JointType = def.type + override var userData: Any? = def.userData + + // KBox2D: In original code, nothing expects bodies to be null + // but certain joints (notably, mouse joint) have no meaningful + // value for second body involved. So, KBox2D CAN handle case where + // joint have only one body, and to avoid null assertions in all places + // possible, bodyA and bodyB getters assert for null by themselves + // However, there is nullable getter for bodies: nullableBodyA and nullableBodyB + protected var _bodyA: Body? = def.bodyA as Body? + protected var _bodyB: Body? = def.bodyB as Body? + + final override val bodyA: Body get() = checkNotNull(_bodyA) { "Body A is not present" } + final override val bodyB: Body get() = checkNotNull(_bodyB) { "Body B is not present" } + + val hasBodyA: Boolean get() = _bodyA != null + val hasBodyB: Boolean get() = _bodyB != null + val hasTwoBodies: Boolean get() = hasBodyA && hasBodyB + + val nullableBodyA get() = _bodyA + val nullableBodyB get() = _bodyB + + private var _edgeA: JointEdge? = JointEdge(other = _bodyB, joint = this, next = _bodyA?.jointList) + private var _edgeB: JointEdge? = JointEdge(other = _bodyA, joint = this, next = _bodyB?.jointList) + + internal val edgeA: JointEdge get() = checkNotNull(_edgeA) { "Edge A is not present" } + internal val edgeB: JointEdge get() = checkNotNull(_edgeB) { "Edge B is not present" } + + var isValid: Boolean = true + private set + + init { + // Connect to the bodies' doubly linked lists. + _bodyA?.jointList?.prev = edgeA + _bodyA?.jointList = edgeA + _bodyB?.jointList?.prev = edgeB + _bodyB?.jointList = edgeB + } + + final override var next: IJoint? = null + internal set + final override var prev: IJoint? = null + internal set + + /** + * Signals that this joint was destroyed, invalidate stuff + * to fail-fast this object + */ + internal open fun unlink() { + _edgeA = null + _edgeB = null + _bodyA = null + _bodyB = null + isValid = false + } + + internal abstract fun initVelocityConstraints(data: B2SolverData) + internal abstract fun solveVelocityConstraints(data: B2SolverData) + internal abstract fun solvePositionConstraints(data: B2SolverData): Boolean + + override fun shiftOrigin(newOrigin: Vector2d) { + // no-op + } + + companion object { + private val registry = Object2ObjectArrayMap() + + 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) + } + } + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/DistanceJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/DistanceJoint.kt new file mode 100644 index 00000000..8fa5f763 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/DistanceJoint.kt @@ -0,0 +1,336 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.B2SolverData +import ru.dbotthepony.kbox2d.api.b2Max +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times +import ru.dbotthepony.kstarbound.util.Color + +class DistanceJoint(def: DistanceJointDef) : AbstractJoint(def) { + var stiffness: Double = def.stiffness + var damping: Double = def.damping + + var length: Double = b2Max(def.length, b2_linearSlop) + set(value) { + impulse = 0.0 + field = b2Max(value, b2_linearSlop) + } + + var minLength: Double = b2Max(def.minLength, b2_linearSlop) + set(value) { + lowerImpulse = 0.0 + field = b2Clamp(value, b2_linearSlop, maxLength) + } + + var maxLength: Double = b2Max(def.maxLength, b2_linearSlop) + set(value) { + upperImpulse = 0.0 + field = b2Max(value, minLength) + } + + val currentLength: Double get() { + val pA = bodyA.getWorldPoint(localAnchorA) + val pB = bodyB.getWorldPoint(localAnchorB) + val d = pB - pA + return d.length + } + + private var bias: Double = 0.0 + + // Solver shared + private val localAnchorA: Vector2d = def.localAnchorA + private val localAnchorB: Vector2d = def.localAnchorB + private var gamma: Double = 0.0 + private var impulse: Double = 0.0 + private var lowerImpulse: Double = 0.0 + private var upperImpulse: Double = 0.0 + private var _currentLength: Double = 0.0 + + // Solver temp + private var indexA: Int = 0 + private var indexB: Int = 0 + private var u: Vector2d = Vector2d.ZERO + private var rA: Vector2d = Vector2d.ZERO + private var rB: Vector2d = Vector2d.ZERO + private var localCenterA: Vector2d = Vector2d.ZERO + private var localCenterB: Vector2d = Vector2d.ZERO + private var invMassA: Double = 0.0 + private var invMassB: Double = 0.0 + private var invIA: Double = 0.0 + private var invIB: Double = 0.0 + private var softMass: Double = 0.0 + private var mass: Double = 0.0 + + override fun initVelocityConstraints(data: B2SolverData) { + indexA = bodyA.islandIndex + indexB = bodyB.islandIndex + invMassA = bodyA.invMass + invMassB = bodyB.invMass + invIA = bodyA.rotInertiaInv + invIB = bodyB.rotInertiaInv + + localCenterA = bodyA.sweep.localCenter + localCenterB = bodyB.sweep.localCenter + + val cA = data.positions[indexA].c + val aA = data.positions[indexA].a + var vA = data.velocities[indexA].v + var wA = data.velocities[indexA].w + val cB = data.positions[indexB].c + val aB = data.positions[indexB].a + var vB = data.velocities[indexB].v + var wB = data.velocities[indexB].w + + val qA = Rotation(aA) + val qB = Rotation(aB) + + rA = b2Mul(qA, localAnchorA - localCenterA) + rB = b2Mul(qB, localAnchorB - localCenterB) + u = cB + rB - cA - rA + + // Handle singularity. + _currentLength = u.length + + if (_currentLength > b2_linearSlop) { + u *= 1.0f / _currentLength + } else { + u = Vector2d.ZERO + mass = 0.0 + impulse = 0.0 + lowerImpulse = 0.0 + upperImpulse = 0.0 + } + + val crAu = b2Cross(rA, u) + val crBu = b2Cross(rB, u) + var invMass = invMassA + invIA * crAu * crAu + invMassB + invIB * crBu * crBu + mass = if (invMass != 0.0) 1.0 / invMass else 0.0 + + if (stiffness > 0.0 && minLength < maxLength) { + // soft + val C = _currentLength - length + + val d = damping + val k = stiffness + + // magic formulas + val h = data.step.dt + + // gamma = 1 / (h * (d + h * k)) + // the extra factor of h in the denominator is since the lambda is an impulse, not a force + gamma = h * (d + h * k) + gamma = if (gamma != 0.0) 1.0 / gamma else 0.0 + bias = C * h * k * gamma + + invMass += gamma + softMass = if (invMass != 0.0) 1.0 / invMass else 0.0 + } else { + // rigid + gamma = 0.0 + bias = 0.0 + softMass = mass + } + + if (data.step.warmStarting) { + // Scale the impulse to support a variable time step. + impulse *= data.step.dtRatio + lowerImpulse *= data.step.dtRatio + upperImpulse *= data.step.dtRatio + + val P = (impulse + lowerImpulse - upperImpulse) * u + vA -= invMassA * P + wA -= invIA * b2Cross(rA, P) + vB += invMassB * P + wB += invIB * b2Cross(rB, P) + } else { + impulse = 0.0 + } + + data.velocities[indexA].v = vA + data.velocities[indexA].w = wA + data.velocities[indexB].v = vB + data.velocities[indexB].w = wB + } + + override fun solveVelocityConstraints(data: B2SolverData) { + var vA = data.velocities[indexA].v + var wA = data.velocities[indexA].w + var vB = data.velocities[indexB].v + var wB = data.velocities[indexB].w + + if (minLength < maxLength) { + if (stiffness > 0.0) { + // Cdot = dot(u, v + cross(w, r)) + val vpA = vA + b2Cross(wA, rA) + val vpB = vB + b2Cross(wB, rB) + val Cdot = b2Dot(u, vpB - vpA) + + var impulse = -softMass * (Cdot + bias + gamma * impulse) + impulse += impulse + + val P = impulse * u + vA -= invMassA * P + wA -= invIA * b2Cross(rA, P) + vB += invMassB * P + wB += invIB * b2Cross(rB, P) + } + + // lower + run { + val C = _currentLength - minLength + val bias = b2Max(0.0, C) * data.step.inv_dt + + val vpA = vA + b2Cross(wA, rA) + val vpB = vB + b2Cross(wB, rB) + val Cdot = b2Dot(u, vpB - vpA) + + var impulse = -mass * (Cdot + bias) + val oldImpulse = lowerImpulse + lowerImpulse = b2Max(0.0, lowerImpulse + impulse) + impulse = lowerImpulse - oldImpulse + val P = impulse * u + + vA -= invMassA * P + wA -= invIA * b2Cross(rA, P) + vB += invMassB * P + wB += invIB * b2Cross(rB, P) + } + + // upper + run { + val C = maxLength - _currentLength + val bias = b2Max(0.0, C) * data.step.inv_dt + + val vpA = vA + b2Cross(wA, rA) + val vpB = vB + b2Cross(wB, rB) + val Cdot = b2Dot(u, vpA - vpB) + + var impulse = -mass * (Cdot + bias) + val oldImpulse = upperImpulse + upperImpulse = b2Max(0.0, upperImpulse + impulse) + impulse = upperImpulse - oldImpulse + val P = -impulse * u + + vA -= invMassA * P + wA -= invIA * b2Cross(rA, P) + vB += invMassB * P + wB += invIB * b2Cross(rB, P) + } + } else { + // Equal limits + + // Cdot = dot(u, v + cross(w, r)) + val vpA = vA + b2Cross(wA, rA) + val vpB = vB + b2Cross(wB, rB) + val Cdot = b2Dot(u, vpB - vpA) + + var impulse = -mass * Cdot + impulse += impulse + + val P = impulse * u + vA -= invMassA * P + wA -= invIA * b2Cross(rA, P) + vB += invMassB * P + wB += invIB * b2Cross(rB, P) + } + + data.velocities[indexA].v = vA + data.velocities[indexA].w = wA + data.velocities[indexB].v = vB + data.velocities[indexB].w = wB + } + + override fun solvePositionConstraints(data: B2SolverData): Boolean { + var cA = data.positions[indexA].c + var aA = data.positions[indexA].a + var cB = data.positions[indexB].c + var aB = data.positions[indexB].a + + val qA = Rotation(aA) + val qB = Rotation(aB) + + val rA = b2Mul(qA, localAnchorA - localCenterA) + val rB = b2Mul(qB, localAnchorB - localCenterB) + var u = cB + rB - cA - rA + + u.isFiniteOrThrow { + "u is invalid, $cB, $rB, $cA, $rA" + } + + val length = u.length + u = u.normalized + val C: Double + + if (minLength == maxLength) { + C = length - minLength + } else if (length < minLength) { + C = length - minLength + } else if (maxLength < length) { + C = length - maxLength + } else { + return true + } + + val impulse = -mass * C + val P = impulse * u + + P.isFiniteOrThrow { + "P is not finite, impulse: $impulse, u: $u, mass: $mass, C: $C" + } + + cA -= invMassA * P + aA -= invIA * b2Cross(rA, P) + cB += invMassB * P + aB += invIB * b2Cross(rB, P) + + data.positions[indexA].c = cA + data.positions[indexA].a = aA + data.positions[indexB].c = cB + data.positions[indexB].a = aB + + return b2Abs(C) < b2_linearSlop + } + + override fun getReactionForce(inv_dt: Double): Vector2d { + return inv_dt * (impulse + lowerImpulse - upperImpulse) * u + } + + override fun getReactionTorque(inv_dt: Double): Double { + return 0.0 + } + + override val anchorA: Vector2d get() = bodyA.getWorldPoint(localAnchorA) + override val anchorB: Vector2d get() = bodyB.getWorldPoint(localAnchorB) + + override fun draw(draw: IDebugDraw) { + val pA = b2Mul(bodyA.transform, localAnchorA) + val pB = b2Mul(bodyB.transform, localAnchorB) + + val axis = (pB - pA).normalized + + draw.drawSegment(pA, pB, c4) + + val pRest = pA + length * axis + draw.drawPoint(pRest, 8.0, c1) + + if (minLength != maxLength) { + if (minLength > b2_linearSlop) { + val pMin = pA + minLength * axis + draw.drawPoint(pMin, 4.0, c2) + } + + if (maxLength < Double.MAX_VALUE) { + val pMax = pA + maxLength * axis + draw.drawPoint(pMax, 4.0, c3) + } + } + } + + companion object { + private val c1 = Color(0.7f, 0.7f, 0.7f) + private val c2 = Color(0.3f, 0.9f, 0.3f) + private val c3 = Color(0.9f, 0.3f, 0.3f) + private val c4 = Color(0.4f, 0.4f, 0.4f) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/FrictionJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/FrictionJoint.kt new file mode 100644 index 00000000..4c8de0b4 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/FrictionJoint.kt @@ -0,0 +1,210 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.B2SolverData +import ru.dbotthepony.kbox2d.api.b2Mul +import ru.dbotthepony.kstarbound.math.MutableMatrix2d +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times + +// Point-to-point constraint +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Angle constraint +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +/** + * Friction joint. This is used for top-down friction. + * It provides 2D translational friction and angular friction. + */ +class FrictionJoint(def: FrictionJointDef) : AbstractJoint(def) { + val localAnchorA: Vector2d = def.localAnchorA + val localAnchorB: Vector2d = def.localAnchorB + + // Solver shared + private var linearImpulse: Vector2d = Vector2d.ZERO + private var angularImpulse: Double = 0.0 + var maxForce: Double = def.maxForce + set(value) { + require(!value.isNaN()) { "Tried to set NaN force" } + require(value.isFinite()) { "Tried to set infinite force" } + require(value >= 0.0) { "Tried to set negative force: $value" } + field = value + } + + var maxTorque: Double = def.maxTorque + set(value) { + require(!value.isNaN()) { "Tried to set NaN torque" } + require(value.isFinite()) { "Tried to set infinite torque" } + require(value >= 0.0) { "Tried to set negative torque: $value" } + field = value + } + + + // Solver temp + private var indexA: Int = 0 + private var indexB: Int = 0 + private var rA: Vector2d = Vector2d.ZERO + private var rB: Vector2d = Vector2d.ZERO + private var localCenterA: Vector2d = Vector2d.ZERO + private var localCenterB: Vector2d = Vector2d.ZERO + private var invMassA: Double = 0.0 + private var invMassB: Double = 0.0 + private var invIA: Double = 0.0 + private var invIB: Double = 0.0 + private var linearMass: MutableMatrix2d = MutableMatrix2d() + private var angularMass: Double = 0.0 + + override fun initVelocityConstraints(data: B2SolverData) { + this.indexA = this.bodyA.islandIndex + this.indexB = this.bodyB.islandIndex + this.localCenterA = this.bodyA.sweep.localCenter + this.localCenterB = this.bodyB.sweep.localCenter + this.invMassA = this.bodyA.invMass + this.invMassB = this.bodyB.invMass + this.invIA = this.bodyA.invI + this.invIB = this.bodyB.invI + + val aA = data.positions[this.indexA].a + var vA = data.velocities[this.indexA].v + var wA = data.velocities[this.indexA].w + + val aB = data.positions[this.indexB].a + var vB = data.velocities[this.indexB].v + var wB = data.velocities[this.indexB].w + + val qA = Rotation(aA) + val qB = Rotation(aB) + + // Compute the effective mass matrix. + this.rA = b2Mul(qA, this.localAnchorA - this.localCenterA) + this.rB = b2Mul(qB, this.localAnchorB - this.localCenterB) + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + val mA = this.invMassA + val mB = this.invMassB + val iA = this.invIA + val iB = this.invIB + + val K = MutableMatrix2d() + K.m00 = mA + mB + iA * this.rA.y * this.rA.y + iB * this.rB.y * this.rB.y + K.m10 = -iA * this.rA.x * this.rA.y - iB * this.rB.x * this.rB.y + K.m01 = K.m10 + K.m11 = mA + mB + iA * this.rA.x * this.rA.x + iB * this.rB.x * this.rB.x + + this.linearMass = K.getInverse().asMutableMatrix() + + this.angularMass = iA + iB + if (this.angularMass > 0.0) { + this.angularMass = 1.0 / this.angularMass + } + + if (data.step.warmStarting) { + // Scale impulses to support a variable time step. + this.linearImpulse *= data.step.dtRatio + this.angularImpulse *= data.step.dtRatio + + val P = Vector2d(this.linearImpulse.x, this.linearImpulse.y) + vA -= mA * P + wA -= iA * (b2Cross(this.rA, P) + this.angularImpulse) + vB += mB * P + wB += iB * (b2Cross(this.rB, P) + this.angularImpulse) + } else { + this.linearImpulse = Vector2d.ZERO + this.angularImpulse = 0.0 + } + + data.velocities[this.indexA].v = vA + data.velocities[this.indexA].w = wA + data.velocities[this.indexB].v = vB + data.velocities[this.indexB].w = wB + } + + override fun solveVelocityConstraints(data: B2SolverData) { + var vA = data.velocities[this.indexA].v + var wA = data.velocities[this.indexA].w + var vB = data.velocities[this.indexB].v + var wB = data.velocities[this.indexB].w + + val mA = this.invMassA + val mB = this.invMassB + val iA = this.invIA + val iB = this.invIB + + val h = data.step.dt + + // Solve angular friction + run { + val Cdot = wB - wA + var impulse = -this.angularMass * Cdot + + val oldImpulse = this.angularImpulse + val maxImpulse = h * this.maxTorque + this.angularImpulse = b2Clamp(this.angularImpulse + impulse, -maxImpulse, maxImpulse) + impulse = this.angularImpulse - oldImpulse + + wA -= iA * impulse + wB += iB * impulse + } + + // Solve linear friction + run { + val Cdot = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA) + + var impulse = -b2Mul(this.linearMass, Cdot) + val oldImpulse = this.linearImpulse + this.linearImpulse += impulse + + val maxImpulse = h * this.maxForce + + if (this.linearImpulse.lengthSquared > maxImpulse * maxImpulse) { + this.linearImpulse = this.linearImpulse.normalized + this.linearImpulse *= maxImpulse + } + + impulse = this.linearImpulse - oldImpulse + + vA -= mA * impulse + wA -= iA * b2Cross(this.rA, impulse) + + vB += mB * impulse + wB += iB * b2Cross(this.rB, impulse) + } + + data.velocities[this.indexA].v = vA + data.velocities[this.indexA].w = wA + data.velocities[this.indexB].v = vB + data.velocities[this.indexB].w = wB + } + + override fun solvePositionConstraints(data: B2SolverData): Boolean { + return true + } + + override val anchorA: Vector2d + get() = bodyA.getWorldPoint(localAnchorA) + override val anchorB: Vector2d + get() = bodyB.getWorldPoint(localAnchorB) + + override fun getReactionForce(inv_dt: Double): Vector2d { + return inv_dt * linearImpulse + } + + override fun getReactionTorque(inv_dt: Double): Double { + return inv_dt * angularImpulse + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/GearJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/GearJoint.kt new file mode 100644 index 00000000..006b1da0 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/GearJoint.kt @@ -0,0 +1,468 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.dynamics.Body +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times + +// Gear Joint: +// C0 = (coordinate1 + ratio * coordinate2)_initial +// C = (coordinate1 + ratio * coordinate2) - C0 = 0 +// J = [J1 ratio * J2] +// K = J * invM * JT +// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T +// +// Revolute: +// coordinate = rotation +// Cdot = angularVelocity +// J = [0 0 1] +// K = J * invM * JT = invI +// +// Prismatic: +// coordinate = dot(p - pg, ug) +// Cdot = dot(v + cross(w, r), ug) +// J = [ug cross(r, ug)] +// K = J * invM * JT = invMass + invI * cross(r, ug)^2 + +/** + * A gear joint is used to connect two joints together. Either joint + * can be a revolute or prismatic joint. You specify a gear ratio + * to bind the motions together: + * coordinate1 + ratio * coordinate2 = constant + * The ratio can be negative or positive. If one joint is a revolute joint + * and the other joint is a prismatic joint, then the ratio will have units + * of length or units of 1/length. + * @warning You have to manually destroy the gear joint if joint1 or joint2 + * is destroyed. + */ +class GearJoint(def: GearJointDef) : AbstractJoint(def) { + // KBox2D: Due to multiple local variables names clashing with class fields + // this class contain hard reference to this in methods to clear confusion + init { + require(def.joint1 != def.joint2) { "Definition specify the same joint ${def.joint1}" } + } + + /** + * Get the first joint. + */ + val joint1: AbstractJoint = def.joint1 + + /** + * Get the second joint. + */ + val joint2: AbstractJoint = def.joint2 + + val typeA: JointType = def.joint1.type + val typeB: JointType = def.joint2.type + + init { + check(typeA == JointType.REVOLUTE || typeA == JointType.PRISMATIC) { "Invalid joint A: $typeA" } + check(typeB == JointType.REVOLUTE || typeB == JointType.PRISMATIC) { "Invalid joint B: $typeB" } + } + + // Solver shared + private var localAnchorA: Vector2d + private var localAnchorB: Vector2d + private var localAnchorC: Vector2d + private var localAnchorD: Vector2d + + private var localAxisC: Vector2d + private var localAxisD: Vector2d + + private var referenceAngleA: Double + private var referenceAngleB: Double + + private var constant: Double + + /** + * Set/Get the gear ratio. + */ + var ratio: Double = def.ratio + set(value) { + if (!value.isFinite()) { + throw IllegalArgumentException("Tried to set infinite ratio") + } + + if (value.isNaN()) { + throw IllegalArgumentException("Tried to set NaN ratio") + } + + field = value + } + + private var tolerance: Double + + private var impulse: Double = 0.0 + + // Solver temp + private var indexA: Int = 0 + private var indexB: Int = 0 + private var indexC: Int = 0 + private var indexD: Int = 0 + + private var lcA: Vector2d = Vector2d.ZERO + private var lcB: Vector2d = Vector2d.ZERO + private var lcC: Vector2d = Vector2d.ZERO + private var lcD: Vector2d = Vector2d.ZERO + + private var mA: Double = 0.0 + private var mB: Double = 0.0 + private var mC: Double = 0.0 + private var mD: Double = 0.0 + private var iA: Double = 0.0 + private var iB: Double = 0.0 + private var iC: Double = 0.0 + private var iD: Double = 0.0 + + private var JvAC: Vector2d = Vector2d.ZERO + private var JvBD: Vector2d = Vector2d.ZERO + + private var JwA: Double = 0.0 + private var JwB: Double = 0.0 + private var JwC: Double = 0.0 + private var JwD: Double = 0.0 + + private var mass: Double = 0.0 + + // Body A is connected to body C + // Body B is connected to body D + private var _bodyC: Body? = joint1.bodyA + private var _bodyD: Body? + + override fun unlink() { + super.unlink() + _bodyC = null + _bodyD = null + } + + val bodyC: Body get() = checkNotNull(_bodyC) { "Body C is not present" } + val bodyD: Body get() = checkNotNull(_bodyD) { "Body D is not present" } + + init { + _bodyA = joint1.bodyB + + val coordinateA: Double + val coordinateB: Double + + // Body B on joint1 must be dynamic + check(bodyA.type == BodyType.DYNAMIC) { "Body A is expected to be DYNAMIC, hot ${bodyA.type}" } + + // Get geometry of joint1 + val xfA = bodyA.transform + val aA = bodyA.sweep.a + val xfC = bodyC.transform + val aC = bodyC.sweep.a + + if (typeA == JointType.REVOLUTE) { + val revolute = def.joint1 as RevoluteJoint + + localAnchorC = revolute.localAnchorA + localAnchorA = revolute.localAnchorB + referenceAngleA = revolute.referenceAngle + localAxisC = Vector2d.ZERO + + coordinateA = aA - aC - referenceAngleA + + // position error is measured in radians + tolerance = b2_angularSlop + } else { + val prismatic = def.joint1 as PrismaticJoint + localAnchorC = prismatic.localAnchorA + localAnchorA = prismatic.localAnchorB + referenceAngleA = prismatic.referenceAngle + localAxisC = prismatic.localXAxisA + + val pC = localAnchorC + val pA = b2MulT(xfC.q, b2Mul(xfA.q, localAnchorA) + (xfA.p - xfC.p)) + coordinateA = b2Dot(pA - pC, localAxisC) + + // position error is measured in meters + tolerance = b2_linearSlop + } + + _bodyD = joint2.bodyA + _bodyB = joint2.bodyB + + // Body B on joint2 must be dynamic + check(bodyB.type == BodyType.DYNAMIC) { "Body is expected to be DYNAMIC, got ${bodyB.type}"} + + // Get geometry of joint2 + val xfB = bodyB.transform + val aB = bodyB.sweep.a + val xfD = bodyD.transform + val aD = bodyD.sweep.a + + if (typeB == JointType.REVOLUTE) { + val revolute = def.joint2 as RevoluteJoint + localAnchorD = revolute.localAnchorA + localAnchorB = revolute.localAnchorB + referenceAngleB = revolute.referenceAngle + localAxisD = Vector2d.ZERO + + coordinateB = aB - aD - referenceAngleB + } else { + val prismatic = def.joint2 as PrismaticJoint + localAnchorD = prismatic.localAnchorA + localAnchorB = prismatic.localAnchorB + referenceAngleB = prismatic.referenceAngle + localAxisD = prismatic.localXAxisA + + val pD = localAnchorD + val pB = b2MulT(xfD.q, b2Mul(xfB.q, localAnchorB) + (xfB.p - xfD.p)) + coordinateB = b2Dot(pB - pD, localAxisD) + } + + constant = coordinateA + ratio * coordinateB + } + + override fun initVelocityConstraints(data: B2SolverData) { + if (!joint1.isValid || !joint2.isValid) { + throw IllegalStateException("$this is orphaned!") + } + + this.indexA = this.bodyA.islandIndex + this.indexB = this.bodyB.islandIndex + this.indexC = this.bodyC.islandIndex + this.indexD = this.bodyD.islandIndex + this.lcA = this.bodyA.sweep.localCenter + this.lcB = this.bodyB.sweep.localCenter + this.lcC = this.bodyC.sweep.localCenter + this.lcD = this.bodyD.sweep.localCenter + this.mA = this.bodyA.invMass + this.mB = this.bodyB.invMass + this.mC = this.bodyC.invMass + this.mD = this.bodyD.invMass + this.iA = this.bodyA.rotInertiaInv + this.iB = this.bodyB.rotInertiaInv + this.iC = this.bodyC.rotInertiaInv + this.iD = this.bodyD.rotInertiaInv + + val aA = data.positions[this.indexA].a + var vA = data.velocities[this.indexA].v + var wA = data.velocities[this.indexA].w + + val aB = data.positions[this.indexB].a + var vB = data.velocities[this.indexB].v + var wB = data.velocities[this.indexB].w + + val aC = data.positions[this.indexC].a + var vC = data.velocities[this.indexC].v + var wC = data.velocities[this.indexC].w + + val aD = data.positions[this.indexD].a + var vD = data.velocities[this.indexD].v + var wD = data.velocities[this.indexD].w + + val qA = Rotation(aA) + val qB = Rotation(aB) + val qC = Rotation(aC) + val qD = Rotation(aD) + + this.mass = 0.0 + + if (this.typeA == JointType.REVOLUTE) { + this.JvAC = Vector2d.ZERO + this.JwA = 1.0 + this.JwC = 1.0 + this.mass += this.iA + this.iC + } else { + val u = b2Mul(qC, this.localAxisC) + val rC = b2Mul(qC, this.localAnchorC - this.lcC) + val rA = b2Mul(qA, this.localAnchorA - this.lcA) + this.JvAC = u + this.JwC = b2Cross(rC, u) + this.JwA = b2Cross(rA, u) + this.mass += this.mC + this.mA + this.iC * this.JwC * this.JwC + this.iA * this.JwA * this.JwA + } + + if (this.typeB == JointType.REVOLUTE) { + this.JvBD = Vector2d.ZERO + this.JwB = this.ratio + this.JwD = this.ratio + this.mass += this.ratio * this.ratio * (this.iB + this.iD) + } else { + val u = b2Mul(qD, this.localAxisD) + val rD = b2Mul(qD, this.localAnchorD - this.lcD) + val rB = b2Mul(qB, this.localAnchorB - this.lcB) + this.JvBD = this.ratio * u + this.JwD = this.ratio * b2Cross(rD, u) + this.JwB = this.ratio * b2Cross(rB, u) + this.mass += this.ratio * this.ratio * (this.mD + this.mB) + this.iD * this.JwD * this.JwD + this.iB * this.JwB * this.JwB + } + + // Compute effective mass. + this.mass = if (this.mass > 0.0) 1.0 / this.mass else 0.0 + + if (data.step.warmStarting) { + vA += (this.mA * this.impulse) * this.JvAC + wA += this.iA * this.impulse * this.JwA + vB += (this.mB * this.impulse) * this.JvBD + wB += this.iB * this.impulse * this.JwB + vC -= (this.mC * this.impulse) * this.JvAC + wC -= this.iC * this.impulse * this.JwC + vD -= (this.mD * this.impulse) * this.JvBD + wD -= this.iD * this.impulse * this.JwD + } else { + this.impulse = 0.0 + } + + data.velocities[this.indexA].v = vA + data.velocities[this.indexA].w = wA + data.velocities[this.indexB].v = vB + data.velocities[this.indexB].w = wB + data.velocities[this.indexC].v = vC + data.velocities[this.indexC].w = wC + data.velocities[this.indexD].v = vD + data.velocities[this.indexD].w = wD + } + + override fun solveVelocityConstraints(data: B2SolverData) { + var vA = data.velocities[this.indexA].v + var wA = data.velocities[this.indexA].w + var vB = data.velocities[this.indexB].v + var wB = data.velocities[this.indexB].w + var vC = data.velocities[this.indexC].v + var wC = data.velocities[this.indexC].w + var vD = data.velocities[this.indexD].v + var wD = data.velocities[this.indexD].w + + var Cdot = b2Dot(this.JvAC, vA - vC) + b2Dot(this.JvBD, vB - vD) + Cdot += (this.JwA * wA - this.JwC * wC) + (this.JwB * wB - this.JwD * wD) + + val impulse = -this.mass * Cdot + this.impulse += impulse + + vA += (this.mA * impulse) * this.JvAC + wA += this.iA * impulse * this.JwA + vB += (this.mB * impulse) * this.JvBD + wB += this.iB * impulse * this.JwB + vC -= (this.mC * impulse) * this.JvAC + wC -= this.iC * impulse * this.JwC + vD -= (this.mD * impulse) * this.JvBD + wD -= this.iD * impulse * this.JwD + + data.velocities[this.indexA].v = vA + data.velocities[this.indexA].w = wA + data.velocities[this.indexB].v = vB + data.velocities[this.indexB].w = wB + data.velocities[this.indexC].v = vC + data.velocities[this.indexC].w = wC + data.velocities[this.indexD].v = vD + data.velocities[this.indexD].w = wD + } + + override fun solvePositionConstraints(data: B2SolverData): Boolean { + var cA = data.positions[this.indexA].c + var aA = data.positions[this.indexA].a + var cB = data.positions[this.indexB].c + var aB = data.positions[this.indexB].a + var cC = data.positions[this.indexC].c + var aC = data.positions[this.indexC].a + var cD = data.positions[this.indexD].c + var aD = data.positions[this.indexD].a + + val qA = Rotation(aA) + val qB = Rotation(aB) + val qC = Rotation(aC) + val qD = Rotation(aD) + + val coordinateA: Double + val coordinateB: Double + + val JvAC: Vector2d + val JvBD: Vector2d + val JwA: Double + val JwB: Double + val JwC: Double + val JwD: Double + + var mass = 0.0 + + if (this.typeA == JointType.REVOLUTE) { + JvAC = Vector2d.ZERO + JwA = 1.0 + JwC = 1.0 + mass += this.iA + this.iC + + coordinateA = aA - aC - this.referenceAngleA + } else { + val u = b2Mul(qC, this.localAxisC) + val rC = b2Mul(qC, this.localAnchorC - this.lcC) + val rA = b2Mul(qA, this.localAnchorA - this.lcA) + JvAC = u + JwC = b2Cross(rC, u) + JwA = b2Cross(rA, u) + mass += this.mC + this.mA + this.iC * JwC * JwC + this.iA * JwA * JwA + + val pC = this.localAnchorC - this.lcC + val pA = b2MulT(qC, rA + (cA - cC)) + coordinateA = b2Dot(pA - pC, this.localAxisC) + } + + if (this.typeB == JointType.REVOLUTE) { + JvBD = Vector2d.ZERO + JwB = this.ratio + JwD = this.ratio + mass += this.ratio * this.ratio * (this.iB + this.iD) + + coordinateB = aB - aD - this.referenceAngleB + } else { + val u = b2Mul(qD, this.localAxisD) + val rD = b2Mul(qD, this.localAnchorD - this.lcD) + val rB = b2Mul(qB, this.localAnchorB - this.lcB) + JvBD = this.ratio * u + JwD = this.ratio * b2Cross(rD, u) + JwB = this.ratio * b2Cross(rB, u) + mass += this.ratio * this.ratio * (this.mD + this.mB) + this.iD * JwD * JwD + this.iB * JwB * JwB + + val pD = this.localAnchorD - this.lcD + val pB = b2MulT(qD, rB + (cB - cD)) + coordinateB = b2Dot(pB - pD, this.localAxisD) + } + + val C = (coordinateA + this.ratio * coordinateB) - this.constant + + var impulse = 0.0 + + if (mass > 0.0f) { + impulse = -C / mass + } + + cA += this.mA * impulse * JvAC + aA += this.iA * impulse * JwA + cB += this.mB * impulse * JvBD + aB += this.iB * impulse * JwB + cC -= this.mC * impulse * JvAC + aC -= this.iC * impulse * JwC + cD -= this.mD * impulse * JvBD + aD -= this.iD * impulse * JwD + + data.positions[this.indexA].c = cA + data.positions[this.indexA].a = aA + data.positions[this.indexB].c = cB + data.positions[this.indexB].a = aB + data.positions[this.indexC].c = cC + data.positions[this.indexC].a = aC + data.positions[this.indexD].c = cD + data.positions[this.indexD].a = aD + + if (b2Abs(C) < this.tolerance) { + return true + } + + return false + } + + override val anchorA: Vector2d + get() = bodyA.getWorldPoint(localAnchorA) + override val anchorB: Vector2d + get() = bodyB.getWorldPoint(localAnchorB) + + override fun getReactionForce(inv_dt: Double): Vector2d { + return inv_dt * (impulse * JvAC) + } + + override fun getReactionTorque(inv_dt: Double): Double { + return inv_dt * (impulse * JwA) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MotorJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MotorJoint.kt new file mode 100644 index 00000000..929e0393 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MotorJoint.kt @@ -0,0 +1,225 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.B2SolverData +import ru.dbotthepony.kbox2d.api.b2Mul +import ru.dbotthepony.kstarbound.math.MutableMatrix2d +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times + +class MotorJoint(def: MotorJointDef) : AbstractJoint(def) { + // Solver shared + var linearOffset: Vector2d = def.linearOffset + set(value) { + if (value != field) { + field = value + bodyA.isAwake = true + bodyB.isAwake = true + } + } + + var angularOffset: Double = def.angularOffset + set(value) { + if (value != field) { + field = value + bodyA.isAwake = true + bodyB.isAwake = true + } + } + + private var linearImpulse: Vector2d = Vector2d.ZERO + private var angularImpulse: Double = 0.0 + + var maxForce: Double = def.maxForce + set(value) { + require(!value.isNaN()) { "Tried to set NaN force" } + require(value.isFinite()) { "Tried to set infinite force" } + require(value >= 0.0) { "Tried to set negative force: $value" } + field = value + } + + var maxTorque: Double = def.maxTorque + set(value) { + require(!value.isNaN()) { "Tried to set NaN torque" } + require(value.isFinite()) { "Tried to set infinite torque" } + require(value >= 0.0) { "Tried to set negative torque: $value" } + field = value + } + + var correctionFactor: Double = def.correctionFactor + set(value) { + require(!value.isNaN()) { "Tried to set NaN correction factor" } + require(value.isFinite()) { "Tried to set infinite correction factor" } + require(value in 0.0 .. 1.0) { "Tried to set correction factor out of bounds: $value" } + field = value + } + + // Solver temp + private var indexA: Int = 0 + private var indexB: Int = 0 + private var rA: Vector2d = Vector2d.ZERO + private var rB: Vector2d = Vector2d.ZERO + private var localCenterA: Vector2d = Vector2d.ZERO + private var localCenterB: Vector2d = Vector2d.ZERO + private var linearError: Vector2d = Vector2d.ZERO + private var angularError: Double = 0.0 + private var invMassA: Double = 0.0 + private var invMassB: Double = 0.0 + private var invIA: Double = 0.0 + private var invIB: Double = 0.0 + private var linearMass: MutableMatrix2d = MutableMatrix2d() + private var angularMass: Double = 0.0 + + override fun initVelocityConstraints(data: B2SolverData) { + this.indexA = this.bodyA.islandIndex + this.indexB = this.bodyB.islandIndex + this.localCenterA = this.bodyA.sweep.localCenter + this.localCenterB = this.bodyB.sweep.localCenter + this.invMassA = this.bodyA.invMass + this.invMassB = this.bodyB.invMass + this.invIA = this.bodyA.invI + this.invIB = this.bodyB.invI + + val cA = data.positions[this.indexA].c + val aA = data.positions[this.indexA].a + var vA = data.velocities[this.indexA].v + var wA = data.velocities[this.indexA].w + + val cB = data.positions[this.indexB].c + val aB = data.positions[this.indexB].a + var vB = data.velocities[this.indexB].v + var wB = data.velocities[this.indexB].w + + val qA = Rotation(aA) + val qB = Rotation(aB) + + // Compute the effective mass matrix. + this.rA = b2Mul(qA, this.linearOffset - this.localCenterA) + this.rB = b2Mul(qB, -this.localCenterB) + + // J = [-I -r1_skew I r2_skew] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + val mA = this.invMassA + val mB = this.invMassB + val iA = this.invIA + val iB = this.invIB + + // Upper 2 by 2 of K for point to point + val K = MutableMatrix2d() + K.m00 = mA + mB + iA * this.rA.y * this.rA.y + iB * this.rB.y * this.rB.y + K.m10 = -iA * this.rA.x * this.rA.y - iB * this.rB.x * this.rB.y + K.m01 = K.m10 + K.m11 = mA + mB + iA * this.rA.x * this.rA.x + iB * this.rB.x * this.rB.x + + this.linearMass = K.getInverse().asMutableMatrix() + + this.angularMass = iA + iB + if (this.angularMass > 0.0) { + this.angularMass = 1.0 / this.angularMass + } + + this.linearError = cB + this.rB - cA - this.rA + this.angularError = aB - aA - this.angularOffset + + if (data.step.warmStarting) { + // Scale impulses to support a variable time step. + this.linearImpulse *= data.step.dtRatio + this.angularImpulse *= data.step.dtRatio + + val P = Vector2d(this.linearImpulse.x, this.linearImpulse.y) + vA -= mA * P + wA -= iA * (b2Cross(this.rA, P) + this.angularImpulse) + vB += mB * P + wB += iB * (b2Cross(this.rB, P) + this.angularImpulse) + } else { + this.linearImpulse = Vector2d.ZERO + this.angularImpulse = 0.0 + } + + data.velocities[this.indexA].v = vA + data.velocities[this.indexA].w = wA + data.velocities[this.indexB].v = vB + data.velocities[this.indexB].w = wB + } + + override fun solveVelocityConstraints(data: B2SolverData) { + var vA = data.velocities[this.indexA].v + var wA = data.velocities[this.indexA].w + var vB = data.velocities[this.indexB].v + var wB = data.velocities[this.indexB].w + + val mA = this.invMassA + val mB = this.invMassB + val iA = this.invIA + val iB = this.invIB + + val h = data.step.dt + val inv_h = data.step.inv_dt + + // Solve angular friction + run { + val Cdot = wB - wA + inv_h * this.correctionFactor * this.angularError + var impulse = -this.angularMass * Cdot + + val oldImpulse = this.angularImpulse + val maxImpulse = h * this.maxTorque + this.angularImpulse = b2Clamp(this.angularImpulse + impulse, -maxImpulse, maxImpulse) + impulse = this.angularImpulse - oldImpulse + + wA -= iA * impulse + wB += iB * impulse + } + + // Solve linear friction + run { + val Cdot = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA) + inv_h * this.correctionFactor * this.linearError + + var impulse = -b2Mul(this.linearMass, Cdot) + val oldImpulse = this.linearImpulse + this.linearImpulse += impulse + + val maxImpulse = h * this.maxForce + + if (this.linearImpulse.lengthSquared > maxImpulse * maxImpulse) { + this.linearImpulse = this.linearImpulse.normalized + this.linearImpulse *= maxImpulse + } + + impulse = this.linearImpulse - oldImpulse + + vA -= mA * impulse + wA -= iA * b2Cross(this.rA, impulse) + + vB += mB * impulse + wB += iB * b2Cross(this.rB, impulse) + } + + data.velocities[this.indexA].v = vA + data.velocities[this.indexA].w = wA + data.velocities[this.indexB].v = vB + data.velocities[this.indexB].w = wB + } + + override fun solvePositionConstraints(data: B2SolverData): Boolean { + return true + } + + override val anchorA: Vector2d + get() = bodyA.position + override val anchorB: Vector2d + get() = bodyB.position + + override fun getReactionForce(inv_dt: Double): Vector2d { + return inv_dt * linearImpulse + } + + override fun getReactionTorque(inv_dt: Double): Double { + return inv_dt * angularImpulse + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MouseJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MouseJoint.kt new file mode 100644 index 00000000..c9cc2cd3 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/MouseJoint.kt @@ -0,0 +1,150 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.B2SolverData +import ru.dbotthepony.kbox2d.api.b2MulT +import ru.dbotthepony.kstarbound.math.MutableMatrix2d +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times + +// p = attached point, m = mouse point +// C = p - m +// Cdot = v +// = v + cross(w, r) +// J = [I r_skew] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +class MouseJoint(def: MouseJointDef) : AbstractJoint(def) { + private val localAnchorB: Vector2d = b2MulT(bodyB.transform, def.target) + var targetA: Vector2d = def.target + set(value) { + value.isFiniteOrThrow { "Tried to set illegal target $value" } + field = value + bodyB.isAwake = true + } + + var maxForce = def.maxForce + var stiffness = def.stiffness + var damping = def.damping + + // Solver shared + private var impulse: Vector2d = Vector2d.ZERO + private var gamma: Double = 0.0 + private var beta: Double = 0.0 + + // Solver temp + private var indexA: Int = 0 + private var indexB: Int = 0 + private var rB: Vector2d = Vector2d.ZERO + private var localCenterB: Vector2d = Vector2d.ZERO + private var invMassB: Double = 0.0 + private var invIB: Double = 0.0 + private var mass: MutableMatrix2d = MutableMatrix2d().also { it.zero() } + private var C: Vector2d = Vector2d.ZERO + + override fun initVelocityConstraints(data: B2SolverData) { + indexB = bodyB.islandIndex + localCenterB = bodyB.sweep.localCenter + invMassB = bodyB.invMass + invIB = bodyB.invI + + val cB = data.positions[indexB].c + val aB = data.positions[indexB].a + var vB = data.velocities[indexB].v + var wB = data.velocities[indexB].w + + val qB = Rotation(aB) + + val d = damping + val k = stiffness + + // magic formulas + // gamma has units of inverse mass. + // beta has units of inverse time. + val h = data.step.dt + gamma = h * (d + h * k) + + if (gamma != 0.0) { + gamma = 1.0f / gamma + } + + beta = h * k * gamma + + // Compute the effective mass matrix. + rB = b2Mul(qB, localAnchorB - localCenterB) + + // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] + // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] + val K = MutableMatrix2d() + K.m00 = invMassB + invIB * rB.y * rB.y + gamma + K.m10 = -invIB * rB.x * rB.y + K.m01 = K.m10 + K.m11 = invMassB + invIB * rB.x * rB.x + gamma + + mass = K.getInverse().asMutableMatrix() + + C = cB + rB - targetA + C *= beta + + // Cheat with some damping + wB *= 0.98f + + if (data.step.warmStarting) { + impulse *= data.step.dtRatio + vB += invMassB * impulse + wB += invIB * b2Cross(rB, impulse) + } else { + impulse = Vector2d.ZERO + } + + data.velocities[indexB].v = vB + data.velocities[indexB].w = wB + } + + override fun solveVelocityConstraints(data: B2SolverData) { + var vB = data.velocities[indexB].v; + var wB = data.velocities[indexB].w; + + // Cdot = v + cross(w, r) + val Cdot = vB + b2Cross(wB, rB); + var impulse = b2Mul(mass, -(Cdot + C + gamma * impulse)); + + val oldImpulse = this.impulse; + this.impulse += impulse; + val maxImpulse = data.step.dt * maxForce; + if (this.impulse.lengthSquared > maxImpulse * maxImpulse) { + this.impulse *= maxImpulse / this.impulse.length + } + + impulse = this.impulse - oldImpulse; + + vB += invMassB * impulse; + wB += invIB * b2Cross(rB, impulse); + + data.velocities[indexB].v = vB; + data.velocities[indexB].w = wB; + } + + override fun solvePositionConstraints(data: B2SolverData): Boolean { + return true + } + + override val anchorA: Vector2d + get() = targetA + override val anchorB: Vector2d + get() = bodyB.getWorldPoint(localAnchorB) + + override fun getReactionForce(inv_dt: Double): Vector2d { + return inv_dt * impulse + } + + override fun getReactionTorque(inv_dt: Double): Double { + return 0.0 + } + + override fun shiftOrigin(newOrigin: Vector2d) { + targetA -= newOrigin + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PrismaticJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PrismaticJoint.kt new file mode 100644 index 00000000..af5b6459 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PrismaticJoint.kt @@ -0,0 +1,605 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.B2SolverData +import ru.dbotthepony.kbox2d.api.b2Cross +import ru.dbotthepony.kbox2d.api.b2Mul +import ru.dbotthepony.kstarbound.math.* +import ru.dbotthepony.kstarbound.util.Color + +// Linear constraint (point-to-line) +// d = p2 - p1 = x2 + r2 - x1 - r1 +// C = dot(perp, d) +// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) +// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] +// +// Angular constraint +// C = a2 - a1 + a_initial +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// +// K = J * invM * JT +// +// J = [-a -s1 a s2] +// [0 -1 0 1] +// a = perp +// s1 = cross(d + r1, a) = cross(p2 - x1, a) +// s2 = cross(r2, a) = cross(p2 - x2, a) + +// Motor/Limit linear constraint +// C = dot(ax1, d) +// Cdot = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) +// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] + +// Predictive limit is applied even when the limit is not active. +// Prevents a constraint speed that can lead to a constraint error in one time step. +// Want C2 = C1 + h * Cdot >= 0 +// Or: +// Cdot + C1/h >= 0 +// I do not apply a negative constraint error because that is handled in position correction. +// So: +// Cdot + max(C1, 0)/h >= 0 + +// Block Solver +// We develop a block solver that includes the angular and linear constraints. This makes the limit stiffer. +// +// The Jacobian has 2 rows: +// J = [-uT -s1 uT s2] // linear +// [0 -1 0 1] // angular +// +// u = perp +// s1 = cross(d + r1, u), s2 = cross(r2, u) +// a1 = cross(d + r1, v), a2 = cross(r2, v) + +class PrismaticJoint(def: PrismaticJointDef) : AbstractJoint(def) { + internal val localAnchorA: Vector2d = def.localAnchorA + internal val localAnchorB: Vector2d = def.localAnchorB + internal val localXAxisA: Vector2d = def.localAxisA.normalized + internal val localYAxisA: Vector2d = b2Cross(1.0, localXAxisA) + internal val referenceAngle: Double = def.referenceAngle + + private var impulse: Vector2d = Vector2d.ZERO + private var motorImpulse: Double = 0.0 + private var lowerImpulse: Double = 0.0 + private var upperImpulse: Double = 0.0 + + // Solver temp + private var indexA: Int = 0 + private var indexB: Int = 0 + private var localCenterA: Vector2d = Vector2d.ZERO + private var localCenterB: Vector2d = Vector2d.ZERO + private var invMassA: Double = 0.0 + private var invMassB: Double = 0.0 + private var invIA: Double = 0.0 + private var invIB: Double = 0.0 + private var axis: Vector2d = Vector2d.ZERO + private var perp: Vector2d = Vector2d.ZERO + private var s1: Double = 0.0 + private var s2: Double = 0.0 + private var a1: Double = 0.0 + private var a2: Double = 0.0 + private var K: MutableMatrix2d = MutableMatrix2d() + private var translation: Double = 0.0 + private var axialMass: Double = 0.0 + + override fun initVelocityConstraints(data: B2SolverData) { + indexA = bodyA.islandIndex + indexB = bodyB.islandIndex + localCenterA = bodyA.sweep.localCenter + localCenterB = bodyB.sweep.localCenter + invMassA = bodyA.invMass + invMassB = bodyB.invMass + invIA = bodyA.rotInertiaInv + invIB = bodyB.rotInertiaInv + + val cA = data.positions[indexA].c + val aA = data.positions[indexA].a + var vA = data.velocities[indexA].v + var wA = data.velocities[indexA].w + + val cB = data.positions[indexB].c + val aB = data.positions[indexB].a + var vB = data.velocities[indexB].v + var wB = data.velocities[indexB].w + + val qA = Rotation(aA) + val qB = Rotation(aB) + + // Compute the effective masses. + val rA = b2Mul(qA, localAnchorA - localCenterA) + val rB = b2Mul(qB, localAnchorB - localCenterB) + val d = (cB - cA) + rB - rA + + val mA = invMassA + val mB = invMassB + val iA = invIA + val iB = invIB + + // Compute motor Jacobian and effective mass. + run { + this.axis = b2Mul(qA, this.localXAxisA) + this.a1 = b2Cross(d + rA, this.axis) + this.a2 = b2Cross(rB, this.axis) + + this.axialMass = mA + mB + iA * this.a1 * this.a1 + iB * this.a2 * this.a2 + if (this.axialMass > 0.0f) + { + this.axialMass = 1.0f / this.axialMass + } + } + + // Prismatic constraint. + run { + this.perp = b2Mul(qA, this.localYAxisA) + + this.s1 = b2Cross(d + rA, this.perp) + this.s2 = b2Cross(rB, this.perp) + + val k11 = mA + mB + iA * this.s1 * this.s1 + iB * this.s2 * this.s2 + val k12 = iA * this.s1 + iB * this.s2 + var k22 = iA + iB + + if (k22 == 0.0) { + // For bodies with fixed rotation. + k22 = 1.0 + } + + this.K.m00 = k11 + this.K.m10 = k12 + + this.K.m01 = k12 + this.K.m11 = k22 + } + + if (enableLimit) { + translation = b2Dot(axis, d) + } else { + lowerImpulse = 0.0 + upperImpulse = 0.0 + } + + if (!enableMotor) { + motorImpulse = 0.0 + } + + if (data.step.warmStarting) { + // Account for variable time step. + impulse *= data.step.dtRatio + motorImpulse *= data.step.dtRatio + lowerImpulse *= data.step.dtRatio + upperImpulse *= data.step.dtRatio + + val axialImpulse = motorImpulse + lowerImpulse - upperImpulse + val P = impulse.x * perp + axialImpulse * axis + val LA = impulse.x * s1 + impulse.y + axialImpulse * a1 + val LB = impulse.x * s2 + impulse.y + axialImpulse * a2 + + vA -= mA * P + wA -= iA * LA + + vB += mB * P + wB += iB * LB + } else { + impulse = Vector2d.ZERO + motorImpulse = 0.0 + lowerImpulse = 0.0 + upperImpulse = 0.0 + } + + data.velocities[indexA].v = vA + data.velocities[indexA].w = wA + data.velocities[indexB].v = vB + data.velocities[indexB].w = wB + } + + override fun solveVelocityConstraints(data: B2SolverData) { + var vA = data.velocities[indexA].v + var wA = data.velocities[indexA].w + var vB = data.velocities[indexB].v + var wB = data.velocities[indexB].w + + val mA = invMassA + val mB = invMassB + val iA = invIA + val iB = invIB + + // Solve linear motor constraint + if (enableMotor) { + val Cdot = b2Dot(axis, vB - vA) + a2 * wB - a1 * wA + var impulse = axialMass * (motorSpeed - Cdot) + val oldImpulse = motorImpulse + val maxImpulse = data.step.dt * maxMotorForce + motorImpulse = b2Clamp(motorImpulse + impulse, -maxImpulse, maxImpulse) + impulse = motorImpulse - oldImpulse + + val P = impulse * axis + val LA = impulse * a1 + val LB = impulse * a2 + + vA -= mA * P + wA -= iA * LA + vB += mB * P + wB += iB * LB + } + + if (enableLimit) { + // Lower limit + run { + val C = this.translation - this.lowerTranslation + val Cdot = b2Dot(this.axis, vB - vA) + this.a2 * wB - this.a1 * wA + var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt) + val oldImpulse = this.lowerImpulse + this.lowerImpulse = b2Max(this.lowerImpulse + impulse, 0.0) + impulse = this.lowerImpulse - oldImpulse + + val P = impulse * this.axis + val LA = impulse * this.a1 + val LB = impulse * this.a2 + + vA -= mA * P + wA -= iA * LA + vB += mB * P + wB += iB * LB + } + + // Upper limit + // Note: signs are flipped to keep C positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + run { + val C = this.upperTranslation - this.translation + val Cdot = b2Dot(this.axis, vA - vB) + this.a1 * wA - this.a2 * wB + var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt) + val oldImpulse = this.upperImpulse + this.upperImpulse = b2Max(this.upperImpulse + impulse, 0.0) + impulse = this.upperImpulse - oldImpulse + + val P = impulse * this.axis + val LA = impulse * this.a1 + val LB = impulse * this.a2 + + vA += mA * P + wA += iA * LA + vB -= mB * P + wB -= iB * LB + } + } + + // Solve the prismatic constraint in block form. + run { + val Cdot = Vector2d( + x = b2Dot(this.perp, vB - vA) + this.s2 * wB - this.s1 * wA, + y = wB - wA + ) + + val df = this.K.solve(-Cdot) + this.impulse += df + + val P = df.x * this.perp + val LA = df.x * this.s1 + df.y + val LB = df.x * this.s2 + df.y + + vA -= mA * P + wA -= iA * LA + + vB += mB * P + wB += iB * LB + } + + data.velocities[indexA].v = vA + data.velocities[indexA].w = wA + data.velocities[indexB].v = vB + data.velocities[indexB].w = wB + } + + /** + * A velocity based solver computes reaction forces(impulses) using the velocity constraint solver.Under this context, + * the position solver is not there to resolve forces.It is only there to cope with integration error. + * + * Therefore, the pseudo impulses in the position solver do not have any physical meaning.Thus it is okay if they suck. + * + * We could take the active state from the velocity solver.However, the joint might push past the limit when the velocity + * solver indicates the limit is inactive. + */ + override fun solvePositionConstraints(data: B2SolverData): Boolean { + var cA = data.positions[indexA].c + var aA = data.positions[indexA].a + var cB = data.positions[indexB].c + var aB = data.positions[indexB].a + + val qA = Rotation(aA) + val qB = Rotation(aB) + + val mA = invMassA + val mB = invMassB + val iA = invIA + val iB = invIB + + // Compute fresh Jacobians + val rA = b2Mul(qA, localAnchorA - localCenterA) + val rB = b2Mul(qB, localAnchorB - localCenterB) + val d = cB + rB - cA - rA + + val axis = b2Mul(qA, localXAxisA) + val a1 = b2Cross(d + rA, axis) + val a2 = b2Cross(rB, axis) + val perp = b2Mul(qA, localYAxisA) + + val s1 = b2Cross(d + rA, perp) + val s2 = b2Cross(rB, perp) + + val impulse: Vector3d + + val C1 = Vector2d( + x = b2Dot(perp, d), + y = aB - aA - referenceAngle + ) + + var linearError = b2Abs(C1.x) + val angularError = b2Abs(C1.y) + + var active = false + var C2 = 0.0 + + if (enableLimit) { + val translation = b2Dot(axis, d) + if (b2Abs(upperTranslation - lowerTranslation) < 2.0 * b2_linearSlop) { + C2 = translation + linearError = b2Max(linearError, b2Abs(translation)) + active = true + } else if (translation <= lowerTranslation) { + C2 = b2Min(translation - lowerTranslation, 0.0) + linearError = b2Max(linearError, lowerTranslation - translation) + active = true + } else if (translation >= upperTranslation) { + C2 = b2Max(translation - upperTranslation, 0.0) + linearError = b2Max(linearError, translation - upperTranslation) + active = true + } + } + + if (active) { + val k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2 + val k12 = iA * s1 + iB * s2 + val k13 = iA * s1 * a1 + iB * s2 * a2 + var k22 = iA + iB + + if (k22 == 0.0) { + // For fixed rotation + k22 = 1.0 + } + + val k23 = iA * a1 + iB * a2 + val k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2 + + val K = MutableMatrix3d() + + K.m00 = k11 + K.m10 = k12 + K.m20 = k13 + + K.m01 = k12 + K.m11 = k22 + K.m21 = k23 + + K.m02 = k13 + K.m12 = k23 + K.m22 = k33 + + val C = Vector3d( + x = C1.x, + y = C1.y, + z = C2, + ) + + impulse = K.solve(-C) + } else { + val k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2 + val k12 = iA * s1 + iB * s2 + var k22 = iA + iB + + if (k22 == 0.0) { + k22 = 1.0 + } + + val K = MutableMatrix2d() + + K.m00 = k11 + K.m10 = k12 + + K.m01 = k12 + K.m11 = k22 + + val impulse1 = K.solve(-C1) + impulse = Vector3d(impulse1.x, impulse1.y) + } + + val P = impulse.x * perp + impulse.z * axis + val LA = impulse.x * s1 + impulse.y + impulse.z * a1 + val LB = impulse.x * s2 + impulse.y + impulse.z * a2 + + cA -= mA * P + aA -= iA * LA + cB += mB * P + aB += iB * LB + + data.positions[indexA].c = cA + data.positions[indexA].a = aA + data.positions[indexB].c = cB + data.positions[indexB].a = aB + + return linearError <= b2_linearSlop && angularError <= b2_angularSlop + } + + override val anchorA: Vector2d + get() = bodyA.getWorldPoint(localAnchorA) + override val anchorB: Vector2d + get() = bodyB.getWorldPoint(localAnchorB) + + override fun getReactionForce(inv_dt: Double): Vector2d { + return inv_dt * (impulse.x * perp + (motorImpulse + lowerImpulse - upperImpulse) * axis) + } + + override fun getReactionTorque(inv_dt: Double): Double { + return inv_dt * impulse.y + } + + /** + * Get the current joint translation, usually in meters. + */ + val jointTranslation: Double get() { + val pA = bodyA.getWorldPoint(localAnchorA) + val pB = bodyB.getWorldPoint(localAnchorB) + val d = pB - pA + val axis = bodyA.getWorldVector(localXAxisA) + + return b2Dot(d, axis) + } + + /** + * Get the current joint translation speed, usually in meters per second. + */ + val jointSpeed: Double get() { + val bA = bodyA + val bB = bodyB + + val rA = b2Mul(bA.transform.q, localAnchorA - bA.sweep.localCenter) + val rB = b2Mul(bB.transform.q, localAnchorB - bB.sweep.localCenter) + val p1 = bA.sweep.c + rA + val p2 = bB.sweep.c + rB + val d = p2 - p1 + val axis = b2Mul(bA.transform.q, localXAxisA) + + val vA = bA.linearVelocity + val vB = bB.linearVelocity + val wA = bA.angularVelocity + val wB = bB.angularVelocity + + return b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)) + } + + /** + * Get the lower joint limit, usually in meters. + */ + var lowerTranslation: Double = def.lowerTranslation + private set + + /** + * Get the upper joint limit, usually in meters. + */ + var upperTranslation: Double = def.upperTranslation + private set + + /** + * Set the joint limits, usually in meters. + */ + fun setLimits(lower: Double, upper: Double) { + require(lower <= upper) { "$lower !<= $upper" } + + if (lower != lowerTranslation || upper != upperTranslation) { + bodyA.isAwake = true + bodyB.isAwake = true + lowerImpulse = 0.0 + upperImpulse = 0.0 + lowerTranslation = lower + upperTranslation = upper + } + } + + /** + * Set the maximum motor force, usually in N. + */ + var maxMotorForce: Double = def.maxMotorForce + set(value) { + if (value != field) { + field = value + + if (enableMotor) { + bodyA.isAwake = true + bodyB.isAwake = true + } + } + } + + /** + * Set the motor speed, usually in meters per second. + */ + var motorSpeed: Double = def.motorSpeed + set(value) { + if (value != field) { + field = value + + if (enableMotor) { + bodyA.isAwake = true + bodyB.isAwake = true + } + } + } + + /** + * Enable/disable the joint limit. + */ + var enableLimit: Boolean = def.enableLimit + set(value) { + if (value != field) { + field = value + bodyA.isAwake = true + bodyB.isAwake = true + lowerImpulse = 0.0 + upperImpulse = 0.0 + } + } + + /** + * Enable/disable the joint motor. + */ + var enableMotor: Boolean = def.enableMotor + set(value) { + if (value != field) { + field = value + bodyA.isAwake = true + bodyB.isAwake = true + } + } + + init { + require(lowerTranslation <= upperTranslation) { "$lowerTranslation !<= $upperTranslation" } + } + + fun getMotorForce(inv_dt: Double): Double { + return inv_dt * motorImpulse + } + + override fun draw(draw: IDebugDraw) { + val xfA = bodyA.transform + val xfB = bodyB.transform + val pA = b2Mul(xfA, localAnchorA) + val pB = b2Mul(xfB, localAnchorB) + + val axis = b2Mul(xfA.q, localXAxisA) + + draw.drawSegment(pA, pB, c5) + + if (enableLimit) { + val lower = pA + lowerTranslation * axis + val upper = pA + upperTranslation * axis + val perp = b2Mul(xfA.q, localYAxisA) + draw.drawSegment(lower, upper, c1) + draw.drawSegment(lower - 0.5 * perp, lower + 0.5 * perp, c2) + draw.drawSegment(upper - 0.5 * perp, upper + 0.5 * perp, c3) + } else { + draw.drawSegment(pA - 1.0 * axis, pA + 1.0 * axis, c1) + } + + draw.drawPoint(pA, 5.0, c1) + draw.drawPoint(pB, 5.0, c4) + } + + companion object { + private val c1 = Color(0.7f, 0.7f, 0.7f) + private val c2 = Color(0.3f, 0.9f, 0.3f) + private val c3 = Color(0.9f, 0.3f, 0.3f) + private val c4 = Color(0.3f, 0.3f, 0.9f) + private val c5 = Color(0.4f, 0.4f, 0.4f) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PulleyJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PulleyJoint.kt new file mode 100644 index 00000000..6c9c5e02 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/PulleyJoint.kt @@ -0,0 +1,281 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.B2SolverData +import ru.dbotthepony.kbox2d.api.b2Mul +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times + +/** + * The pulley joint is connected to two bodies and two fixed ground points. + * The pulley supports a ratio such that: + * length1 + ratio * length2 <= constant + * Yes, the force transmitted is scaled by the ratio. + * Warning: the pulley joint can get a bit squirrelly by itself. They often + * work better when combined with prismatic joints. You should also cover the + * the anchor points with static shapes to prevent one side from going to + * zero length. + */ +class PulleyJoint(def: PulleyJointDef) : AbstractJoint(def) { + /** + * Get the first ground anchor. + */ + var groundAnchorA = def.groundAnchorA + private set + + /** + * Get the second ground anchor. + */ + var groundAnchorB = def.groundAnchorB + private set + + /** + * Get the current length of the segment attached to bodyA. + */ + val lengthA = def.lengthA + + /** + * Get the current length of the segment attached to bodyB. + */ + val lengthB = def.lengthB + + // Solver shared + private val localAnchorA: Vector2d = def.localAnchorA + private val localAnchorB: Vector2d = def.localAnchorB + private val constant: Double = def.lengthA + def.ratio * def.lengthB + + /** + * Get the pulley ratio. + */ + val ratio: Double = def.ratio + + init { + require(ratio != 0.0) { "Ratio is zero" } + } + + private var impulse: Double = 0.0 + + // Solver temp + private var indexA: Int = 0 + private var indexB: Int = 0 + private var uA: Vector2d = Vector2d.ZERO + private var uB: Vector2d = Vector2d.ZERO + private var rA: Vector2d = Vector2d.ZERO + private var rB: Vector2d = Vector2d.ZERO + private var localCenterA: Vector2d = Vector2d.ZERO + private var localCenterB: Vector2d = Vector2d.ZERO + private var invMassA: Double = 0.0 + private var invMassB: Double = 0.0 + private var invIA: Double = 0.0 + private var invIB: Double = 0.0 + private var mass: Double = 0.0 + + override fun initVelocityConstraints(data: B2SolverData) { + indexA = bodyA.islandIndex + indexB = bodyB.islandIndex + localCenterA = bodyA.sweep.localCenter + localCenterB = bodyB.sweep.localCenter + invMassA = bodyA.invMass + invMassB = bodyB.invMass + invIA = bodyA.rotInertiaInv + invIB = bodyB.rotInertiaInv + + val cA = data.positions[indexA].c + val aA = data.positions[indexA].a + var vA = data.velocities[indexA].v + var wA = data.velocities[indexA].w + + val cB = data.positions[indexB].c + val aB = data.positions[indexB].a + var vB = data.velocities[indexB].v + var wB = data.velocities[indexB].w + + val qA = Rotation(aA) + val qB = Rotation(aB) + + rA = b2Mul(qA, localAnchorA - localCenterA) + rB = b2Mul(qB, localAnchorB - localCenterB) + + // Get the pulley axes. + uA = cA + rA - groundAnchorA + uB = cB + rB - groundAnchorB + + val lengthA = uA.length + val lengthB = uB.length + + if (lengthA > 10.0 * b2_linearSlop) { + uA *= 1.0 / lengthA + } else { + uA = Vector2d.ZERO + } + + if (lengthB > 10.0 * b2_linearSlop) { + uB *= 1.0 / lengthB + } else { + uB = Vector2d.ZERO + } + + // Compute effective mass. + val ruA = b2Cross(rA, uA) + val ruB = b2Cross(rB, uB) + + val mA = invMassA + invIA * ruA * ruA + val mB = invMassB + invIB * ruB * ruB + + mass = mA + ratio * ratio * mB + + if (mass > 0.0) { + mass = 1.0 / mass + } + + if (data.step.warmStarting) { + // Scale impulses to support variable time steps. + impulse *= data.step.dtRatio + + // Warm starting. + val PA = -(impulse) * uA + val PB = (-ratio * impulse) * uB + + vA += invMassA * PA + wA += invIA * b2Cross(rA, PA) + vB += invMassB * PB + wB += invIB * b2Cross(rB, PB) + } else { + impulse = 0.0 + } + + data.velocities[indexA].v = vA + data.velocities[indexA].w = wA + data.velocities[indexB].v = vB + data.velocities[indexB].w = wB + } + + override fun solveVelocityConstraints(data: B2SolverData) { + var vA = data.velocities[indexA].v + var wA = data.velocities[indexA].w + var vB = data.velocities[indexB].v + var wB = data.velocities[indexB].w + + val vpA = vA + b2Cross(wA, rA) + val vpB = vB + b2Cross(wB, rB) + + val Cdot = -b2Dot(uA, vpA) - ratio * b2Dot(uB, vpB) + val impulse = -mass * Cdot + this.impulse += impulse + + val PA = -impulse * uA + val PB = -ratio * impulse * uB + vA += invMassA * PA + wA += invIA * b2Cross(rA, PA) + vB += invMassB * PB + wB += invIB * b2Cross(rB, PB) + + data.velocities[indexA].v = vA + data.velocities[indexA].w = wA + data.velocities[indexB].v = vB + data.velocities[indexB].w = wB + } + + override fun solvePositionConstraints(data: B2SolverData): Boolean { + var cA = data.positions[indexA].c + var aA = data.positions[indexA].a + var cB = data.positions[indexB].c + var aB = data.positions[indexB].a + + val qA = Rotation(aA) + val qB = Rotation(aB) + + val rA = b2Mul(qA, localAnchorA - localCenterA) + val rB = b2Mul(qB, localAnchorB - localCenterB) + + // Get the pulley axes. + var uA = cA + rA - groundAnchorA + var uB = cB + rB - groundAnchorB + + val lengthA = uA.length + val lengthB = uB.length + + if (lengthA > 10.0 * b2_linearSlop) { + uA *= 1.0 / lengthA + } else { + uA = Vector2d.ZERO + } + + if (lengthB > 10.0 * b2_linearSlop) { + uB *= 1.0 / lengthB + } else { + uB = Vector2d.ZERO + } + + // Compute effective mass. + val ruA = b2Cross(rA, uA) + val ruB = b2Cross(rB, uB) + + val mA = invMassA + invIA * ruA * ruA + val mB = invMassB + invIB * ruB * ruB + + var mass = mA + ratio * ratio * mB + + if (mass > 0.0f) { + mass = 1.0f / mass + } + + val C = constant - lengthA - ratio * lengthB + val linearError = b2Abs(C) + + val impulse = -mass * C + + val PA = -impulse * uA + val PB = -ratio * impulse * uB + + cA += invMassA * PA + aA += invIA * b2Cross(rA, PA) + cB += invMassB * PB + aB += invIB * b2Cross(rB, PB) + + data.positions[indexA].c = cA + data.positions[indexA].a = aA + data.positions[indexB].c = cB + data.positions[indexB].a = aB + + return linearError < b2_linearSlop + } + + override val anchorA: Vector2d + get() = bodyA.getWorldPoint(localAnchorA) + override val anchorB: Vector2d + get() = bodyB.getWorldPoint(localAnchorB) + + override fun getReactionForce(inv_dt: Double): Vector2d { + return inv_dt * (impulse * uB) + } + + override fun getReactionTorque(inv_dt: Double): Double { + return 0.0 + } + + /** + * Get the current length of the segment attached to bodyA. + */ + val currentLengthA: Double get() { + val p = bodyA.getWorldPoint(localAnchorA) + val s = groundAnchorA + val d = p - s + return d.length + } + + /** + * Get the current length of the segment attached to bodyB. + */ + val currentLengthB: Double get() { + val p = bodyB.getWorldPoint(localAnchorB) + val s = groundAnchorB + val d = p - s + return d.length + } + + override fun shiftOrigin(newOrigin: Vector2d) { + groundAnchorA -= newOrigin + groundAnchorB -= newOrigin + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/RevoluteJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/RevoluteJoint.kt new file mode 100644 index 00000000..394e558b --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/RevoluteJoint.kt @@ -0,0 +1,463 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.B2SolverData +import ru.dbotthepony.kbox2d.api.b2Mul +import ru.dbotthepony.kstarbound.math.MutableMatrix2d +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times +import ru.dbotthepony.kstarbound.util.Color +import kotlin.math.cos +import kotlin.math.sin + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Motor constraint +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +class RevoluteJoint(def: RevoluteJointDef) : AbstractJoint(def) { + // Solver shared + val localAnchorA: Vector2d = def.localAnchorA + val localAnchorB: Vector2d = def.localAnchorB + private var impulse: Vector2d = Vector2d.ZERO + private var motorImpulse: Double = 0.0 + private var lowerImpulse: Double = 0.0 + private var upperImpulse: Double = 0.0 + val referenceAngle: Double = def.referenceAngle + + // Solver temp + private var indexA: Int = 0 + private var indexB: Int = 0 + private var rA: Vector2d = Vector2d.ZERO + private var rB: Vector2d = Vector2d.ZERO + private var localCenterA: Vector2d = Vector2d.ZERO + private var localCenterB: Vector2d = Vector2d.ZERO + private var invMassA: Double = 0.0 + private var invMassB: Double = 0.0 + private var invIA: Double = 0.0 + private var invIB: Double = 0.0 + private var K: MutableMatrix2d = MutableMatrix2d().also { it.zero() } + private var angle: Double = 0.0 + private var axialMass: Double = 0.0 + + override fun initVelocityConstraints(data: B2SolverData) { + indexA = bodyA.islandIndex + indexB = bodyB.islandIndex + localCenterA = bodyA.sweep.localCenter + localCenterB = bodyB.sweep.localCenter + invMassA = bodyA.invMass + invMassB = bodyB.invMass + invIA = bodyA.rotInertiaInv + invIB = bodyB.rotInertiaInv + + val aA = data.positions[indexA].a + var vA = data.velocities[indexA].v + var wA = data.velocities[indexA].w + + val aB = data.positions[indexB].a + var vB = data.velocities[indexB].v + var wB = data.velocities[indexB].w + + val qA = Rotation(aA) + val qB = Rotation(aB) + + rA = b2Mul(qA, localAnchorA - localCenterA) + rB = b2Mul(qB, localAnchorB - localCenterB) + + // J = [-I -r1_skew I r2_skew] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB] + + val mA = invMassA + val mB = invMassB + val iA = invIA + val iB = invIB + + K.m00 = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB + K.m01 = -rA.y * rA.x * iA - rB.y * rB.x * iB + K.m10 = K.m01 + K.m11 = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB + + axialMass = iA + iB + val fixedRotation: Boolean + + if (axialMass > 0.0f) { + axialMass = 1.0f / axialMass + fixedRotation = false + } else { + fixedRotation = true + } + + angle = aB - aA - referenceAngle + + if (!enableLimit || fixedRotation) { + lowerImpulse = 0.0 + upperImpulse = 0.0 + } + + if (!enableMotor || fixedRotation) { + motorImpulse = 0.0 + } + + if (data.step.warmStarting) { + // Scale impulses to support a variable time step. + impulse *= data.step.dtRatio + motorImpulse *= data.step.dtRatio + lowerImpulse *= data.step.dtRatio + upperImpulse *= data.step.dtRatio + + val axialImpulse = motorImpulse + lowerImpulse - upperImpulse + val P = Vector2d(impulse.x, impulse.y) + + vA -= mA * P + wA -= iA * (b2Cross(rA, P) + axialImpulse) + + vB += mB * P + wB += iB * (b2Cross(rB, P) + axialImpulse) + } else { + impulse = Vector2d.ZERO + motorImpulse = 0.0 + lowerImpulse = 0.0 + upperImpulse = 0.0 + } + + data.velocities[indexA].v = vA + data.velocities[indexA].w = wA + data.velocities[indexB].v = vB + data.velocities[indexB].w = wB + } + + override fun solveVelocityConstraints(data: B2SolverData) { + var vA = data.velocities[indexA].v + var wA = data.velocities[indexA].w + var vB = data.velocities[indexB].v + var wB = data.velocities[indexB].w + + val mA = invMassA + val mB = invMassB + val iA = invIA + val iB = invIB + + val fixedRotation = iA + iB == 0.0 + + // Solve motor constraint. + if (enableMotor && !fixedRotation) { + val Cdot = wB - wA - motorSpeed + var impulse = -axialMass * Cdot + val oldImpulse = motorImpulse + val maxImpulse = data.step.dt * maxMotorTorque + motorImpulse = b2Clamp(motorImpulse + impulse, -maxImpulse, maxImpulse) + impulse = motorImpulse - oldImpulse + + wA -= iA * impulse + wB += iB * impulse + } + + if (enableLimit && !fixedRotation) { + // Lower limit + run { + val C = this.angle - this.lowerAngle + val Cdot = wB - wA + var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt) + val oldImpulse = this.lowerImpulse + this.lowerImpulse = b2Max(this.lowerImpulse + impulse, 0.0) + impulse = this.lowerImpulse - oldImpulse + + wA -= iA * impulse + wB += iB * impulse + } + + // Upper limit + // Note: signs are flipped to keep C positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + run { + val C = this.upperAngle - this.angle + val Cdot = wA - wB + var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt) + val oldImpulse = this.upperImpulse + this.upperImpulse = b2Max(this.upperImpulse + impulse, 0.0) + impulse = this.upperImpulse - oldImpulse + + wA += iA * impulse + wB -= iB * impulse + } + } + + // Solve point-to-point constraint + run { + val Cdot = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA) + val impulse = this.K.solve(-Cdot) + + this.impulse += impulse + + vA -= mA * impulse + wA -= iA * b2Cross(this.rA, impulse) + + vB += mB * impulse + wB += iB * b2Cross(this.rB, impulse) + } + + data.velocities[indexA].v = vA + data.velocities[indexA].w = wA + data.velocities[indexB].v = vB + data.velocities[indexB].w = wB + } + + override fun solvePositionConstraints(data: B2SolverData): Boolean { + var cA = data.positions[indexA].c + var aA = data.positions[indexA].a + var cB = data.positions[indexB].c + var aB = data.positions[indexB].a + + val qA = Rotation(aA) + val qB = Rotation(aB) + + var angularError = 0.0 + var positionError = 0.0 + + val fixedRotation = invIA + invIB == 0.0 + + // Solve angular limit constraint + if (enableLimit && !fixedRotation) { + val angle = aB - aA - referenceAngle + var C = 0.0 + + if (b2Abs(upperAngle - lowerAngle) < 2.0f * b2_angularSlop) { + // Prevent large angular corrections + C = b2Clamp(angle - lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection) + } else if (angle <= lowerAngle) { + // Prevent large angular corrections and allow some slop. + C = b2Clamp(angle - lowerAngle + b2_angularSlop, -b2_maxAngularCorrection, 0.0) + } else if (angle >= upperAngle) { + // Prevent large angular corrections and allow some slop. + C = b2Clamp(angle - upperAngle - b2_angularSlop, 0.0, b2_maxAngularCorrection) + } + + val limitImpulse = -axialMass * C + aA -= invIA * limitImpulse + aB += invIB * limitImpulse + angularError = b2Abs(C) + } + + // Solve point-to-point constraint. + run { + qA.set(aA) + qB.set(aB) + val rA = b2Mul(qA, this.localAnchorA - this.localCenterA) + val rB = b2Mul(qB, this.localAnchorB - this.localCenterB) + + val C = cB + rB - cA - rA + positionError = C.length + + val mA = this.invMassA + val mB = this.invMassB + val iA = this.invIA + val iB = this.invIB + + val K = MutableMatrix2d() + K.m00 = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y + K.m10 = -iA * rA.x * rA.y - iB * rB.x * rB.y + K.m01 = K.m10 + K.m11 = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x + + val impulse = -K.solve(C) + + cA -= mA * impulse + aA -= iA * b2Cross(rA, impulse) + + cB += mB * impulse + aB += iB * b2Cross(rB, impulse) + } + + data.positions[indexA].c = cA + data.positions[indexA].a = aA + data.positions[indexB].c = cB + data.positions[indexB].a = aB + + return positionError <= b2_linearSlop && angularError <= b2_angularSlop + } + + /** + * Get the reaction force given the inverse time step. + * + * Unit is N. + */ + override fun getReactionForce(inv_dt: Double): Vector2d { + return inv_dt * Vector2d(impulse.x, impulse.y) + } + + /** + * Get the reaction torque due to the joint limit given the inverse time step. + * + * Unit is N*m. + */ + override fun getReactionTorque(inv_dt: Double): Double { + return inv_dt * (motorImpulse + lowerImpulse - upperImpulse) + } + + override val anchorA: Vector2d + get() = bodyA.getWorldPoint(localAnchorA) + override val anchorB: Vector2d + get() = bodyB.getWorldPoint(localAnchorB) + + /** + * Get the current joint angle in radians. + */ + val jointAngle: Double get() { + return bodyB.sweep.a - bodyA.sweep.a - referenceAngle + } + + /** + * Get the current joint angle speed in radians per second. + */ + val jointSpeed: Double get() { + return bodyB.angularVelocity - bodyA.angularVelocity + } + + var enableMotor: Boolean = def.enableMotor + set(value) { + if (value != field) { + field = value + bodyA.isAwake = true + bodyB.isAwake = true + } + } + + /** + * Get the current motor torque given the inverse time step. + * + * Unit is N*m. + */ + fun getMotorTorque(inv_dt: Double): Double { + return inv_dt * motorImpulse + } + + /** + * Set the motor speed in radians per second. + */ + var motorSpeed: Double = def.motorSpeed + set(value) { + if (field != value) { + if (enableMotor) { + bodyA.isAwake = true + bodyB.isAwake = true + } + + field = value + } + } + + /** + * Set the maximum motor torque, usually in N-m. + */ + var maxMotorTorque: Double = def.maxMotorTorque + set(value) { + if (field != value) { + if (enableMotor) { + bodyA.isAwake = true + bodyB.isAwake = true + } + + field = value + } + } + + /** + * Enable/disable the joint limit. + */ + var enableLimit: Boolean = def.enableLimit + set(value) { + if (field != value) { + bodyA.isAwake = true + bodyB.isAwake = true + field = value + lowerImpulse = 0.0 + upperImpulse = 0.0 + } + } + + /** + * Get the lower joint limit in radians. + */ + var lowerAngle: Double = def.lowerAngle + private set + + /** + * Get the upper joint limit in radians. + */ + var upperAngle: Double = def.upperAngle + private set + + init { + // KBox2D: Overlooked check in constructor + // TODO: Notify Erin + require(lowerAngle <= upperAngle) { "$lowerAngle !<= $upperAngle" } + } + + /** + * Set the joint limits in radians. + */ + fun setLimits(lower: Double, upper: Double) { + require(lower <= upper) { "$lower !<= $upper" } + + if (lower != lowerAngle || upper != upperAngle) { + bodyA.isAwake = true + bodyB.isAwake = true + lowerImpulse = 0.0 + upperImpulse = 0.0 + lowerAngle = lower + upperAngle = upper + } + } + + override fun draw(draw: IDebugDraw) { + val xfA = bodyA.transform + val xfB = bodyB.transform + val pA = b2Mul(xfA, localAnchorA) + val pB = b2Mul(xfB, localAnchorB) + + draw.drawPoint(pA, 5.0, c4) + draw.drawPoint(pB, 5.0, c5) + + val aA = bodyA.angle + val aB = bodyB.angle + val angle = aB - aA - referenceAngle + + val r = L * Vector2d(cos(angle), sin(angle)) + draw.drawSegment(pB, pB + r, c1) + draw.drawCircle(pB, L, c1) + + if (enableLimit) { + val rlo = L * Vector2d(cos(lowerAngle), sin(lowerAngle)) + val rhi = L * Vector2d(cos(upperAngle), sin(upperAngle)) + + draw.drawSegment(pB, pB + rlo, c2) + draw.drawSegment(pB, pB + rhi, c3) + } + + draw.drawSegment(xfA.p, pA, color) + draw.drawSegment(pA, pB, color) + draw.drawSegment(xfB.p, pB, color) + } + + companion object { + private const val L = 0.5 + private val color = Color(0.5f, 0.8f, 0.8f) + private val c1 = Color(0.7f, 0.7f, 0.7f) + private val c2 = Color(0.3f, 0.9f, 0.3f) + private val c3 = Color(0.9f, 0.3f, 0.3f) + private val c4 = Color(0.3f, 0.3f, 0.9f) + private val c5 = Color(0.4f, 0.4f, 0.4f) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WeldJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WeldJoint.kt new file mode 100644 index 00000000..bcafe22a --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WeldJoint.kt @@ -0,0 +1,281 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.B2SolverData +import ru.dbotthepony.kbox2d.api.b2Mul +import ru.dbotthepony.kstarbound.math.* + +class WeldJoint(def: WeldJointDef) : AbstractJoint(def) { + var stiffness: Double = def.stiffness + var damping: Double = def.damping + private var bias: Double = 0.0 + + // Solver shared + val localAnchorA = def.localAnchorA + val localAnchorB = def.localAnchorB + val referenceAngle = def.referenceAngle + private var gamma: Double = 0.0 + private var impulse: Vector3d = Vector3d() + + // Solver temp + private var indexA: Int = 0 + private var indexB: Int = 0 + private var rA: Vector2d = Vector2d.ZERO + private var rB: Vector2d = Vector2d.ZERO + private var localCenterA: Vector2d = Vector2d.ZERO + private var localCenterB: Vector2d = Vector2d.ZERO + private var invMassA: Double = 0.0 + private var invMassB: Double = 0.0 + private var invIA: Double = 0.0 + private var invIB: Double = 0.0 + private var mass: MutableMatrix3d = MutableMatrix3d().also { it.zero() } + + override fun initVelocityConstraints(data: B2SolverData) { + this.indexA = this.bodyA.islandIndex + this.indexB = this.bodyB.islandIndex + this.localCenterA = this.bodyA.sweep.localCenter + this.localCenterB = this.bodyB.sweep.localCenter + this.invMassA = this.bodyA.invMass + this.invMassB = this.bodyB.invMass + this.invIA = this.bodyA.invI + this.invIB = this.bodyB.invI + + val aA = data.positions[this.indexA].a + var vA = data.velocities[this.indexA].v + var wA = data.velocities[this.indexA].w + + val aB = data.positions[this.indexB].a + var vB = data.velocities[this.indexB].v + var wB = data.velocities[this.indexB].w + + val qA = Rotation(aA) + val qB = Rotation(aB) + + this.rA = b2Mul(qA, this.localAnchorA - this.localCenterA) + this.rB = b2Mul(qB, this.localAnchorB - this.localCenterB) + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + val mA = this.invMassA + val mB = this.invMassB + val iA = this.invIA + val iB = this.invIB + + val K = MutableMatrix3d() + K.m00 = mA + mB + this.rA.y * this.rA.y * iA + this.rB.y * this.rB.y * iB + K.m01 = -this.rA.y * this.rA.x * iA - this.rB.y * this.rB.x * iB + K.m02 = -this.rA.y * iA - this.rB.y * iB + + K.m10 = K.m01 + K.m11 = mA + mB + this.rA.x * this.rA.x * iA + this.rB.x * this.rB.x * iB + K.m12 = this.rA.x * iA + this.rB.x * iB + + K.m20 = K.m02 + K.m21 = K.m12 + K.m22 = iA + iB + + if (this.stiffness > 0.0) { + this.mass = K.getInverse2().asMutableMatrix() + + var invM = iA + iB + + val C = aB - aA - this.referenceAngle + + // Damping coefficient + val d = this.damping + + // Spring stiffness + val k = this.stiffness + + // magic formulas + val h = data.step.dt + this.gamma = h * (d + h * k) + this.gamma = if (this.gamma != 0.0) 1.0 / this.gamma else 0.0 + this.bias = C * h * k * this.gamma + + invM += this.gamma + this.mass.m22 = if (invM != 0.0) 1.0 / invM else 0.0 + } else if (K.m22 == 0.0) { + this.mass = K.getInverse2().asMutableMatrix() + this.gamma = 0.0 + this.bias = 0.0 + } else { + this.mass = K.getInverse().asMutableMatrix() + this.gamma = 0.0 + this.bias = 0.0 + } + + if (data.step.warmStarting) { + // Scale impulses to support a variable time step. + this.impulse *= data.step.dtRatio + + val P = Vector2d(this.impulse.x, this.impulse.y) + + vA -= mA * P + wA -= iA * (b2Cross(this.rA, P) + this.impulse.z) + + vB += mB * P + wB += iB * (b2Cross(this.rB, P) + this.impulse.z) + } else { + this.impulse = Vector3d() + } + + data.velocities[this.indexA].v = vA + data.velocities[this.indexA].w = wA + data.velocities[this.indexB].v = vB + data.velocities[this.indexB].w = wB + } + + override fun solveVelocityConstraints(data: B2SolverData) { + var vA = data.velocities[this.indexA].v + var wA = data.velocities[this.indexA].w + var vB = data.velocities[this.indexB].v + var wB = data.velocities[this.indexB].w + + val mA = this.invMassA + val mB = this.invMassB + val iA = this.invIA + val iB = this.invIB + + if (this.stiffness > 0.0) { + val Cdot2 = wB - wA + + val impulse2 = -this.mass.m22 * (Cdot2 + this.bias + this.gamma * this.impulse.z) + this.impulse += Vector3d(z = impulse2) + + wA -= iA * impulse2 + wB += iB * impulse2 + + val Cdot1 = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA) + + val impulse1 = -b2Mul22(this.mass, Cdot1) + this.impulse += Vector3d(impulse1.x, impulse1.y) + + val P = impulse1 + + vA -= mA * P + wA -= iA * b2Cross(this.rA, P) + + vB += mB * P + wB += iB * b2Cross(this.rB, P) + } else { + val Cdot1 = vB + b2Cross(wB, this.rB) - vA - b2Cross(wA, this.rA) + val Cdot2 = wB - wA + val Cdot = Vector3d(Cdot1.x, Cdot1.y, Cdot2) + + val impulse = -b2Mul(this.mass, Cdot) + this.impulse += impulse + + val P = Vector2d(impulse.x, impulse.y) + + vA -= mA * P + wA -= iA * (b2Cross(this.rA, P) + impulse.z) + + vB += mB * P + wB += iB * (b2Cross(this.rB, P) + impulse.z) + } + + data.velocities[this.indexA].v = vA + data.velocities[this.indexA].w = wA + data.velocities[this.indexB].v = vB + data.velocities[this.indexB].w = wB + } + + override fun solvePositionConstraints(data: B2SolverData): Boolean { + var cA = data.positions[this.indexA].c + var aA = data.positions[this.indexA].a + var cB = data.positions[this.indexB].c + var aB = data.positions[this.indexB].a + + val qA = Rotation(aA) + val qB = Rotation(aB) + + val mA = this.invMassA + val mB = this.invMassB + val iA = this.invIA + val iB = this.invIB + + val rA = b2Mul(qA, this.localAnchorA - this.localCenterA) + val rB = b2Mul(qB, this.localAnchorB - this.localCenterB) + + val positionError: Double + val angularError: Double + + val K = MutableMatrix3d() + K.m00 = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB + K.m01 = -rA.y * rA.x * iA - rB.y * rB.x * iB + K.m02 = -rA.y * iA - rB.y * iB + K.m10 = K.m01 + K.m11 = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB + K.m12 = rA.x * iA + rB.x * iB + K.m20 = K.m02 + K.m21 = K.m12 + K.m22 = iA + iB + + if (this.stiffness > 0.0f) { + val C1 = cB + rB - cA - rA + + positionError = C1.length + angularError = 0.0 + + val P = -K.solve(C1) + + cA -= mA * P + aA -= iA * b2Cross(rA, P) + + cB += mB * P + aB += iB * b2Cross(rB, P) + } else { + val C1 = cB + rB - cA - rA + val C2 = aB - aA - this.referenceAngle + + positionError = C1.length + angularError = b2Abs(C2) + + val C = Vector3d(C1.x, C1.y, C2) + + val impulse: Vector3d + if (K.m22 > 0.0) { + impulse = -K.solve(C) + } else { + val impulse2 = -K.solve(C1) + impulse = Vector3d(impulse2.x, impulse2.y) + } + + val P = Vector2d(impulse.x, impulse.y) + + cA -= mA * P + aA -= iA * (b2Cross(rA, P) + impulse.z) + + cB += mB * P + aB += iB * (b2Cross(rB, P) + impulse.z) + } + + data.positions[this.indexA].c = cA + data.positions[this.indexA].a = aA + data.positions[this.indexB].c = cB + data.positions[this.indexB].a = aB + + return positionError <= b2_linearSlop && angularError <= b2_angularSlop + } + + override val anchorA: Vector2d + get() = bodyA.getWorldPoint(localAnchorA) + override val anchorB: Vector2d + get() = bodyB.getWorldPoint(localAnchorB) + + override fun getReactionForce(inv_dt: Double): Vector2d { + return Vector2d(impulse.x * inv_dt, impulse.y * inv_dt) + } + + override fun getReactionTorque(inv_dt: Double): Double { + return inv_dt * impulse.z + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WheelJoint.kt b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WheelJoint.kt new file mode 100644 index 00000000..93096f16 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kbox2d/dynamics/joint/WheelJoint.kt @@ -0,0 +1,534 @@ +package ru.dbotthepony.kbox2d.dynamics.joint + +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.api.B2SolverData +import ru.dbotthepony.kbox2d.api.b2Cross +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.math.times +import ru.dbotthepony.kstarbound.util.Color + +class WheelJoint(def: WheelJointDef) : AbstractJoint(def) { + val localAnchorA: Vector2d = def.localAnchorA + val localAnchorB: Vector2d = def.localAnchorB + val localXAxisA: Vector2d = def.localAxisA + val localYAxisA: Vector2d = b2Cross(1.0, localXAxisA) + + private var impulse: Double = 0.0 + private var motorImpulse: Double = 0.0 + private var springImpulse: Double = 0.0 + + private var lowerImpulse: Double = 0.0 + private var upperImpulse: Double = 0.0 + private var translation: Double = 0.0 + + // Solver temp + private var indexA: Int = 0 + private var indexB: Int = 0 + private var localCenterA: Vector2d = Vector2d.ZERO + private var localCenterB: Vector2d = Vector2d.ZERO + private var invMassA: Double = 0.0 + private var invMassB: Double = 0.0 + private var invIA: Double = 0.0 + private var invIB: Double = 0.0 + + private var ax: Vector2d = Vector2d.ZERO + private var ay: Vector2d = Vector2d.ZERO + + private var sAx: Double = 0.0 + private var sBx: Double = 0.0 + private var sAy: Double = 0.0 + private var sBy: Double = 0.0 + + private var mass: Double = 0.0 + private var motorMass: Double = 0.0 + private var axialMass: Double = 0.0 + private var springMass: Double = 0.0 + + private var bias: Double = 0.0 + private var gamma: Double = 0.0 + + override fun initVelocityConstraints(data: B2SolverData) { + this.indexA = this.bodyA.islandIndex + this.indexB = this.bodyB.islandIndex + this.localCenterA = this.bodyA.sweep.localCenter + this.localCenterB = this.bodyB.sweep.localCenter + this.invMassA = this.bodyA.invMass + this.invMassB = this.bodyB.invMass + this.invIA = this.bodyA.invI + this.invIB = this.bodyB.invI + + val mA = this.invMassA + val mB = this.invMassB + val iA = this.invIA + val iB = this.invIB + + val cA = data.positions[this.indexA].c + val aA = data.positions[this.indexA].a + var vA = data.velocities[this.indexA].v + var wA = data.velocities[this.indexA].w + + val cB = data.positions[this.indexB].c + val aB = data.positions[this.indexB].a + var vB = data.velocities[this.indexB].v + var wB = data.velocities[this.indexB].w + + val qA = Rotation(aA) + val qB = Rotation(aB) + + // Compute the effective masses. + val rA = b2Mul(qA, this.localAnchorA - this.localCenterA) + val rB = b2Mul(qB, this.localAnchorB - this.localCenterB) + val d = cB + rB - cA - rA + + // Point to line constraint + run { + this.ay = b2Mul(qA, this.localYAxisA) + this.sAy = b2Cross(d + rA, this.ay) + this.sBy = b2Cross(rB, this.ay) + + this.mass = mA + mB + iA * this.sAy * this.sAy + iB * this.sBy * this.sBy + + if (this.mass > 0.0) { + this.mass = 1.0 / this.mass + } + } + + // Spring constraint + this.ax = b2Mul(qA, this.localXAxisA) + this.sAx = b2Cross(d + rA, this.ax) + this.sBx = b2Cross(rB, this.ax) + + val invMass = mA + mB + iA * this.sAx * this.sAx + iB * this.sBx * this.sBx + + if (invMass > 0.0) { + this.axialMass = 1.0 / invMass + } else { + this.axialMass = 0.0 + } + + this.springMass = 0.0 + this.bias = 0.0 + this.gamma = 0.0 + + if (this.stiffness > 0.0f && invMass > 0.0f) { + this.springMass = 1.0f / invMass + + val C = b2Dot(d, this.ax) + + // magic formulas + val h = data.step.dt + this.gamma = h * (this.damping + h * this.stiffness) + if (this.gamma > 0.0f) { + this.gamma = 1.0f / this.gamma + } + + this.bias = C * h * this.stiffness * this.gamma + + this.springMass = invMass + this.gamma + if (this.springMass > 0.0) { + this.springMass = 1.0 / this.springMass + } + } else { + this.springImpulse = 0.0 + } + + if (this.enableLimit) { + this.translation = b2Dot(this.ax, d) + } else { + this.lowerImpulse = 0.0 + this.upperImpulse = 0.0 + } + + if (this.enableMotor) { + this.motorMass = iA + iB + + if (this.motorMass > 0.0f) { + this.motorMass = 1.0f / this.motorMass + } + } else { + this.motorMass = 0.0 + this.motorImpulse = 0.0 + } + + if (data.step.warmStarting) { + // Account for variable time step. + this.impulse *= data.step.dtRatio + this.springImpulse *= data.step.dtRatio + this.motorImpulse *= data.step.dtRatio + + val axialImpulse = this.springImpulse + this.lowerImpulse - this.upperImpulse + val P = this.impulse * this.ay + axialImpulse * this.ax + val LA = this.impulse * this.sAy + axialImpulse * this.sAx + this.motorImpulse + val LB = this.impulse * this.sBy + axialImpulse * this.sBx + this.motorImpulse + + vA -= this.invMassA * P + wA -= this.invIA * LA + + vB += this.invMassB * P + wB += this.invIB * LB + } else { + this.impulse = 0.0 + this.springImpulse = 0.0 + this.motorImpulse = 0.0 + this.lowerImpulse = 0.0 + this.upperImpulse = 0.0 + } + + data.velocities[this.indexA].v = vA + data.velocities[this.indexA].w = wA + data.velocities[this.indexB].v = vB + data.velocities[this.indexB].w = wB + } + + override fun solveVelocityConstraints(data: B2SolverData) { + val mA = this.invMassA + val mB = this.invMassB + val iA = this.invIA + val iB = this.invIB + + var vA = data.velocities[this.indexA].v + var wA = data.velocities[this.indexA].w + var vB = data.velocities[this.indexB].v + var wB = data.velocities[this.indexB].w + + // Solve spring constraint + run { + val Cdot = b2Dot(this.ax, vB - vA) + this.sBx * wB - this.sAx * wA + val impulse = -this.springMass * (Cdot + this.bias + this.gamma * this.springImpulse) + this.springImpulse += impulse + + val P = impulse * this.ax + val LA = impulse * this.sAx + val LB = impulse * this.sBx + + vA -= mA * P + wA -= iA * LA + + vB += mB * P + wB += iB * LB + } + + // Solve rotational motor constraint + run { + val Cdot = wB - wA - this.motorSpeed + var impulse = -this.motorMass * Cdot + + val oldImpulse = this.motorImpulse + val maxImpulse = data.step.dt * this.maxMotorTorque + this.motorImpulse = b2Clamp(this.motorImpulse + impulse, -maxImpulse, maxImpulse) + impulse = this.motorImpulse - oldImpulse + + wA -= iA * impulse + wB += iB * impulse + } + + if (this.enableLimit) { + // Lower limit + run { + val C = this.translation - this.lowerTranslation + val Cdot = b2Dot(this.ax, vB - vA) + this.sBx * wB - this.sAx * wA + var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt) + val oldImpulse = this.lowerImpulse + this.lowerImpulse = b2Max(this.lowerImpulse + impulse, 0.0) + impulse = this.lowerImpulse - oldImpulse + + val P = impulse * this.ax + val LA = impulse * this.sAx + val LB = impulse * this.sBx + + vA -= mA * P + wA -= iA * LA + vB += mB * P + wB += iB * LB + } + + // Upper limit + // Note: signs are flipped to keep C positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + run { + val C = this.upperTranslation - this.translation + val Cdot = b2Dot(this.ax, vA - vB) + this.sAx * wA - this.sBx * wB + var impulse = -this.axialMass * (Cdot + b2Max(C, 0.0) * data.step.inv_dt) + val oldImpulse = this.upperImpulse + this.upperImpulse = b2Max(this.upperImpulse + impulse, 0.0) + impulse = this.upperImpulse - oldImpulse + + val P = impulse * this.ax + val LA = impulse * this.sAx + val LB = impulse * this.sBx + + vA += mA * P + wA += iA * LA + vB -= mB * P + wB -= iB * LB + } + } + + // Solve point to line constraint + run { + val Cdot = b2Dot(this.ay, vB - vA) + this.sBy * wB - this.sAy * wA + val impulse = -this.mass * Cdot + this.impulse += impulse + + val P = impulse * this.ay + val LA = impulse * this.sAy + val LB = impulse * this.sBy + + vA -= mA * P + wA -= iA * LA + + vB += mB * P + wB += iB * LB + } + + data.velocities[this.indexA].v = vA + data.velocities[this.indexA].w = wA + data.velocities[this.indexB].v = vB + data.velocities[this.indexB].w = wB + } + + override fun solvePositionConstraints(data: B2SolverData): Boolean { + var cA = data.positions[this.indexA].c + var aA = data.positions[this.indexA].a + var cB = data.positions[this.indexB].c + var aB = data.positions[this.indexB].a + + var linearError = 0.0 + + if (this.enableLimit) { + val qA = Rotation(aA) + val qB = Rotation(aB) + + val rA = b2Mul(qA, this.localAnchorA - this.localCenterA) + val rB = b2Mul(qB, this.localAnchorB - this.localCenterB) + val d = (cB - cA) + rB - rA + + val ax = b2Mul(qA, this.localXAxisA) + val sAx = b2Cross(d + rA, this.ax) + val sBx = b2Cross(rB, this.ax) + + var C = 0.0 + val translation = b2Dot(ax, d) + if (b2Abs(this.upperTranslation - this.lowerTranslation) < 2.0 * b2_linearSlop) { + C = translation + } else if (translation <= this.lowerTranslation) { + C = b2Min(translation - this.lowerTranslation, 0.0) + } else if (translation >= this.upperTranslation) { + C = b2Max(translation - this.upperTranslation, 0.0) + } + + if (C != 0.0) { + val invMass = this.invMassA + this.invMassB + this.invIA * sAx * sAx + this.invIB * sBx * sBx + var impulse = 0.0 + if (invMass != 0.0) { + impulse = -C / invMass + } + + val P = impulse * ax + val LA = impulse * sAx + val LB = impulse * sBx + + cA -= this.invMassA * P + aA -= this.invIA * LA + cB += this.invMassB * P + aB += this.invIB * LB + + linearError = b2Abs(C) + } + } + + // Solve perpendicular constraint + run { + val qA = Rotation(aA) + val qB = Rotation(aB) + + val rA = b2Mul(qA, this.localAnchorA - this.localCenterA) + val rB = b2Mul(qB, this.localAnchorB - this.localCenterB) + val d = (cB - cA) + rB - rA + + val ay = b2Mul(qA, this.localYAxisA) + + val sAy = b2Cross(d + rA, ay) + val sBy = b2Cross(rB, ay) + + val C = b2Dot(d, ay) + + val invMass = this.invMassA + this.invMassB + this.invIA * this.sAy * this.sAy + this.invIB * this.sBy * this.sBy + + var impulse = 0.0 + if (invMass != 0.0) { + impulse = - C / invMass + } + + val P = impulse * ay + val LA = impulse * sAy + val LB = impulse * sBy + + cA -= this.invMassA * P + aA -= this.invIA * LA + cB += this.invMassB * P + aB += this.invIB * LB + + linearError = b2Max(linearError, b2Abs(C)) + } + + data.positions[this.indexA].c = cA + data.positions[this.indexA].a = aA + data.positions[this.indexB].c = cB + data.positions[this.indexB].a = aB + + return linearError <= b2_linearSlop + } + + override val anchorA: Vector2d + get() = bodyA.getWorldPoint(localAnchorA) + override val anchorB: Vector2d + get() = bodyB.getWorldPoint(localAnchorB) + + override fun getReactionForce(inv_dt: Double): Vector2d { + return inv_dt * (impulse * ay + (springImpulse + lowerImpulse - upperImpulse) * ax) + } + + override fun getReactionTorque(inv_dt: Double): Double { + return inv_dt * motorImpulse + } + + val jointTranslation: Double get() { + val bA = bodyA + val bB = bodyB + + val pA = bA.getWorldPoint(localAnchorA) + val pB = bB.getWorldPoint(localAnchorB) + val d = pB - pA + val axis = bA.getWorldVector(localXAxisA) + + return b2Dot(d, axis) + } + + val jointLinearSpeed: Double get() { + val bA = bodyA + val bB = bodyB + + val rA = b2Mul(bA.xf.q, localAnchorA - bA.sweep.localCenter) + val rB = b2Mul(bB.xf.q, localAnchorB - bB.sweep.localCenter) + val p1 = bA.sweep.c + rA + val p2 = bB.sweep.c + rB + val d = p2 - p1 + val axis = b2Mul(bA.xf.q, localXAxisA) + + val vA = bA.linearVelocity + val vB = bB.linearVelocity + val wA = bA.angularVelocity + val wB = bB.angularVelocity + + return b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)) + } + + val jointAngle: Double get() { + return bodyB.sweep.a - bodyA.sweep.a + } + + val jointAngularSpeed: Double get() { + return bodyB.angularVelocity - bodyA.angularVelocity + } + + var lowerTranslation: Double = def.lowerTranslation + private set + var upperTranslation: Double = def.upperTranslation + private set + + init { + require(lowerTranslation <= upperTranslation) { "$lowerTranslation !<= $upperTranslation" } + } + + fun setLimits(lower: Double, upper: Double) { + require(lower <= upper) { "$lower !<= $upper" } + + if (lower != lowerTranslation || upper != upperTranslation) { + bodyA.isAwake = true + bodyB.isAwake = true + lowerTranslation = lower + upperTranslation = upper + lowerImpulse = 0.0 + upperImpulse = 0.0 + } + } + + var maxMotorTorque: Double = 0.0 + set(value) { + if (field != value) { + bodyA.isAwake = true + bodyB.isAwake = true + field = value + } + } + + fun getMotorTorque(inv_dt: Double): Double { + return inv_dt * motorImpulse + } + + var motorSpeed: Double = 0.0 + set(value) { + if (field != value) { + bodyA.isAwake = true + bodyB.isAwake = true + field = value + } + } + + var enableLimit: Boolean = def.enableLimit + set(value) { + if (field != value) { + bodyA.isAwake = true + bodyB.isAwake = true + field = value + lowerImpulse = 0.0 + upperImpulse = 0.0 + } + } + + var enableMotor: Boolean = def.enableMotor + set(value) { + if (field != value) { + bodyA.isAwake = true + bodyB.isAwake = true + field = value + } + } + + var stiffness: Double = def.stiffness + var damping: Double = def.damping + + override fun draw(draw: IDebugDraw) { + val xfA = bodyA.transform + val xfB = bodyB.transform + val pA = b2Mul(xfA, localAnchorA) + val pB = b2Mul(xfB, localAnchorB) + + val axis = b2Mul(xfA.q, localXAxisA) + + draw.drawSegment(pA, pB, c5) + + if (enableLimit) { + val lower = pA + lowerTranslation * axis + val upper = pA + upperTranslation * axis + val perp = b2Mul(xfA.q, localYAxisA) + draw.drawSegment(lower, upper, c1) + draw.drawSegment(lower - 0.5 * perp, lower + 0.5 * perp, c2) + draw.drawSegment(upper - 0.5 * perp, upper + 0.5 * perp, c3) + } else { + draw.drawSegment(pA - 1.0 * axis, pA + 1.0 * axis, c1) + } + + draw.drawPoint(pA, 5.0, c1) + draw.drawPoint(pB, 5.0, c4) + } + + companion object { + private val c1 = Color(0.7f, 0.7f, 0.7f) + private val c2 = Color(0.3f, 0.9f, 0.3f) + private val c3 = Color(0.9f, 0.3f, 0.3f) + private val c4 = Color(0.3f, 0.3f, 0.9f) + private val c5 = Color(0.4f, 0.4f, 0.4f) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/Main.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/Main.kt index 9bb4b0dd..0fbd1ccc 100644 --- a/src/main/kotlin/ru/dbotthepony/kstarbound/Main.kt +++ b/src/main/kotlin/ru/dbotthepony/kstarbound/Main.kt @@ -3,16 +3,20 @@ package ru.dbotthepony.kstarbound import org.apache.logging.log4j.LogManager import org.lwjgl.Version import org.lwjgl.glfw.GLFW.glfwSetWindowShouldClose +import ru.dbotthepony.kbox2d.api.* +import ru.dbotthepony.kbox2d.collision.shapes.ChainShape +import ru.dbotthepony.kbox2d.collision.shapes.CircleShape +import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape +import ru.dbotthepony.kbox2d.dynamics.B2World +import ru.dbotthepony.kbox2d.dynamics.joint.MouseJoint import ru.dbotthepony.kstarbound.client.StarboundClient -import ru.dbotthepony.kstarbound.defs.TileDefinition import ru.dbotthepony.kstarbound.math.Vector2d -import ru.dbotthepony.kstarbound.world.CHUNK_SIZE_FF import ru.dbotthepony.kstarbound.world.Chunk -import ru.dbotthepony.kstarbound.world.ChunkPos import ru.dbotthepony.kstarbound.world.entities.Move import ru.dbotthepony.kstarbound.world.entities.PlayerEntity -import ru.dbotthepony.kstarbound.world.entities.Projectile import java.io.File +import java.util.* +import kotlin.collections.ArrayList private val LOGGER = LogManager.getLogger() @@ -27,14 +31,380 @@ fun main() { //return } + val world = B2World(Vector2d(y = -10.0)) + + val groundDef = BodyDef( + position = Vector2d(y = 0.0) + ) + + val ground = world.createBody(groundDef) + + //val groundPoly = PolygonShape() + //groundPoly.setAsBox(50.0, 10.0) + + val groundPoly = ChainShape() + groundPoly.createLoop(listOf( + Vector2d(-30.0, 10.0), + Vector2d(-25.0, 0.0), + Vector2d(25.0, 0.0), + Vector2d(30.0, 10.0), + Vector2d(30.0, -2.0), + Vector2d(-30.0, -2.0)).asReversed()) + + ground.createFixture(groundPoly, 0.0) + + val boxes = ArrayList() + + /*run { + val movingDef = BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(y = 4.0), + angle = PI / 4.0 + ) + val movingBody = world.createBody(movingDef) + + val dynamicBox = PolygonShape() + dynamicBox.setAsBox(1.0, 1.0) + + val fixtureDef = FixtureDef( + shape = dynamicBox, + density = 1.0, + friction = 0.3 + ) + + movingBody.createFixture(fixtureDef) + boxes.add(movingBody) + }*/ + + run { + if (true) + return@run + val movingDef = BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = -1.0, y = 6.0), + ) + + val movingBody = world.createBody(movingDef) + + val dynamicBox = PolygonShape() + dynamicBox.setAsBox(1.0, 1.0) + + val fixtureDef = FixtureDef( + shape = dynamicBox, + density = 1.0, + friction = 0.3 + ) + + movingBody.createFixture(fixtureDef) + boxes.add(movingBody) + } + + val rand = Random() + + val mouseJoints = ArrayList() + + run { + val stripes = 0 + + for (stripe in 0 until stripes) { + for (x in 0 .. (stripes - stripe)) { + val movingBody = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = (-stripes + stripe) * 1.0 + x * 2.1, y = 8.0 + stripe * 2.1), + gravityScale = 1.1 + )) + + val dynamicBox: IShape<*> + + if (false) { + dynamicBox = PolygonShape() + dynamicBox.setAsBox(1.0, 1.0) + } else { + dynamicBox = CircleShape(1.0) + } + + movingBody.createFixture(FixtureDef( + shape = dynamicBox, + density = 1.0, + friction = 0.3 + )) + + /*for (otherBody in boxes) { + val def = DistanceJointDef(movingBody, otherBody, movingBody.position, otherBody.position) + world.createJoint(def) + }*/ + + /*val mouse = world.createJoint(MouseJointDef( + Vector2d(y = 10.0, x = 10.0), + bodyB = movingBody, + maxForce = 1000.0 + ).linearStiffness(50.0, 0.7, bodyB = movingBody, bodyA = null)) + + mouseJoints.add(mouse as MouseJoint)*/ + + boxes.add(movingBody) + } + } + } + + run { + if (true) + return@run + val movingDef1 = BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = -4.0, y = 4.0), + gravityScale = 1.1 + ) + + val movingBody1 = world.createBody(movingDef1) + + val dynamicBox = PolygonShape() + dynamicBox.setAsBox(1.0, 1.0) + + val fixtureDef1 = FixtureDef( + shape = dynamicBox, + density = 1.0, + friction = 0.3 + ) + + movingBody1.createFixture(fixtureDef1) + + boxes.add(movingBody1) + + val movingDef2 = BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = 4.0, y = 6.0), + gravityScale = 1.1 + ) + + val movingBody2 = world.createBody(movingDef2) + + val fixtureDef2 = FixtureDef( + shape = dynamicBox, + density = 1.0, + friction = 0.3 + ) + + movingBody2.createFixture(fixtureDef2) + boxes.add(movingBody2) + + val groundAnchor1 = movingBody1.position + Vector2d(y = 10.0) + val groundAnchor2 = movingBody2.position + Vector2d(y = 12.0) + val def = PulleyJointDef(movingBody1, movingBody2, groundAnchor1, groundAnchor2, movingBody1.position, movingBody2.position, 1.0) + + world.createJoint(def) + } + + run { + val base = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = 0.0, y = 16.0), + )) + + val wheel1 = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = -2.0, y = 15.0), + )) + + val wheel2 = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = 2.0, y = 15.0), + )) + + base.createFixture(FixtureDef( + shape = PolygonShape().also { it.setAsBox(2.0, 0.5) }, + density = 1.0 + )) + + wheel1.createFixture(FixtureDef( + shape = CircleShape(0.5), + density = 1.0, + friction = 1.5 + )) + + wheel2.createFixture(FixtureDef( + shape = CircleShape(0.5), + density = 1.0, + friction = 1.5 + )) + + world.createJoint(WheelJointDef( + bodyA = base, + bodyB = wheel1, + anchor = Vector2d(x = -2.0, y = 15.0), + axis = Vector2d.UP, + enableLimit = true, + upperTranslation = 0.25, + lowerTranslation = -0.25, + )) + + world.createJoint(WheelJointDef( + bodyA = base, + bodyB = wheel2, + anchor = Vector2d(x = 2.0, y = 15.0), + axis = Vector2d.UP, + enableLimit = true, + upperTranslation = 0.25, + lowerTranslation = -0.25, + )) + + base.setTransform(base.position, 0.56) + } + + run { + val circleNail = world.createBody(BodyDef( + type = BodyType.STATIC, + position = Vector2d(x = -12.0, y = 4.0), + )) + + val circleBody = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = -12.0, y = 4.0), + )) + + circleBody.createFixture(FixtureDef( + shape = CircleShape(2.0), + density = 1.0, + friction = 0.3 + )) + + val circleNailJoint = world.createJoint(RevoluteJointDef( + circleNail, + circleBody, + Vector2d(x = -12.0, y = 4.0) + )) + + val boxNail = world.createBody(BodyDef( + type = BodyType.STATIC, + position = Vector2d(x = 2.0, y = 4.0), + )) + + val boxBody = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = 2.0, y = 4.0), + )) + + boxBody.createFixture(FixtureDef( + shape = PolygonShape().also { it.setAsBox(3.5, 0.5) }, + density = 1.0, + friction = 0.3 + )) + + val boxNailJoint = world.createJoint(RevoluteJointDef( + boxNail, + boxBody, + Vector2d(x = 2.0, y = 4.0) + )) + + val gearJoint = world.createJoint(GearJointDef( + circleBody, + boxBody, + circleNailJoint, + boxNailJoint, + 1.0 + )) + } + + run { + val boxA = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = -2.0, y = 6.0), + )) + + val boxB = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = 0.0, y = 6.0), + )) + + val boxC = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = 2.0, y = 6.0), + )) + + val dynamicBox = PolygonShape() + dynamicBox.setAsBox(1.0, 1.0) + + val fixtureDef = FixtureDef( + shape = dynamicBox, + density = 1.0, + friction = 0.3 + ) + + boxA.createFixture(fixtureDef) + boxB.createFixture(fixtureDef) + boxC.createFixture(fixtureDef) + + world.createJoint(WeldJointDef( + bodyA = boxA, bodyB = boxB, (boxA.position + boxB.position) * 0.5 + ).linearStiffness(5.0, 0.5, boxA, boxB)) + + world.createJoint(WeldJointDef( + bodyA = boxB, bodyB = boxC, (boxB.position + boxC.position) * 0.5 + ).linearStiffness(5.0, 0.5, boxB, boxC)) + } + + run { + val boxA = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = -2.0, y = 8.0), + )) + + val dynamicBox = PolygonShape() + dynamicBox.setAsBox(1.0, 1.0) + + val fixtureDef = FixtureDef( + shape = dynamicBox, + density = 1.0, + friction = 0.3 + ) + + boxA.createFixture(fixtureDef) + + world.createJoint(FrictionJointDef( + bodyA = boxA, + bodyB = ground, + Vector2d(x = -2.0, y = 8.0), + maxForce = 1.0, + maxTorque = 10.0, + collideConnected = true + )) + } + + run { + val boxA = world.createBody(BodyDef( + type = BodyType.DYNAMIC, + position = Vector2d(x = -2.0, y = 8.0), + )) + + val dynamicBox = PolygonShape() + dynamicBox.setAsBox(1.0, 1.0) + + val fixtureDef = FixtureDef( + shape = dynamicBox, + density = 1.0, + friction = 0.3 + ) + + boxA.createFixture(fixtureDef) + + world.createJoint(MotorJointDef( + bodyA = boxA, + bodyB = ground, + maxForce = 1000.0, + collideConnected = true, + )) + } + + val timeStep = 1.0 / 144.0 + val client = StarboundClient() //Starbound.addFilePath(File("./unpacked_assets/")) Starbound.addPakPath(File("J:\\SteamLibrary\\steamapps\\common\\Starbound\\assets\\packed.pak")) - Starbound.initializeGame { finished, replaceStatus, status -> + /*Starbound.initializeGame { finished, replaceStatus, status -> client.putDebugLog(status, replaceStatus) - } + }*/ client.onTermination { Starbound.terminateLoading = true @@ -45,7 +415,7 @@ fun main() { val ent = PlayerEntity(client.world!!) Starbound.onInitialize { - chunkA = client.world!!.computeIfAbsent(ChunkPos(0, 0)).chunk + /*chunkA = client.world!!.computeIfAbsent(ChunkPos(0, 0)).chunk val chunkB = client.world!!.computeIfAbsent(ChunkPos(-1, 0)).chunk val chunkC = client.world!!.computeIfAbsent(ChunkPos(-2, 0)).chunk @@ -121,11 +491,9 @@ fun main() { val projEnt = Projectile(client.world!!, proj) projEnt.pos = Vector2d(i * 2.0, 10.0) projEnt.spawn() - } + }*/ } - //val rand = Random() - ent.pos += Vector2d(y = 36.0, x = -10.0) client.onDrawGUI { @@ -134,8 +502,71 @@ fun main() { } client.onPreDrawWorld { - client.camera.pos.x = ent.pos.x.toFloat() - client.camera.pos.y = ent.pos.y.toFloat() + //client.camera.pos.x = ent.pos.x.toFloat() + //client.camera.pos.y = ent.pos.y.toFloat() + } + + client.camera.pos.y = 10f + + world.debugDraw = client.gl.box2dRenderer + + client.gl.box2dRenderer.drawShapes = true + client.gl.box2dRenderer.drawPairs = false + client.gl.box2dRenderer.drawAABB = false + client.gl.box2dRenderer.drawJoints = false + + client.onPostDrawWorld { + world.debugDraw() + + client.gl.quadWireframe { + var pos: Vector2d + + /*for (box in boxes) { + pos = box.position + + it.quadRotated( + -1f, + -1f, + 1f, + 1f, + pos.x.toFloat(), + pos.y.toFloat(), + box.angle, + ) + }*/ + + /*for (box in boxes) { + val broad = world.contactManager.broadPhase + val f = box.fixtureList!! + val proxy = f.proxies[0].proxyId + val aabb = broad.getFatAABB(proxy) + + it.quad( + aabb, + ) + }*/ + + /*pos = ground.position + + it.quad( + (pos.x - 50f).toFloat(), + (pos.y - 10f).toFloat(), + (pos.x + 50f).toFloat(), + (pos.y + 10f).toFloat(), + )*/ + } + + for (joint in mouseJoints) { + joint.targetA = Vector2d(rand.nextDouble() * 20.0, rand.nextDouble() * 20.0) + + if (rand.nextDouble() < 0.01) { + world.destroyJoint(joint) + mouseJoints.remove(joint) + break + } + } + + world.step(timeStep, 6, 4) } ent.spawn() @@ -144,8 +575,8 @@ fun main() { Starbound.pollCallbacks() //ent.think(client.frameRenderTime) - client.camera.pos.x = ent.pos.x.toFloat() - client.camera.pos.y = ent.pos.y.toFloat() + //client.camera.pos.x = ent.pos.x.toFloat() + //client.camera.pos.y = ent.pos.y.toFloat() //println(client.camera.velocity.toDoubleVector() * client.frameRenderTime * 0.1) diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/client/gl/GLStateTracker.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/client/gl/GLStateTracker.kt index 693a1fa0..042c50a4 100644 --- a/src/main/kotlin/ru/dbotthepony/kstarbound/client/gl/GLStateTracker.kt +++ b/src/main/kotlin/ru/dbotthepony/kstarbound/client/gl/GLStateTracker.kt @@ -6,6 +6,7 @@ import org.lwjgl.opengl.GL46.* import ru.dbotthepony.kstarbound.Starbound import ru.dbotthepony.kstarbound.api.IStruct4f import ru.dbotthepony.kstarbound.client.freetype.FreeType +import ru.dbotthepony.kstarbound.client.render.Box2DRenderer import ru.dbotthepony.kstarbound.math.Matrix4f import ru.dbotthepony.kstarbound.math.Matrix4fStack import ru.dbotthepony.kstarbound.client.render.Font @@ -368,6 +369,26 @@ class GLStateTracker { fragment.unlink() } + val flat2DLines = object : GLStreamBuilderList { + override val small by lazy { + return@lazy StreamVertexBuilder(GLFlatAttributeList.VEC2F, VertexType.LINES, 1024) + } + + override val statefulSmall by lazy { + return@lazy StatefulStreamVertexBuilder(this@GLStateTracker, small) + } + } + + val flat2DTriangles = object : GLStreamBuilderList { + override val small by lazy { + return@lazy StreamVertexBuilder(GLFlatAttributeList.VEC2F, VertexType.TRIANGLES, 1024) + } + + override val statefulSmall by lazy { + return@lazy StatefulStreamVertexBuilder(this@GLStateTracker, small) + } + } + val flat2DQuads = object : GLStreamBuilderList { override val small by lazy { return@lazy StreamVertexBuilder(GLFlatAttributeList.VEC2F, VertexType.QUADS, 1024) @@ -437,6 +458,8 @@ class GLStateTracker { } } + val box2dRenderer = Box2DRenderer(this) + companion object { private val LOGGER = LogManager.getLogger(GLStateTracker::class.java) } diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/client/gl/VertexBuilder.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/client/gl/VertexBuilder.kt index 246d6f1d..f8258737 100644 --- a/src/main/kotlin/ru/dbotthepony/kstarbound/client/gl/VertexBuilder.kt +++ b/src/main/kotlin/ru/dbotthepony/kstarbound/client/gl/VertexBuilder.kt @@ -6,6 +6,8 @@ import java.io.Closeable import java.nio.ByteBuffer import java.nio.ByteOrder import kotlin.collections.ArrayList +import kotlin.math.cos +import kotlin.math.sin enum class VertexType(val elements: Int, val indicies: IntArray) { LINES(2, intArrayOf(0, 1)), @@ -41,6 +43,29 @@ interface IVertexBuilder, VertexType : I return this as This } + fun quadRotated( + x0: Float, + y0: Float, + x1: Float, + y1: Float, + x: Float, + y: Float, + angle: Double, + lambda: VertexTransformer = emptyTransform + ): This { + check(type.elements == 4) { "Currently building $type" } + + val s = sin(angle).toFloat() + val c = cos(angle).toFloat() + + lambda(vertex().pushVec2f(x + x0 * c - s * y0, y + s * x0 + c * y0), 0).end() + lambda(vertex().pushVec2f(x + x1 * c - s * y0, y + s * x1 + c * y0), 1).end() + lambda(vertex().pushVec2f(x + x0 * c - s * y1, y + s * x0 + c * y1), 2).end() + lambda(vertex().pushVec2f(x + x1 * c - s * y1, y + s * x1 + c * y1), 3).end() + + return this as This + } + fun quad(aabb: AABB, lambda: VertexTransformer = emptyTransform): This { return quad( aabb.mins.x.toFloat(), diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/Box2DRenderer.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/Box2DRenderer.kt new file mode 100644 index 00000000..f7e5b2cc --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/Box2DRenderer.kt @@ -0,0 +1,128 @@ +package ru.dbotthepony.kstarbound.client.render + +import org.lwjgl.opengl.GL46.* +import ru.dbotthepony.kbox2d.api.IDebugDraw +import ru.dbotthepony.kbox2d.api.Transform +import ru.dbotthepony.kstarbound.PIXELS_IN_STARBOUND_UNIT +import ru.dbotthepony.kstarbound.client.gl.GLStateTracker +import ru.dbotthepony.kstarbound.math.Vector2d +import ru.dbotthepony.kstarbound.util.Color +import kotlin.math.cos +import kotlin.math.sin + +class Box2DRenderer(val state: GLStateTracker) : IDebugDraw { + override var drawShapes: Boolean = false + override var drawJoints: Boolean = false + override var drawAABB: Boolean = false + override var drawPairs: Boolean = false + override var drawCenterOfMess: Boolean = false + + override fun drawPolygon(vertices: List, color: Color) { + require(vertices.size > 1) { "Vertex list had only ${vertices.size} namings in it" } + + val stateful = state.flat2DLines.statefulSmall + val builder = stateful.builder + + builder.begin() + + for (i in vertices.indices) { + val current = vertices[i] + val next = vertices[(i + 1) % vertices.size] + builder.Vertex().pushVec2f(current.x.toFloat(), current.y.toFloat()) + builder.Vertex().pushVec2f(next.x.toFloat(), next.y.toFloat()) + } + + stateful.upload() + + state.flatProgram.use() + state.flatProgram.color.set(color) + state.flatProgram.transform.set(state.matrixStack.last) + + stateful.draw(GL_LINES) + } + + private fun drawSolid(vertices: List, color: Color) { + require(vertices.size >= 3) { "Vertex list had only ${vertices.size} namings in it" } + + val stateful = state.flat2DTriangles.statefulSmall + val builder = stateful.builder + + builder.begin() + + val zero = vertices[0] + + for (i in 1 until vertices.size) { + val current = vertices[i] + val next = vertices[(i + 1) % vertices.size] + builder.Vertex().pushVec2f(zero.x.toFloat(), zero.y.toFloat()) + builder.Vertex().pushVec2f(current.x.toFloat(), current.y.toFloat()) + builder.Vertex().pushVec2f(next.x.toFloat(), next.y.toFloat()) + } + + stateful.upload() + + state.flatProgram.use() + state.flatProgram.color.set(color) + state.flatProgram.transform.set(state.matrixStack.last) + + stateful.draw(GL_TRIANGLES) + } + + override fun drawSolidPolygon(vertices: List, color: Color) { + drawSolid(vertices, color.copy(alpha = 0.5f)) + drawPolygon(vertices, color) + } + + override fun drawCircle(center: Vector2d, radius: Double, color: Color) { + val vertexList = ArrayList() + + for (i in 0 until 360 step 15) { + val rad = Math.toRadians(i.toDouble()) + val c = cos(rad) + val s = sin(rad) + + vertexList.add(Vector2d( + center.x + c * radius, + center.y + s * radius + )) + } + + drawPolygon(vertexList, color) + } + + override fun drawSolidCircle(center: Vector2d, radius: Double, axis: Vector2d, color: Color) { + val vertexList = ArrayList() + + for (i in 0 until 360 step 15) { + val rad = Math.toRadians(i.toDouble()) + val c = cos(rad) + val s = sin(rad) + + vertexList.add(Vector2d( + center.x + c * radius, + center.y + s * radius + )) + } + + drawSolidPolygon(vertexList, color.copy(alpha = 0.5f)) + drawPolygon(vertexList, color) + drawPolygon(listOf(center, center + axis * radius), color) + } + + override fun drawSegment(p1: Vector2d, p2: Vector2d, color: Color) { + drawPolygon(listOf(p1, p2), color) + } + + override fun drawTransform(xf: Transform) { + TODO("Not yet implemented") + } + + override fun drawPoint(p: Vector2d, size: Double, color: Color) { + drawSolid(listOf( + Vector2d(x = p.x - size / (PIXELS_IN_STARBOUND_UNIT * 2.0), y = p.y - size / (PIXELS_IN_STARBOUND_UNIT * 2.0)), + Vector2d(x = p.x + size / (PIXELS_IN_STARBOUND_UNIT * 2.0), y = p.y - size / (PIXELS_IN_STARBOUND_UNIT * 2.0)), + Vector2d(x = p.x + size / (PIXELS_IN_STARBOUND_UNIT * 2.0), y = p.y + size / (PIXELS_IN_STARBOUND_UNIT * 2.0)), + Vector2d(x = p.x - size / (PIXELS_IN_STARBOUND_UNIT * 2.0), y = p.y + size / (PIXELS_IN_STARBOUND_UNIT * 2.0)), + ), color) + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/EntityRenderer.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/EntityRenderer.kt index 4421f8fe..3e578b96 100644 --- a/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/EntityRenderer.kt +++ b/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/EntityRenderer.kt @@ -67,10 +67,10 @@ open class ProjectileRenderer(state: GLStateTracker, entity: Projectile, chunk: builder.begin() - val (u0, v0) = texture.pixelToUV(def.image.frames[animator.frame].texturePosition) - val (u1, v1) = texture.pixelToUV(def.image.frames[animator.frame].textureEndPosition) + val (u0, v0) = texture.pixelToUV(animator.frameObj.texturePosition) + val (u1, v1) = texture.pixelToUV(animator.frameObj.textureEndPosition) - builder.quadZ(0f, 0f, 1f, def.image.frames[animator.frame].aspectRatioHW, 5f, VertexTransformers.uv(u0, v0, u1, v1)) + builder.quadZ(0f, 0f, 1f, animator.frameObj.aspectRatioHW, 5f, VertexTransformers.uv(u0, v0, u1, v1)) stateful.upload() stateful.draw() diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/FrameSetAnimator.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/FrameSetAnimator.kt index 5f0c448e..1b3b5fea 100644 --- a/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/FrameSetAnimator.kt +++ b/src/main/kotlin/ru/dbotthepony/kstarbound/client/render/FrameSetAnimator.kt @@ -32,6 +32,8 @@ class FrameSetAnimator( var frame = 0 private set + val frameObj get() = set.frames[frame + firstFrame] + /** * Возвращает разницу между последним и первым кадром анимации */ diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/math/AABB.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/math/AABB.kt index 6888fbe7..6d398fe9 100644 --- a/src/main/kotlin/ru/dbotthepony/kstarbound/math/AABB.kt +++ b/src/main/kotlin/ru/dbotthepony/kstarbound/math/AABB.kt @@ -57,7 +57,7 @@ data class AABB(val mins: Vector2d, val maxs: Vector2d) { val xSpan get() = maxs.x - mins.x val ySpan get() = maxs.y - mins.y - val centre get() = mins + maxs * 0.5 + val centre get() = (mins + maxs) * 0.5 val A get() = mins val B get() = Vector2d(mins.x, maxs.y) @@ -69,12 +69,16 @@ data class AABB(val mins: Vector2d, val maxs: Vector2d) { val topRight get() = C val bottomRight get() = D - val width get() = (maxs.x - mins.x) / 2.0 - val height get() = (maxs.y - mins.y) / 2.0 + val width get() = maxs.x - mins.x + val height get() = maxs.y - mins.y + + val extents get() = Vector2d(width * 0.5, height * 0.5) val diameter get() = mins.distance(maxs) val radius get() = diameter / 2.0 + val perimeter get() = (xSpan + ySpan) * 2.0 + fun isInside(point: Vector2d): Boolean { return point.x in mins.x .. maxs.x && point.y in mins.y .. maxs.y } @@ -105,6 +109,19 @@ data class AABB(val mins: Vector2d, val maxs: Vector2d) { return intersectY } + /** + * @return Находится ли [other] внутри этого AABB + */ + fun contains(other: AABB): Boolean { + if (xSpan < other.xSpan || ySpan < other.ySpan) + return false + + return other.mins.x in mins.x .. maxs.x && + other.maxs.x in mins.x .. maxs.x && + other.mins.y in mins.y .. maxs.y && + other.maxs.y in mins.y .. maxs.y + } + /** * Есть ли пересечение между этим AABB и [other] * @@ -436,6 +453,8 @@ data class AABBi(val mins: Vector2i, val maxs: Vector2i) { val diameter get() = mins.distance(maxs) val radius get() = diameter / 2.0 + val perimeter get() = (xSpan + ySpan) * 2 + fun isInside(point: Vector2i): Boolean { return point.x in mins.x .. maxs.x && point.y in mins.y .. maxs.y } diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/math/FastMath.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/math/FastMath.kt new file mode 100644 index 00000000..daff5bd7 --- /dev/null +++ b/src/main/kotlin/ru/dbotthepony/kstarbound/math/FastMath.kt @@ -0,0 +1,24 @@ +package ru.dbotthepony.kstarbound.math + +/** + * Выполняет скалярное умножение между a и векторным произведением b, c на стеке + */ +fun scalarDotWithCross( + ax: Double, + ay: Double, + az: Double, + + bx: Double, + by: Double, + bz: Double, + + cx: Double, + cy: Double, + cz: Double, +): Double { + val crossX = by * cz - bz * cy + val crossY = bz * cx - bx * cz + val crossZ = bx * cy - by * cx + + return ax * crossX + ay * crossY + az * crossZ +} diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/math/Matrix.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/math/Matrix.kt index 47dd09ed..1dc63546 100644 --- a/src/main/kotlin/ru/dbotthepony/kstarbound/math/Matrix.kt +++ b/src/main/kotlin/ru/dbotthepony/kstarbound/math/Matrix.kt @@ -1,11 +1,17 @@ package ru.dbotthepony.kstarbound.math +import ru.dbotthepony.kbox2d.api.b2Cross +import ru.dbotthepony.kbox2d.api.b2Dot +import ru.dbotthepony.kstarbound.api.IStruct3d import ru.dbotthepony.kstarbound.api.IStruct3f +import ru.dbotthepony.kstarbound.api.IStruct4d import ru.dbotthepony.kstarbound.api.IStruct4f import java.nio.ByteBuffer import java.nio.ByteOrder +import java.nio.DoubleBuffer import java.nio.FloatBuffer import kotlin.math.PI +import kotlin.math.absoluteValue import kotlin.math.tan interface IMatrixLike { @@ -86,6 +92,64 @@ interface FloatMatrix> : IMatrix, IMatrixLikeFloat { } } +interface DoubleMatrix> : IMatrix, IMatrixLikeDouble { + operator fun plus(other: Double): T + operator fun minus(other: Double): T + operator fun times(other: Double): T + operator fun div(other: Double): T + + override operator fun plus(other: IMatrix): T + override operator fun minus(other: IMatrix): T + override operator fun times(other: IMatrix): T + + /** + * Если матрица больше или меньше, считать что всевозможные остальные координаты равны единице (не менять) + */ + fun scale(x: Double, y: Double = 1.0, z: Double = 1.0, w: Double = 1.0): T + + fun scale(value: IStruct4d): T { + val (x, y, z, w) = value + return scale(x, y, z, w) + } + + fun translate(x: Double = 0.0, y: Double = 0.0, z: Double = 0.0): T + + fun translate(value: IStruct3d): T { + val (x, y, z) = value + return translate(x, y, z) + } + + fun translateWithScale(x: Double = 0.0, y: Double = 0.0, z: Double = 0.0): T + + fun translateWithScale(value: IStruct3d): T { + val (x, y, z) = value + return translateWithScale(x, y, z) + } + + /** + * Выдает массив в готовом для OpenGL виде (строка -> столбец) по умолчанию + */ + fun toDoubleArray(columnMajor: Boolean = true): DoubleArray { + val buff = DoubleArray(rows * columns) + + if (columnMajor) { + for (row in 0 until rows) { + for (column in 0 until columns) { + buff[row + rows * column] = this[row, column] + } + } + } else { + for (row in 0 until rows) { + for (column in 0 until columns) { + buff[row * columns + column] = this[row, column] + } + } + } + + return buff + } +} + interface MutableFloatMatrix> : FloatMatrix { operator fun set(row: Int, column: Int, value: Float) } @@ -210,7 +274,7 @@ abstract class AbstractMatrix4f> : FloatMatrix { ): T override fun plus(other: IMatrix): T { - if (other !is FloatMatrix<*>) { + if (other !is IMatrixLikeFloat) { throw IllegalArgumentException("Can not use $other for addition") } @@ -223,7 +287,7 @@ abstract class AbstractMatrix4f> : FloatMatrix { val m20: Float; val m21: Float; val m22: Float; val m23: Float; val m30: Float; val m31: Float; val m32: Float; val m33: Float; - if (other is Matrix4f) { + if (other is AbstractMatrix4f<*>) { m00 = other.m00; m01 = other.m01; m02 = other.m02; m03 = other.m03; m10 = other.m10; m11 = other.m11; m12 = other.m12; m13 = other.m13; m20 = other.m20; m21 = other.m21; m22 = other.m22; m23 = other.m23; @@ -244,7 +308,7 @@ abstract class AbstractMatrix4f> : FloatMatrix { } override fun minus(other: IMatrix): T { - if (other !is FloatMatrix<*>) { + if (other !is IMatrixLikeFloat) { throw IllegalArgumentException("Can not use $other for subtraction") } @@ -257,7 +321,7 @@ abstract class AbstractMatrix4f> : FloatMatrix { val m20: Float; val m21: Float; val m22: Float; val m23: Float; val m30: Float; val m31: Float; val m32: Float; val m33: Float; - if (other is Matrix4f) { + if (other is AbstractMatrix4f<*>) { m00 = other.m00; m01 = other.m01; m02 = other.m02; m03 = other.m03; m10 = other.m10; m11 = other.m11; m12 = other.m12; m13 = other.m13; m20 = other.m20; m21 = other.m21; m22 = other.m22; m23 = other.m23; @@ -337,7 +401,7 @@ abstract class AbstractMatrix4f> : FloatMatrix { } override fun times(other: IMatrix): T { - if (other !is FloatMatrix<*>) { + if (other !is IMatrixLikeFloat) { throw IllegalArgumentException("Can not use $other for multiplication") } @@ -350,7 +414,7 @@ abstract class AbstractMatrix4f> : FloatMatrix { val m20: Float; val m21: Float; val m22: Float; val m23: Float; val m30: Float; val m31: Float; val m32: Float; val m33: Float; - if (other is Matrix4f) { + if (other is AbstractMatrix4f<*>) { m00 = other.m00; m01 = other.m01; m02 = other.m02; m03 = other.m03; m10 = other.m10; m11 = other.m11; m12 = other.m12; m13 = other.m13; m20 = other.m20; m21 = other.m21; m22 = other.m22; m23 = other.m23; @@ -478,6 +542,13 @@ abstract class AbstractMatrix4f> : FloatMatrix { m30, m31, m32, m33, ) } + + val absoluteMatrix get() = Matrix4f( + m00 = m00.absoluteValue, m01 = m01.absoluteValue, m02 = m02.absoluteValue, m03 = m03.absoluteValue, + m10 = m10.absoluteValue, m11 = m11.absoluteValue, m12 = m12.absoluteValue, m13 = m13.absoluteValue, + m20 = m20.absoluteValue, m21 = m21.absoluteValue, m22 = m22.absoluteValue, m23 = m23.absoluteValue, + m30 = m30.absoluteValue, m31 = m31.absoluteValue, m32 = m32.absoluteValue, m33 = m33.absoluteValue, + ) } data class Matrix4f( @@ -635,6 +706,558 @@ data class MutableMatrix4f( } } +abstract class AbstractMatrix4d> : DoubleMatrix { + companion object { + private val directBuffer by lazy { ByteBuffer.allocateDirect(4 * 4 * 4).also { it.order(ByteOrder.nativeOrder()) }.asDoubleBuffer()!! } + } + + /** + * Наполняет синглтон и возвращает DirectDoubleBuffer который необходимо использовать сразу, + * нет никаких гарантий что он не будет изменён в дальнейшем + * + * Функция предназначена исключительно для загрузки данной матрицы в память видеокарты + * + * Данный буфер всегда будет одним и тем же, но его содержимое обновлено + */ + fun toDoubleBuffer(columnMajor: Boolean = true): DoubleBuffer { + val directBuffer = directBuffer + directBuffer.position(0) + + if (columnMajor) { + directBuffer.put(m00) + directBuffer.put(m10) + directBuffer.put(m20) + directBuffer.put(m30) + + directBuffer.put(m01) + directBuffer.put(m11) + directBuffer.put(m21) + directBuffer.put(m31) + + directBuffer.put(m02) + directBuffer.put(m12) + directBuffer.put(m22) + directBuffer.put(m32) + + directBuffer.put(m03) + directBuffer.put(m13) + directBuffer.put(m23) + directBuffer.put(m33) + } else { + directBuffer.put(m00) + directBuffer.put(m01) + directBuffer.put(m02) + directBuffer.put(m03) + + directBuffer.put(m10) + directBuffer.put(m11) + directBuffer.put(m12) + directBuffer.put(m13) + + directBuffer.put(m20) + directBuffer.put(m21) + directBuffer.put(m22) + directBuffer.put(m23) + + directBuffer.put(m30) + directBuffer.put(m31) + directBuffer.put(m32) + directBuffer.put(m33) + } + + directBuffer.position(0) + return directBuffer + } + + abstract val m00: Double; abstract val m01: Double; abstract val m02: Double; abstract val m03: Double + abstract val m10: Double; abstract val m11: Double; abstract val m12: Double; abstract val m13: Double + abstract val m20: Double; abstract val m21: Double; abstract val m22: Double; abstract val m23: Double + abstract val m30: Double; abstract val m31: Double; abstract val m32: Double; abstract val m33: Double + + override val columns: Int + get() = 4 + + override val rows: Int + get() = 4 + + override fun get(row: Int, column: Int): Double { + return when (column) { + 0 -> when (row) { + 0 -> m00 + 1 -> m10 + 2 -> m20 + 3 -> m30 + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 1 -> when (row) { + 0 -> m01 + 1 -> m11 + 2 -> m21 + 3 -> m31 + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 2 -> when (row) { + 0 -> m02 + 1 -> m12 + 2 -> m22 + 3 -> m32 + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 3 -> when (row) { + 0 -> m03 + 1 -> m13 + 2 -> m23 + 3 -> m33 + else -> throw IndexOutOfBoundsException("Row: $row") + } + + else -> throw IndexOutOfBoundsException("Column: $column") + } + } + + protected abstract fun createOrModify( + m00: Double, m01: Double, m02: Double, m03: Double, + m10: Double, m11: Double, m12: Double, m13: Double, + m20: Double, m21: Double, m22: Double, m23: Double, + m30: Double, m31: Double, m32: Double, m33: Double, + ): T + + override fun plus(other: IMatrix): T { + if (other !is IMatrixLikeDouble) { + throw IllegalArgumentException("Can not use $other for addition") + } + + if (other.columns != 4 || other.rows != 4) { + throw IllegalArgumentException("Concrete Matrix4d can only use 4x4 matrixes") + } + + val m00: Double; val m01: Double; val m02: Double; val m03: Double; + val m10: Double; val m11: Double; val m12: Double; val m13: Double; + val m20: Double; val m21: Double; val m22: Double; val m23: Double; + val m30: Double; val m31: Double; val m32: Double; val m33: Double; + + if (other is AbstractMatrix4d<*>) { + m00 = other.m00; m01 = other.m01; m02 = other.m02; m03 = other.m03; + m10 = other.m10; m11 = other.m11; m12 = other.m12; m13 = other.m13; + m20 = other.m20; m21 = other.m21; m22 = other.m22; m23 = other.m23; + m30 = other.m30; m31 = other.m31; m32 = other.m32; m33 = other.m33; + } else { + m00 = other[0, 0]; m01 = other[0, 1]; m02 = other[0, 2]; m03 = other[0, 3]; + m10 = other[1, 0]; m11 = other[1, 1]; m12 = other[1, 2]; m13 = other[1, 3]; + m20 = other[2, 0]; m21 = other[2, 1]; m22 = other[2, 2]; m23 = other[2, 3]; + m30 = other[3, 0]; m31 = other[3, 1]; m32 = other[3, 2]; m33 = other[3, 3]; + } + + return createOrModify( + this.m00 + m00, this.m01 + m01, this.m02 + m02, this.m03 + m03, + this.m10 + m10, this.m11 + m11, this.m12 + m12, this.m13 + m13, + this.m20 + m20, this.m21 + m21, this.m22 + m22, this.m23 + m23, + this.m30 + m30, this.m31 + m31, this.m32 + m32, this.m33 + m33, + ) + } + + override fun minus(other: IMatrix): T { + if (other !is IMatrixLikeDouble) { + throw IllegalArgumentException("Can not use $other for subtraction") + } + + if (other.columns != 4 || other.rows != 4) { + throw IllegalArgumentException("Concrete Matrix4d can only use 4x4 matrices") + } + + val m00: Double; val m01: Double; val m02: Double; val m03: Double; + val m10: Double; val m11: Double; val m12: Double; val m13: Double; + val m20: Double; val m21: Double; val m22: Double; val m23: Double; + val m30: Double; val m31: Double; val m32: Double; val m33: Double; + + if (other is AbstractMatrix4d<*>) { + m00 = other.m00; m01 = other.m01; m02 = other.m02; m03 = other.m03; + m10 = other.m10; m11 = other.m11; m12 = other.m12; m13 = other.m13; + m20 = other.m20; m21 = other.m21; m22 = other.m22; m23 = other.m23; + m30 = other.m30; m31 = other.m31; m32 = other.m32; m33 = other.m33; + } else { + m00 = other[0, 0]; m01 = other[0, 1]; m02 = other[0, 2]; m03 = other[0, 3]; + m10 = other[1, 0]; m11 = other[1, 1]; m12 = other[1, 2]; m13 = other[1, 3]; + m20 = other[2, 0]; m21 = other[2, 1]; m22 = other[2, 2]; m23 = other[2, 3]; + m30 = other[3, 0]; m31 = other[3, 1]; m32 = other[3, 2]; m33 = other[3, 3]; + } + + return createOrModify( + this.m00 - m00, this.m01 - m01, this.m02 - m02, this.m03 - m03, + this.m10 - m10, this.m11 - m11, this.m12 - m12, this.m13 - m13, + this.m20 - m20, this.m21 - m21, this.m22 - m22, this.m23 - m23, + this.m30 - m30, this.m31 - m31, this.m32 - m32, this.m33 - m33, + ) + } + + override fun plus(other: Double): T { + return createOrModify( + this.m00 + other, this.m01 + other, this.m02 + other, this.m03 + other, + this.m10 + other, this.m11 + other, this.m12 + other, this.m13 + other, + this.m20 + other, this.m21 + other, this.m22 + other, this.m23 + other, + this.m30 + other, this.m31 + other, this.m32 + other, this.m33 + other, + ) + } + + override fun minus(other: Double): T { + return createOrModify( + this.m00 - other, this.m01 - other, this.m02 - other, this.m03 - other, + this.m10 - other, this.m11 - other, this.m12 - other, this.m13 - other, + this.m20 - other, this.m21 - other, this.m22 - other, this.m23 - other, + this.m30 - other, this.m31 - other, this.m32 - other, this.m33 - other, + ) + } + + override fun times(other: Double): T { + return createOrModify( + this.m00 * other, this.m01 * other, this.m02 * other, this.m03 * other, + this.m10 * other, this.m11 * other, this.m12 * other, this.m13 * other, + this.m20 * other, this.m21 * other, this.m22 * other, this.m23 * other, + this.m30 * other, this.m31 * other, this.m32 * other, this.m33 * other, + ) + } + + override fun scale(x: Double, y: Double, z: Double, w: Double): T { + return createOrModify( + this.m00 * x, this.m01, this.m02, this.m03, + this.m10, this.m11 * y, this.m12, this.m13, + this.m20, this.m21, this.m22 * z, this.m23, + this.m30, this.m31, this.m32, this.m33 * w, + ) + } + + override fun translate(x: Double, y: Double, z: Double): T { + return createOrModify( + this.m00, this.m01, this.m02, this.m03 + x, + this.m10, this.m11, this.m12, this.m13 + y, + this.m20, this.m21, this.m22, this.m23 + z, + this.m30, this.m31, this.m32, this.m33, + ) + } + + open val translation: Vector3d + get() { + return Vector3d(m03, m13, m23) + } + + override fun div(other: Double): T { + return createOrModify( + this.m00 / other, this.m01 / other, this.m02 / other, this.m03 / other, + this.m10 / other, this.m11 / other, this.m12 / other, this.m13 / other, + this.m20 / other, this.m21 / other, this.m22 / other, this.m23 / other, + this.m30 / other, this.m31 / other, this.m32 / other, this.m33 / other, + ) + } + + override fun times(other: IMatrix): T { + if (other !is IMatrixLikeDouble) { + throw IllegalArgumentException("Can not use $other for multiplication") + } + + if (other.columns != 4 || other.rows != 4) { + throw IllegalArgumentException("Concrete Matrix4d can only use 4x4 matrices") + } + + val m00: Double; val m01: Double; val m02: Double; val m03: Double; + val m10: Double; val m11: Double; val m12: Double; val m13: Double; + val m20: Double; val m21: Double; val m22: Double; val m23: Double; + val m30: Double; val m31: Double; val m32: Double; val m33: Double; + + if (other is AbstractMatrix4d<*>) { + m00 = other.m00; m01 = other.m01; m02 = other.m02; m03 = other.m03; + m10 = other.m10; m11 = other.m11; m12 = other.m12; m13 = other.m13; + m20 = other.m20; m21 = other.m21; m22 = other.m22; m23 = other.m23; + m30 = other.m30; m31 = other.m31; m32 = other.m32; m33 = other.m33; + } else { + m00 = other[0, 0]; m01 = other[0, 1]; m02 = other[0, 2]; m03 = other[0, 3]; + m10 = other[1, 0]; m11 = other[1, 1]; m12 = other[1, 2]; m13 = other[1, 3]; + m20 = other[2, 0]; m21 = other[2, 1]; m22 = other[2, 2]; m23 = other[2, 3]; + m30 = other[3, 0]; m31 = other[3, 1]; m32 = other[3, 2]; m33 = other[3, 3]; + } + + // первый столбец + val newm00 = + this.m00 * m00 + + this.m01 * m10 + + this.m02 * m20 + + this.m03 * m30 + + val newm10 = + this.m10 * m00 + + this.m11 * m10 + + this.m12 * m20 + + this.m13 * m30 + + val newm20 = + this.m20 * m00 + + this.m21 * m10 + + this.m22 * m20 + + this.m23 * m30 + + val newm30 = + this.m30 * m00 + + this.m31 * m10 + + this.m32 * m20 + + this.m33 * m30 + + // второй столбец + val newm01 = + this.m00 * m01 + + this.m01 * m11 + + this.m02 * m21 + + this.m03 * m31 + + val newm11 = + this.m10 * m01 + + this.m11 * m11 + + this.m12 * m21 + + this.m13 * m31 + + val newm21 = + this.m20 * m01 + + this.m21 * m11 + + this.m22 * m21 + + this.m23 * m31 + + val newm31 = + this.m30 * m01 + + this.m31 * m11 + + this.m32 * m21 + + this.m33 * m31 + + // третий столбец + val newm02 = + this.m00 * m02 + + this.m01 * m12 + + this.m02 * m22 + + this.m03 * m32 + + val newm12 = + this.m10 * m02 + + this.m11 * m12 + + this.m12 * m22 + + this.m13 * m32 + + val newm22 = + this.m20 * m02 + + this.m21 * m12 + + this.m22 * m22 + + this.m23 * m32 + + val newm32 = + this.m30 * m02 + + this.m31 * m12 + + this.m32 * m22 + + this.m33 * m32 + + // четвёртый столбец + val newm03 = + this.m00 * m03 + + this.m01 * m13 + + this.m02 * m23 + + this.m03 * m33 + + val newm13 = + this.m10 * m03 + + this.m11 * m13 + + this.m12 * m23 + + this.m13 * m33 + + val newm23 = + this.m20 * m03 + + this.m21 * m13 + + this.m22 * m23 + + this.m23 * m33 + + val newm33 = + this.m30 * m03 + + this.m31 * m13 + + this.m32 * m23 + + this.m33 * m33 + + return createOrModify( + newm00, newm01, newm02, newm03, + newm10, newm11, newm12, newm13, + newm20, newm21, newm22, newm23, + newm30, newm31, newm32, newm33, + ) + } + + override fun translateWithScale(x: Double, y: Double, z: Double): T { + return createOrModify( + m00, m01, m02, m03 + x * m00 + y * m01 + z * m02, + m10, m11, m12, m13 + x * m10 + y * m11 + z * m12, + m20, m21, m22, m23 + x * m20 + y * m21 + z * m22, + m30, m31, m32, m33, + ) + } + + val absoluteMatrix get() = Matrix4d( + m00 = m00.absoluteValue, m01 = m01.absoluteValue, m02 = m02.absoluteValue, m03 = m03.absoluteValue, + m10 = m10.absoluteValue, m11 = m11.absoluteValue, m12 = m12.absoluteValue, m13 = m13.absoluteValue, + m20 = m20.absoluteValue, m21 = m21.absoluteValue, m22 = m22.absoluteValue, m23 = m23.absoluteValue, + m30 = m30.absoluteValue, m31 = m31.absoluteValue, m32 = m32.absoluteValue, m33 = m33.absoluteValue, + ) +} + +data class Matrix4d( + override val m00: Double = 1.0, override val m01: Double = 0.0, override val m02: Double = 0.0, override val m03: Double = 0.0, + override val m10: Double = 0.0, override val m11: Double = 1.0, override val m12: Double = 0.0, override val m13: Double = 0.0, + override val m20: Double = 0.0, override val m21: Double = 0.0, override val m22: Double = 1.0, override val m23: Double = 0.0, + override val m30: Double = 0.0, override val m31: Double = 0.0, override val m32: Double = 0.0, override val m33: Double = 1.0, +) : AbstractMatrix4d() { + override fun createOrModify( + m00: Double, m01: Double, m02: Double, m03: Double, + m10: Double, m11: Double, m12: Double, m13: Double, + m20: Double, m21: Double, m22: Double, m23: Double, + m30: Double, m31: Double, m32: Double, m33: Double, + ): Matrix4d { + return Matrix4d( + m00 = m00, m01 = m01, m02 = m02, m03 = m03, + m10 = m10, m11 = m11, m12 = m12, m13 = m13, + m20 = m20, m21 = m21, m22 = m22, m23 = m23, + m30 = m30, m31 = m31, m32 = m32, m33 = m33, + ) + } + + fun toMutableMatrix(): MutableMatrix4d { + return MutableMatrix4d( + m00 = m00, m01 = m01, m02 = m02, m03 = m03, + m10 = m10, m11 = m11, m12 = m12, m13 = m13, + m20 = m20, m21 = m21, m22 = m22, m23 = m23, + m30 = m30, m31 = m31, m32 = m32, m33 = m33, + ) + } + + companion object { + val IDENTITY = Matrix4d() + + val SCREEN_FLIP = IDENTITY.let { + return@let it * Vector3d.FORWARD.rotateAroundThis(-PI / 2) + } + + /** + * Возвращает матрицу ортографической проекции, с ИНВЕНТИРОВАННОЙ y координатой, и с добавлением 2f + * + * Т.е. любое значение компоненты вектора y будет иметь противоположный знак после перемножения на данную матрицу + * + * Смысл данного изменения знака в преобразовании экранных координат OpenGL к вменяемому виду. Многие примеры указывают Z как отрицательную компоненту, + * что так же "убирает" это недоумение, только вот у нас Z это глубина, а не высота + * + * y = 0 будет соответствовать верхнему левому углу окна + */ + fun ortho(left: Double, right: Double, bottom: Double, top: Double, zNear: Double, zFar: Double): Matrix4d { + return Matrix4d( + m00 = 2f / (right - left), + m11 = -2f / (top - bottom), + m22 = 2f / (zFar - zNear), + m03 = -(right + left) / (right - left), + m13 = -(top + bottom) / (top - bottom) + 2f, + m23 = -(zFar + zNear) / (zFar - zNear) + ) + } + + /** + * Возвращает матрицу ортографической проекции, без инвентирования + * + * y = 0 будет соответствовать нижнему левому углу окна + */ + fun orthoDirect(left: Double, right: Double, bottom: Double, top: Double, zNear: Double, zFar: Double): Matrix4d { + return Matrix4d( + m00 = 2f / (right - left), + m11 = 2f / (top - bottom), + m22 = 2f / (zFar - zNear), + m03 = -(right + left) / (right - left), + m13 = -(top + bottom) / (top - bottom), + m23 = -(zFar + zNear) / (zFar - zNear) + ) + } + + fun perspective(fov: Double, zFar: Double, zNear: Double): Matrix4d { + val scale = (1.0 / (tan(Math.toRadians(fov.toDouble()) / 2.0))).toDouble() + val r = zFar - zNear + + return Matrix4d( + m00 = scale, + m11 = scale, + m22 = -zFar / r, + m23 = -1.0, + m32 = -zFar * zNear / r, + ) + } + } +} + +data class MutableMatrix4d( + override var m00: Double = 1.0, override var m01: Double = 0.0, override var m02: Double = 0.0, override var m03: Double = 0.0, + override var m10: Double = 0.0, override var m11: Double = 1.0, override var m12: Double = 0.0, override var m13: Double = 0.0, + override var m20: Double = 0.0, override var m21: Double = 0.0, override var m22: Double = 1.0, override var m23: Double = 0.0, + override var m30: Double = 0.0, override var m31: Double = 0.0, override var m32: Double = 0.0, override var m33: Double = 1.0, +) : AbstractMatrix4d() { + override fun createOrModify( + m00: Double, m01: Double, m02: Double, m03: Double, + m10: Double, m11: Double, m12: Double, m13: Double, + m20: Double, m21: Double, m22: Double, m23: Double, + m30: Double, m31: Double, m32: Double, m33: Double, + ): MutableMatrix4d { + this.m00 = m00; this.m01 = m01; this.m02 = m02; this.m03 = m03 + this.m10 = m10; this.m11 = m11; this.m12 = m12; this.m13 = m13 + this.m20 = m20; this.m21 = m21; this.m22 = m22; this.m23 = m23 + this.m30 = m30; this.m31 = m31; this.m32 = m32; this.m33 = m33 + + return this + } + + override var translation: Vector3d + get() { + return Vector3d(m03, m13, m23) + } + set(value) { + translate(value) + } + + fun set(row: Int, column: Int, value: Double) { + when (column) { + 0 -> when (row) { + 0 -> m00 = value + 1 -> m10 = value + 2 -> m20 = value + 3 -> m30 = value + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 1 -> when (row) { + 0 -> m01 = value + 1 -> m11 = value + 2 -> m21 = value + 3 -> m31 = value + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 2 -> when (row) { + 0 -> m02 = value + 1 -> m12 = value + 2 -> m22 = value + 3 -> m32 = value + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 3 -> when (row) { + 0 -> m03 = value + 1 -> m13 = value + 2 -> m23 = value + 3 -> m33 = value + else -> throw IndexOutOfBoundsException("Row: $row") + } + + else -> throw IndexOutOfBoundsException("Column: $column") + } + } +} + abstract class AbstractMatrix3f> : FloatMatrix { companion object { private val directBuffer by lazy { ByteBuffer.allocateDirect(4 * 3 * 3).also { it.order(ByteOrder.nativeOrder()) }.asFloatBuffer()!! } @@ -686,6 +1309,10 @@ abstract class AbstractMatrix3f> : FloatMatrix { abstract val m10: Float; abstract val m11: Float; abstract val m12: Float abstract val m20: Float; abstract val m21: Float; abstract val m22: Float + open val a11 get() = m00; open val a12 get() = m01; open val a13 get() = m02 + open val a21 get() = m10; open val a22 get() = m11; open val a23 get() = m12 + open val a31 get() = m20; open val a32 get() = m21; open val a33 get() = m22 + override val columns: Int get() = 3 @@ -726,7 +1353,7 @@ abstract class AbstractMatrix3f> : FloatMatrix { ): T override fun plus(other: IMatrix): T { - if (other !is FloatMatrix<*>) { + if (other !is IMatrixLikeFloat) { throw IllegalArgumentException("Can not use $other for addition") } @@ -738,7 +1365,7 @@ abstract class AbstractMatrix3f> : FloatMatrix { val m10: Float; val m11: Float; val m12: Float; val m20: Float; val m21: Float; val m22: Float; - if (other is Matrix3f) { + if (other is AbstractMatrix3f<*>) { m00 = other.m00; m01 = other.m01; m02 = other.m02; m10 = other.m10; m11 = other.m11; m12 = other.m12; m20 = other.m20; m21 = other.m21; m22 = other.m22; @@ -756,7 +1383,7 @@ abstract class AbstractMatrix3f> : FloatMatrix { } override fun minus(other: IMatrix): T { - if (other !is FloatMatrix<*>) { + if (other !is IMatrixLikeFloat) { throw IllegalArgumentException("Can not use $other for subtraction") } @@ -768,7 +1395,7 @@ abstract class AbstractMatrix3f> : FloatMatrix { val m10: Float; val m11: Float; val m12: Float; val m20: Float; val m21: Float; val m22: Float; - if (other is Matrix3f) { + if (other is AbstractMatrix3f<*>) { m00 = other.m00; m01 = other.m01; m02 = other.m02; m10 = other.m10; m11 = other.m11; m12 = other.m12; m20 = other.m20; m21 = other.m21; m22 = other.m22; @@ -834,7 +1461,7 @@ abstract class AbstractMatrix3f> : FloatMatrix { } override fun times(other: IMatrix): T { - if (other !is FloatMatrix<*>) { + if (other !is IMatrixLikeFloat) { throw IllegalArgumentException("Can not use $other for multiplication") } @@ -846,7 +1473,7 @@ abstract class AbstractMatrix3f> : FloatMatrix { val m10: Float; val m11: Float; val m12: Float; val m20: Float; val m21: Float; val m22: Float; - if (other is Matrix3f) { + if (other is AbstractMatrix3f<*>) { m00 = other.m00; m01 = other.m01; m02 = other.m02; m10 = other.m10; m11 = other.m11; m12 = other.m12; m20 = other.m20; m21 = other.m21; m22 = other.m22; @@ -918,6 +1545,14 @@ abstract class AbstractMatrix3f> : FloatMatrix { m20, m21, m22, ) } + + val determinant get() = m00 * m11 * m22 + m01 * m12 * m20 + m02 * m10 * m21 - m02 * m11 * m20 - m00 * m12 * m21 - m01 * m10 * m22 + + val absoluteMatrix get() = Matrix3f( + m00 = m00.absoluteValue, m01 = m01.absoluteValue, m02 = m02.absoluteValue, + m10 = m10.absoluteValue, m11 = m11.absoluteValue, m12 = m12.absoluteValue, + m20 = m20.absoluteValue, m21 = m21.absoluteValue, m22 = m22.absoluteValue, + ) } data class Matrix3f( @@ -959,6 +1594,30 @@ data class MutableMatrix3f( return this } + fun load(from: FloatMatrix<*>) { + val m00: Float; val m01: Float; val m02: Float; + val m10: Float; val m11: Float; val m12: Float; + val m20: Float; val m21: Float; val m22: Float; + + if (from is AbstractMatrix3f<*>) { + m00 = from.m00; m01 = from.m01; m02 = from.m02; + m10 = from.m10; m11 = from.m11; m12 = from.m12; + m20 = from.m20; m21 = from.m21; m22 = from.m22; + } else { + m00 = from[0, 0]; m01 = from[0, 1]; m02 = from[0, 2]; + m10 = from[1, 0]; m11 = from[1, 1]; m12 = from[1, 2]; + m20 = from[2, 0]; m21 = from[2, 1]; m22 = from[2, 2]; + } + + this.m00 = m00; this.m01 = m01; this.m02 = m02 + this.m10 = m10; this.m11 = m11; this.m12 = m12 + this.m20 = m20; this.m21 = m21; this.m22 = m22 + } + + override var a11 by this::m00; override var a12 by this::m01; override var a13 by this::m02 + override var a21 by this::m10; override var a22 by this::m11; override var a23 by this::m12 + override var a31 by this::m20; override var a32 by this::m21; override var a33 by this::m22 + operator fun set(row: Int, column: Int, value: Float) { when (column) { 0 -> when (row) { @@ -986,3 +1645,881 @@ data class MutableMatrix3f( } } } + +abstract class AbstractMatrix3d> : DoubleMatrix { + companion object { + private val directBuffer by lazy { ByteBuffer.allocateDirect(4 * 3 * 3).also { it.order(ByteOrder.nativeOrder()) }.asDoubleBuffer()!! } + } + + /** + * Наполняет синглтон и возвращает DirectDoubleBuffer который необходимо использовать сразу, + * нет никаких гарантий что он не будет изменён в дальнейшем + * + * Функция предназначена исключительно для загрузки данной матрицы в память видеокарты + * + * Данный буфер всегда будет одним и тем же, но его содержимое обновлено + */ + fun toDoubleBuffer(columnMajor: Boolean = true): DoubleBuffer { + val directBuffer = directBuffer + directBuffer.position(0) + + if (columnMajor) { + directBuffer.put(m00) + directBuffer.put(m10) + directBuffer.put(m20) + + directBuffer.put(m01) + directBuffer.put(m11) + directBuffer.put(m21) + + directBuffer.put(m02) + directBuffer.put(m12) + directBuffer.put(m22) + } else { + directBuffer.put(m00) + directBuffer.put(m01) + directBuffer.put(m02) + + directBuffer.put(m10) + directBuffer.put(m11) + directBuffer.put(m12) + + directBuffer.put(m20) + directBuffer.put(m21) + directBuffer.put(m22) + } + + directBuffer.position(0) + return directBuffer + } + + abstract val m00: Double; abstract val m01: Double; abstract val m02: Double + abstract val m10: Double; abstract val m11: Double; abstract val m12: Double + abstract val m20: Double; abstract val m21: Double; abstract val m22: Double + + open val a11 get() = m00; open val a12 get() = m01; open val a13 get() = m02 + open val a21 get() = m10; open val a22 get() = m11; open val a23 get() = m12 + open val a31 get() = m20; open val a32 get() = m21; open val a33 get() = m22 + + override val columns: Int + get() = 3 + + override val rows: Int + get() = 3 + + override fun get(row: Int, column: Int): Double { + return when (column) { + 0 -> when (row) { + 0 -> m00 + 1 -> m10 + 2 -> m20 + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 1 -> when (row) { + 0 -> m01 + 1 -> m11 + 2 -> m21 + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 2 -> when (row) { + 0 -> m02 + 1 -> m12 + 2 -> m22 + else -> throw IndexOutOfBoundsException("Row: $row") + } + + else -> throw IndexOutOfBoundsException("Column: $column") + } + } + + protected abstract fun createOrModify( + m00: Double, m01: Double, m02: Double, + m10: Double, m11: Double, m12: Double, + m20: Double, m21: Double, m22: Double, + ): T + + override fun plus(other: IMatrix): T { + if (other !is IMatrixLikeDouble) { + throw IllegalArgumentException("Can not use $other for addition") + } + + if (other.columns != 3 || other.rows != 3) { + throw IllegalArgumentException("Concrete Matrix3d can only use 3x3 matrices") + } + + val m00: Double; val m01: Double; val m02: Double; + val m10: Double; val m11: Double; val m12: Double; + val m20: Double; val m21: Double; val m22: Double; + + if (other is AbstractMatrix3d<*>) { + m00 = other.m00; m01 = other.m01; m02 = other.m02; + m10 = other.m10; m11 = other.m11; m12 = other.m12; + m20 = other.m20; m21 = other.m21; m22 = other.m22; + } else { + m00 = other[0, 0]; m01 = other[0, 1]; m02 = other[0, 2]; + m10 = other[1, 0]; m11 = other[1, 1]; m12 = other[1, 2]; + m20 = other[2, 0]; m21 = other[2, 1]; m22 = other[2, 2]; + } + + return createOrModify( + this.m00 + m00, this.m01 + m01, this.m02 + m02, + this.m10 + m10, this.m11 + m11, this.m12 + m12, + this.m20 + m20, this.m21 + m21, this.m22 + m22, + ) + } + + override fun minus(other: IMatrix): T { + if (other !is IMatrixLikeDouble) { + throw IllegalArgumentException("Can not use $other for subtraction") + } + + if (other.columns != 3 || other.rows != 3) { + throw IllegalArgumentException("Concrete Matrix3d can only use 3x3 matrices") + } + + val m00: Double; val m01: Double; val m02: Double; + val m10: Double; val m11: Double; val m12: Double; + val m20: Double; val m21: Double; val m22: Double; + + if (other is AbstractMatrix3d<*>) { + m00 = other.m00; m01 = other.m01; m02 = other.m02; + m10 = other.m10; m11 = other.m11; m12 = other.m12; + m20 = other.m20; m21 = other.m21; m22 = other.m22; + } else { + m00 = other[0, 0]; m01 = other[0, 1]; m02 = other[0, 2]; + m10 = other[1, 0]; m11 = other[1, 1]; m12 = other[1, 2]; + m20 = other[2, 0]; m21 = other[2, 1]; m22 = other[2, 2]; + } + + return createOrModify( + this.m00 - m00, this.m01 - m01, this.m02 - m02, + this.m10 - m10, this.m11 - m11, this.m12 - m12, + this.m20 - m20, this.m21 - m21, this.m22 - m22, + ) + } + + override fun plus(other: Double): T { + return createOrModify( + this.m00 + other, this.m01 + other, this.m02 + other, + this.m10 + other, this.m11 + other, this.m12 + other, + this.m20 + other, this.m21 + other, this.m22 + other, + ) + } + + override fun minus(other: Double): T { + return createOrModify( + this.m00 - other, this.m01 - other, this.m02 - other, + this.m10 - other, this.m11 - other, this.m12 - other, + this.m20 - other, this.m21 - other, this.m22 - other, + ) + } + + override fun times(other: Double): T { + return createOrModify( + this.m00 * other, this.m01 * other, this.m02 * other, + this.m10 * other, this.m11 * other, this.m12 * other, + this.m20 * other, this.m21 * other, this.m22 * other, + ) + } + + override fun div(other: Double): T { + return createOrModify( + this.m00 / other, this.m01 / other, this.m02 / other, + this.m10 / other, this.m11 / other, this.m12 / other, + this.m20 / other, this.m21 / other, this.m22 / other, + ) + } + + override fun scale(x: Double, y: Double, z: Double, w: Double): T { + return createOrModify( + this.m00 * x, this.m01, this.m02, + this.m10, this.m11 * y, this.m12, + this.m20, this.m21, this.m22 * z, + ) + } + + override fun translate(x: Double, y: Double, z: Double): T { + return createOrModify( + this.m00, this.m01, this.m02 + x, + this.m10, this.m11, this.m12 + y, + this.m20, this.m21, this.m22, + ) + } + + override fun times(other: IMatrix): T { + if (other !is IMatrixLikeDouble) { + throw IllegalArgumentException("Can not use $other for multiplication") + } + + if (other.columns != 3 || other.rows != 3) { + throw IllegalArgumentException("Concrete Matrix3d can only use 3x3 matrixes") + } + + val m00: Double; val m01: Double; val m02: Double; + val m10: Double; val m11: Double; val m12: Double; + val m20: Double; val m21: Double; val m22: Double; + + if (other is AbstractMatrix3d<*>) { + m00 = other.m00; m01 = other.m01; m02 = other.m02; + m10 = other.m10; m11 = other.m11; m12 = other.m12; + m20 = other.m20; m21 = other.m21; m22 = other.m22; + } else { + m00 = other[0, 0]; m01 = other[0, 1]; m02 = other[0, 2]; + m10 = other[1, 0]; m11 = other[1, 1]; m12 = other[1, 2]; + m20 = other[2, 0]; m21 = other[2, 1]; m22 = other[2, 2]; + } + + // первый столбец + val newm00 = + this.m00 * m00 + + this.m01 * m10 + + this.m02 * m20 + + val newm10 = + this.m10 * m00 + + this.m11 * m10 + + this.m12 * m20 + + val newm20 = + this.m20 * m00 + + this.m21 * m10 + + this.m22 * m20 + + // второй столбец + val newm01 = + this.m00 * m01 + + this.m01 * m11 + + this.m02 * m21 + + val newm11 = + this.m10 * m01 + + this.m11 * m11 + + this.m12 * m21 + + val newm21 = + this.m20 * m01 + + this.m21 * m11 + + this.m22 * m21 + + // третий столбец + val newm02 = + this.m00 * m02 + + this.m01 * m12 + + this.m02 * m22 + + val newm12 = + this.m10 * m02 + + this.m11 * m12 + + this.m12 * m22 + + val newm22 = + this.m20 * m02 + + this.m21 * m12 + + this.m22 * m22 + + return createOrModify( + newm00, newm01, newm02, + newm10, newm11, newm12, + newm20, newm21, newm22, + ) + } + + override fun translateWithScale(x: Double, y: Double, z: Double): T { + return createOrModify( + m00, m01, m02 + x * m00 + y * m01, + m10, m11, m12 + x * m10 + y * m11, + m20, m21, m22, + ) + } + + val determinant get() = m00 * m11 * m22 + m01 * m12 * m20 + m02 * m10 * m21 - m02 * m11 * m20 - m00 * m12 * m21 - m01 * m10 * m22 + + val absoluteMatrix get() = Matrix3d( + m00 = m00.absoluteValue, m01 = m01.absoluteValue, m02 = m02.absoluteValue, + m10 = m10.absoluteValue, m11 = m11.absoluteValue, m12 = m12.absoluteValue, + m20 = m20.absoluteValue, m21 = m21.absoluteValue, m22 = m22.absoluteValue, + ) + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + fun solve(b: IVector3d<*>): Vector3d { + var determinant = determinant + + if (determinant != 0.0) { + determinant = 1.0 / determinant + } + + return Vector3d( + x = determinant * scalarDotWithCross( + b.x, b.y, b.z, + m01, m11, m21, + m02, m12, m22, + ), + + y = determinant * scalarDotWithCross( + m00, m10, m20, + b.x, b.y, b.z, + m02, m12, m22, + ), + + z = determinant * scalarDotWithCross( + m00, m10, m20, + m01, m11, m21, + 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. + fun solve(b: IVector2d<*>): Vector2d { + var determinant = determinant + + if (determinant != 0.0) { + determinant = 1.0 / determinant + } + + return Vector2d( + x = determinant * (m11 * b.x - m01 * b.y), + y = determinant * (m00 * b.y - m10 * b.x), + ) + } + + fun getInverse2(): Matrix3d { + val a = m00 + val b = m01 + val c = m10 + val d = m11 + + var determinant = a * d - b * c + + if (determinant != 0.0) { + determinant = 1.0 / determinant + } + + return Matrix3d( + m00 = determinant * d, m01 = -determinant * b, m02 = 0.0, + m10 = -determinant * c, m11 = determinant * a, m12 = 0.0, + m20 = 0.0, m21 = 0.0, m22 = 0.0, + ) + } + + fun getInverse(): Matrix3d { + var determinant = determinant + + if (determinant != 0.0) { + determinant = 1.0 / determinant + } else { + return Matrix3d( + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + ) + } + + val m00 = determinant * (a22 * a33 - a23 * a23) + val m10 = determinant * (a13 * a23 - a12 * a33) + val m20 = determinant * (a12 * a23 - a13 * a22) + + val m01 = m10 + val m11 = determinant * (a11 * a33 - a13 * a13) + val m21 = determinant * (a13 * a12 - a11 * a23) + + val m02 = m20 + val m12 = m21 + val m22 = determinant * (a11 * a22 - a12 * a12) + + return Matrix3d( + m00 = m00, + m10 = m10, + m20 = m20, + m01 = m01, + m11 = m11, + m21 = m21, + m02 = m02, + m12 = m12, + m22 = m22, + ) + } +} + +data class Matrix3d( + override val m00: Double = 1.0, override val m01: Double = 0.0, override val m02: Double = 0.0, + override val m10: Double = 0.0, override val m11: Double = 1.0, override val m12: Double = 0.0, + override val m20: Double = 0.0, override val m21: Double = 0.0, override val m22: Double = 1.0, +) : AbstractMatrix3d() { + override fun createOrModify( + m00: Double, m01: Double, m02: Double, + m10: Double, m11: Double, m12: Double, + m20: Double, m21: Double, m22: Double, + ): Matrix3d { + return Matrix3d( + m00 = m00, m01 = m01, m02 = m02, + m10 = m10, m11 = m11, m12 = m12, + m20 = m20, m21 = m21, m22 = m22, + ) + } + + fun asMutableMatrix(): MutableMatrix3d { + return MutableMatrix3d( + m00 = m00, m01 = m01, m02 = m02, + m10 = m10, m11 = m11, m12 = m12, + m20 = m20, m21 = m21, m22 = m22, + ) + } + + companion object { + val IDENTITY = Matrix3d() + } +} + +data class MutableMatrix3d( + override var m00: Double = 1.0, override var m01: Double = 0.0, override var m02: Double = 0.0, + override var m10: Double = 0.0, override var m11: Double = 1.0, override var m12: Double = 0.0, + override var m20: Double = 0.0, override var m21: Double = 0.0, override var m22: Double = 1.0, +) : AbstractMatrix3d() { + override fun createOrModify( + m00: Double, m01: Double, m02: Double, + m10: Double, m11: Double, m12: Double, + m20: Double, m21: Double, m22: Double, + ): MutableMatrix3d { + this.m00 = m00; this.m01 = m01; this.m02 = m02 + this.m10 = m10; this.m11 = m11; this.m12 = m12 + this.m20 = m20; this.m21 = m21; this.m22 = m22 + + return this + } + + override var a11 by this::m00; override var a12 by this::m01; override var a13 by this::m02 + override var a21 by this::m10; override var a22 by this::m11; override var a23 by this::m12 + override var a31 by this::m20; override var a32 by this::m21; override var a33 by this::m22 + + fun load(from: DoubleMatrix<*>) { + val m00: Double; val m01: Double; val m02: Double; + val m10: Double; val m11: Double; val m12: Double; + val m20: Double; val m21: Double; val m22: Double; + + if (from is AbstractMatrix3d<*>) { + m00 = from.m00; m01 = from.m01; m02 = from.m02; + m10 = from.m10; m11 = from.m11; m12 = from.m12; + m20 = from.m20; m21 = from.m21; m22 = from.m22; + } else { + m00 = from[0, 0]; m01 = from[0, 1]; m02 = from[0, 2]; + m10 = from[1, 0]; m11 = from[1, 1]; m12 = from[1, 2]; + m20 = from[2, 0]; m21 = from[2, 1]; m22 = from[2, 2]; + } + + this.m00 = m00; this.m01 = m01; this.m02 = m02 + this.m10 = m10; this.m11 = m11; this.m12 = m12 + this.m20 = m20; this.m21 = m21; this.m22 = m22 + } + + fun setColumns(column1: IVector3d<*>, column2: IVector3d<*>, column3: IVector3d<*>) { + m00 = column1.x + m10 = column1.y + m20 = column1.z + + m01 = column2.x + m11 = column2.y + m21 = column2.y + + m02 = column3.x + m12 = column3.y + m22 = column3.y + } + + fun setIdentity() { + m00 = 1.0 + m01 = 0.0 + m02 = 0.0 + + m10 = 0.0 + m11 = 1.0 + m12 = 0.0 + + m20 = 0.0 + m21 = 0.0 + m22 = 1.0 + } + + fun zero() { + m00 = 0.0 + m01 = 0.0 + m02 = 0.0 + + m10 = 0.0 + m11 = 0.0 + m12 = 0.0 + + m20 = 0.0 + m21 = 0.0 + m22 = 0.0 + } + + operator fun set(row: Int, column: Int, value: Double) { + when (column) { + 0 -> when (row) { + 0 -> m00 = value + 1 -> m10 = value + 2 -> m20 = value + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 1 -> when (row) { + 0 -> m01 = value + 1 -> m11 = value + 2 -> m21 = value + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 2 -> when (row) { + 0 -> m02 = value + 1 -> m12 = value + 2 -> m22 = value + else -> throw IndexOutOfBoundsException("Row: $row") + } + + else -> throw IndexOutOfBoundsException("Column: $column") + } + } +} + +abstract class AbstractMatrix2d> : DoubleMatrix { + companion object { + private val directBuffer by lazy { ByteBuffer.allocateDirect(8 * 2 * 2).also { it.order(ByteOrder.nativeOrder()) }.asDoubleBuffer()!! } + } + + /** + * Наполняет синглтон и возвращает DirectFloatBuffer который необходимо использовать сразу, + * нет никаких гарантий что он не будет изменён в дальнейшем + * + * Функция предназначена исключительно для загрузки данной матрицы в память видеокарты + * + * Данный буфер всегда будет одним и тем же, но его содержимое обновлено + */ + fun toDoubleBuffer(columnMajor: Boolean = true): DoubleBuffer { + val directBuffer = directBuffer + directBuffer.position(0) + + if (columnMajor) { + directBuffer.put(m00) + directBuffer.put(m10) + + directBuffer.put(m01) + directBuffer.put(m11) + } else { + directBuffer.put(m00) + directBuffer.put(m01) + + directBuffer.put(m10) + directBuffer.put(m11) + } + + directBuffer.position(0) + return directBuffer + } + + abstract val m00: Double; abstract val m01: Double; + abstract val m10: Double; abstract val m11: Double; + + override val columns: Int + get() = 2 + + override val rows: Int + get() = 2 + + override fun get(row: Int, column: Int): Double { + return when (column) { + 0 -> when (row) { + 0 -> m00 + 1 -> m10 + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 1 -> when (row) { + 0 -> m01 + 1 -> m11 + else -> throw IndexOutOfBoundsException("Row: $row") + } + + else -> throw IndexOutOfBoundsException("Column: $column") + } + } + + protected abstract fun createOrModify( + m00: Double, m01: Double, + m10: Double, m11: Double, + ): T + + override fun plus(other: IMatrix): T { + if (other !is DoubleMatrix<*>) { + throw IllegalArgumentException("Can not use $other for addition") + } + + if (other.columns != 2 || other.rows != 2) { + throw IllegalArgumentException("Concrete Matrix2f can only use 2x2 matrices") + } + + val m00: Double; val m01: Double; + val m10: Double; val m11: Double; + + if (other is AbstractMatrix2d<*>) { + m00 = other.m00; m01 = other.m01; + m10 = other.m10; m11 = other.m11; + } else { + m00 = other[0, 0]; m01 = other[0, 1]; + m10 = other[1, 0]; m11 = other[1, 1]; + } + + return createOrModify( + this.m00 + m00, this.m01 + m01, + this.m10 + m10, this.m11 + m11, + ) + } + + override fun minus(other: IMatrix): T { + if (other !is IMatrixLikeDouble) { + throw IllegalArgumentException("Can not use $other for subtraction") + } + + if (other.columns != 2 || other.rows != 2) { + throw IllegalArgumentException("Concrete Matrix2f can only use 2x2 matrices") + } + + val m00: Double; val m01: Double; + val m10: Double; val m11: Double; + + if (other is AbstractMatrix2d<*>) { + m00 = other.m00; m01 = other.m01; + m10 = other.m10; m11 = other.m11; + } else { + m00 = other[0, 0]; m01 = other[0, 1]; + m10 = other[1, 0]; m11 = other[1, 1]; + } + + return createOrModify( + this.m00 - m00, this.m01 - m01, + this.m10 - m10, this.m11 - m11, + ) + } + + override fun plus(other: Double): T { + return createOrModify( + this.m00 + other, this.m01 + other, + this.m10 + other, this.m11 + other, + ) + } + + override fun minus(other: Double): T { + return createOrModify( + this.m00 - other, this.m01 - other, + this.m10 - other, this.m11 - other, + ) + } + + override fun times(other: Double): T { + return createOrModify( + this.m00 * other, this.m01 * other, + this.m10 * other, this.m11 * other, + ) + } + + override fun div(other: Double): T { + return createOrModify( + this.m00 / other, this.m01 / other, + this.m10 / other, this.m11 / other, + ) + } + + override fun scale(x: Double, y: Double, z: Double, w: Double): T { + return createOrModify( + this.m00 * x, this.m01, + this.m10, this.m11 * y, + ) + } + + override fun translate(x: Double, y: Double, z: Double): T { + return createOrModify( + this.m00, this.m01 + x, + this.m10, this.m11, + ) + } + + override fun times(other: IMatrix): T { + if (other !is IMatrixLikeDouble) { + throw IllegalArgumentException("Can not use $other for multiplication") + } + + if (other.columns != 3 || other.rows != 3) { + throw IllegalArgumentException("Concrete Matrix3f can only use 3x3 matrixes") + } + + val m00: Double; val m01: Double; + val m10: Double; val m11: Double; + + if (other is AbstractMatrix2d<*>) { + m00 = other.m00; m01 = other.m01; + m10 = other.m10; m11 = other.m11; + } else { + m00 = other[0, 0]; m01 = other[0, 1]; + m10 = other[1, 0]; m11 = other[1, 1]; + } + + // первый столбец + val newm00 = + this.m00 * m00 + + this.m01 * m10 + + val newm10 = + this.m10 * m00 + + this.m11 * m10 + + // второй столбец + val newm01 = + this.m00 * m01 + + this.m01 * m11 + + val newm11 = + this.m10 * m01 + + this.m11 * m11 + + return createOrModify( + newm00, newm01, + newm10, newm11, + ) + } + + override fun translateWithScale(x: Double, y: Double, z: Double): T { + return createOrModify( + m00, m01 + x * m00, + m10, m11, + ) + } + + val determinant: Double get() = m00 * m11 - m01 * m10 + + val absoluteMatrix get() = Matrix2d( + m00 = m00.absoluteValue, m01 = m01.absoluteValue, + m10 = m10.absoluteValue, m11 = m11.absoluteValue, + ) + + fun getInverse(): Matrix2d { + val a = m00 + val b = m01 + val c = m10 + val d = m11 + + var determinant = determinant + + if (determinant != 0.0) { + determinant = 1.0 / determinant + } + + return Matrix2d( + m00 = determinant * d, m01 = -determinant * b, + m10 = -determinant * c, m11 = determinant * a, + ) + } + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + fun solve(b: IVector2d<*>): Vector2d { + var determinant = determinant + + if (determinant != 0.0) { + determinant = 1.0 / determinant + } + + return Vector2d( + x = determinant * (m11 * b.x - m01 * b.y), + y = determinant * (m00 * b.y - m10 * b.x), + ) + } +} + +// ex.x = m00 +// ex.y = m10 +// ey.x = m01 +// ey.y = m11 +data class Matrix2d( + override val m00: Double = 1.0, override val m01: Double = 0.0, + override val m10: Double = 0.0, override val m11: Double = 1.0, +) : AbstractMatrix2d() { + override fun createOrModify( + m00: Double, m01: Double, + m10: Double, m11: Double, + ): Matrix2d { + return Matrix2d( + m00 = m00, m01 = m01, + m10 = m10, m11 = m11, + ) + } + + fun asMutableMatrix(): MutableMatrix2d { + return MutableMatrix2d( + m00 = m00, m01 = m01, + m10 = m10, m11 = m11, + ) + } + + companion object { + val IDENTITY = Matrix2d() + } +} + +data class MutableMatrix2d( + override var m00: Double = 1.0, override var m01: Double = 0.0, + override var m10: Double = 0.0, override var m11: Double = 1.0, +) : AbstractMatrix2d() { + override fun createOrModify( + m00: Double, m01: Double, + m10: Double, m11: Double, + ): MutableMatrix2d { + this.m00 = m00; this.m01 = m01; + this.m10 = m10; this.m11 = m11; + + return this + } + + fun setColumns(column1: IVector2d<*>, column2: IVector2d<*>) { + m00 = column1.x + m10 = column1.y + + m01 = column2.x + m11 = column2.y + } + + fun setIdentity() { + m00 = 1.0 + m01 = 0.0 + m11 = 1.0 + m10 = 0.0 + } + + fun zero() { + m00 = 0.0 + m01 = 0.0 + m11 = 0.0 + m10 = 0.0 + } + + operator fun set(row: Int, column: Int, value: Double) { + when (column) { + 0 -> when (row) { + 0 -> m00 = value + 1 -> m10 = value + else -> throw IndexOutOfBoundsException("Row: $row") + } + + 1 -> when (row) { + 0 -> m01 = value + 1 -> m11 = value + else -> throw IndexOutOfBoundsException("Row: $row") + } + + else -> throw IndexOutOfBoundsException("Column: $column") + } + } +} diff --git a/src/main/kotlin/ru/dbotthepony/kstarbound/math/Vector.kt b/src/main/kotlin/ru/dbotthepony/kstarbound/math/Vector.kt index af26d9f1..8c1c75b9 100644 --- a/src/main/kotlin/ru/dbotthepony/kstarbound/math/Vector.kt +++ b/src/main/kotlin/ru/dbotthepony/kstarbound/math/Vector.kt @@ -5,10 +5,7 @@ import com.google.gson.TypeAdapter import com.google.gson.stream.JsonReader import com.google.gson.stream.JsonWriter import ru.dbotthepony.kstarbound.api.* -import kotlin.math.cos -import kotlin.math.pow -import kotlin.math.sin -import kotlin.math.sqrt +import kotlin.math.* // Так как у нас нет шаблонов ни в Java, ни в Kotlin // а дженерики вызывают autoboxing @@ -56,6 +53,7 @@ abstract class IVector2i> : IMatrixLike, IMatrixLikeInt, IStruc operator fun unaryMinus() = make(-x, -y) val length get() = sqrt(x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble()) + val lengthSquared get() = x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble() fun dotProduct(other: IVector2i<*>): Double { return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble() @@ -69,15 +67,15 @@ abstract class IVector2i> : IMatrixLike, IMatrixLikeInt, IStruc return other.x * x.toDouble() + other.y * y.toDouble() } - fun InvDotProduct(other: IVector2i<*>): Double { + fun invDotProduct(other: IVector2i<*>): Double { return other.x.toDouble() * y.toDouble() + other.y.toDouble() * x.toDouble() } - fun InvDotProduct(other: IVector2f<*>): Double { + fun invDotProduct(other: IVector2f<*>): Double { return other.x.toDouble() * y.toDouble() + other.y.toDouble() * x.toDouble() } - fun InvDotProduct(other: IVector2d<*>): Double { + fun invDotProduct(other: IVector2d<*>): Double { return other.x * y.toDouble() + other.y * x.toDouble() } @@ -98,6 +96,29 @@ abstract class IVector2i> : IMatrixLike, IMatrixLikeInt, IStruc return Vector2d(x / len, y / len) } + val absoluteVector: Vector2i get() = Vector2i(x.absoluteValue, y.absoluteValue) + + fun minimumPerComponent(other: IVector2i<*>): Vector2i { + return Vector2i( + x = x.coerceAtMost(other.x), + y = y.coerceAtMost(other.y), + ) + } + + fun maximumPerComponent(other: IVector2i<*>): Vector2i { + return Vector2i( + x = x.coerceAtLeast(other.x), + y = y.coerceAtLeast(other.y), + ) + } + + fun clampPerComponent(min: IVector2i<*>, max: IVector2i<*>): Vector2i { + return Vector2i( + x = x.coerceAtLeast(min.x).coerceAtMost(max.x), + y = y.coerceAtLeast(min.y).coerceAtMost(max.y), + ) + } + fun left() = make(x - 1, y) fun right() = make(x + 1, y) fun up() = make(x, y + 1) @@ -197,6 +218,9 @@ abstract class IVector2f> : IMatrixLike, IMatrixLikeFloat, IStr fun down() = make(x, y - 1) val length get() = sqrt(x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble()) + val lengthSquared get() = x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble() + + val isFinite get() = x.isFinite() && y.isFinite() && !x.isNaN() && !y.isNaN() fun dotProduct(other: IVector2i<*>): Double { return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble() @@ -210,15 +234,15 @@ abstract class IVector2f> : IMatrixLike, IMatrixLikeFloat, IStr return other.x * x.toDouble() + other.y * y.toDouble() } - fun InvDotProduct(other: IVector2i<*>): Double { + fun invDotProduct(other: IVector2i<*>): Double { return other.x.toDouble() * y.toDouble() + other.y.toDouble() * x.toDouble() } - fun InvDotProduct(other: IVector2f<*>): Double { + fun invDotProduct(other: IVector2f<*>): Double { return other.x.toDouble() * y.toDouble() + other.y.toDouble() * x.toDouble() } - fun InvDotProduct(other: IVector2d<*>): Double { + fun invDotProduct(other: IVector2d<*>): Double { return other.x * y.toDouble() + other.y * x.toDouble() } @@ -239,6 +263,29 @@ abstract class IVector2f> : IMatrixLike, IMatrixLikeFloat, IStr return Vector2d(x / len, y / len) } + val absoluteVector: Vector2f get() = Vector2f(x.absoluteValue, y.absoluteValue) + + fun minimumPerComponent(other: IVector2f<*>): Vector2f { + return Vector2f( + x = x.coerceAtMost(other.x), + y = y.coerceAtMost(other.y), + ) + } + + fun maximumPerComponent(other: IVector2f<*>): Vector2f { + return Vector2f( + x = x.coerceAtLeast(other.x), + y = y.coerceAtLeast(other.y), + ) + } + + fun clampPerComponent(min: IVector2f<*>, max: IVector2f<*>): Vector2f { + return Vector2f( + x = x.coerceAtLeast(min.x).coerceAtMost(max.x), + y = y.coerceAtLeast(min.y).coerceAtMost(max.y), + ) + } + override fun get(row: Int, column: Int): Float { if (column != 0) { throw IndexOutOfBoundsException("Column must be 0 ($column given)") @@ -326,6 +373,15 @@ abstract class IVector2d> : IMatrixLike, IMatrixLikeDouble, ISt operator fun unaryMinus() = make(-x, -y) val length get() = sqrt(x * x + y * y) + val lengthSquared get() = x * x + y * y + + val isFinite get() = x.isFinite() && y.isFinite() && !x.isNaN() && !y.isNaN() + + inline fun isFiniteOrThrow(lazy: () -> Any) { + if (!isFinite) { + throw IllegalStateException(lazy.invoke().toString()) + } + } fun dotProduct(other: IVector2i<*>): Double { return other.x * x + other.y * y @@ -351,6 +407,22 @@ abstract class IVector2d> : IMatrixLike, IMatrixLikeDouble, ISt return other.x * y + other.y * x } + fun crossProduct(other: IVector2i<*>): Double { + return x * other.y - y * other.x + } + + fun crossProduct(other: IVector2f<*>): Double { + return x * other.y - y * other.x + } + + fun crossProduct(other: IVector2d<*>): Double { + return x * other.y - y * other.x + } + + fun crossProduct(other: Double): Vector2d { + return Vector2d(y * other, x * -other) + } + fun distance(other: IVector2i<*>): Double { return sqrt((x - other.x).toDouble().pow(2.0) + (y - other.y).toDouble().pow(2.0)) } @@ -363,11 +435,46 @@ abstract class IVector2d> : IMatrixLike, IMatrixLikeDouble, ISt return sqrt((x - other.x).toDouble().pow(2.0) + (y - other.y).toDouble().pow(2.0)) } + fun distanceSquared(other: IVector2i<*>): Double { + return (x - other.x).toDouble().pow(2.0) + (y - other.y).toDouble().pow(2.0) + } + + fun distanceSquared(other: IVector2f<*>): Double { + return (x - other.x).toDouble().pow(2.0) + (y - other.y).toDouble().pow(2.0) + } + + fun distanceSquared(other: IVector2d<*>): Double { + return (x - other.x).toDouble().pow(2.0) + (y - other.y).toDouble().pow(2.0) + } + val normalized: Vector2d get() { val len = length return Vector2d(x / len, y / len) } + val absoluteVector: Vector2d get() = Vector2d(x.absoluteValue, y.absoluteValue) + + fun minimumPerComponent(other: IVector2d<*>): Vector2d { + return Vector2d( + x = x.coerceAtMost(other.x), + y = y.coerceAtMost(other.y), + ) + } + + fun maximumPerComponent(other: IVector2d<*>): Vector2d { + return Vector2d( + x = x.coerceAtLeast(other.x), + y = y.coerceAtLeast(other.y), + ) + } + + fun clampPerComponent(min: IVector2d<*>, max: IVector2d<*>): Vector2d { + return Vector2d( + x = x.coerceAtLeast(min.x).coerceAtMost(max.x), + y = y.coerceAtLeast(min.y).coerceAtMost(max.y), + ) + } + fun left() = make(x - 1, y) fun right() = make(x + 1, y) fun up() = make(x, y + 1) @@ -385,13 +492,38 @@ abstract class IVector2d> : IMatrixLike, IMatrixLikeDouble, ISt } } + operator fun times(other: IMatrixLikeDouble): T { + if (other.rows >= 2 && other.columns >= 2) { + val x = this.x * other[0, 0] + + this.y * other[0, 1] + + val y = this.x * other[1, 0] + + this.y * other[1, 1] + + return make(x, y) + } + + throw IllegalArgumentException("Incompatible matrix provided: ${other.rows} x ${other.columns}") + } + protected abstract fun make(x: Double, y: Double): T fun toFloatVector(): Vector2f = Vector2f(x.toFloat(), y.toFloat()) } +fun Double.crossProduct(other: IVector2d<*>): Vector2d { + return Vector2d(-this * other.y, this * other.x) +} + +// Только Vector2d во избежание двоякого поведения с мутирующими векторами +operator fun Double.times(other: Vector2d) = Vector2d(this * other.x, this * other.y) + data class Vector2d(override val x: Double = 0.0, override val y: Double = 0.0) : IVector2d() { override fun make(x: Double, y: Double) = Vector2d(x, y) + fun toMutableVector(): MutableVector2d { + return MutableVector2d(x, y) + } + companion object { fun fromJson(input: JsonArray): Vector2d { return Vector2d(input[0].asDouble, input[1].asDouble) @@ -438,6 +570,21 @@ data class MutableVector2d(override var x: Double = 0.0, override var y: Double return this } + fun zero(): MutableVector2d { + this.x = 0.0 + this.y = 0.0 + return this + } + + fun toVector(): Vector2d { + return Vector2d(x, y) + } + + fun load(from: IStruct2d) { + x = from.component1() + y = from.component2() + } + companion object { fun fromJson(input: JsonArray): MutableVector2d { return MutableVector2d(input[0].asDouble, input[1].asDouble) @@ -466,6 +613,32 @@ abstract class IVector3f> : IMatrixLike, IMatrixLikeFloat, IStr operator fun unaryMinus() = make(-x, -y, -z) val length get() = sqrt(x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble() + z.toDouble() * z.toDouble()) + val lengthSquared get() = x.toDouble() * x.toDouble() + y.toDouble() * y.toDouble() + z.toDouble() * z.toDouble() + + val isFinite get() = x.isFinite() && y.isFinite() && z.isFinite() && !x.isNaN() && !y.isNaN() && !z.isNaN() + + val absoluteVector: Vector3f get() = Vector3f(x.absoluteValue, y.absoluteValue, z.absoluteValue) + + fun minimumPerComponent(other: IVector3f<*>): Vector3f { + return Vector3f( + x = x.coerceAtMost(other.x), + y = y.coerceAtMost(other.y), + ) + } + + fun maximumPerComponent(other: IVector3f<*>): Vector3f { + return Vector3f( + x = x.coerceAtLeast(other.x), + y = y.coerceAtLeast(other.y), + ) + } + + fun clampPerComponent(min: IVector3f<*>, max: IVector3f<*>): Vector3f { + return Vector3f( + x = x.coerceAtLeast(min.x).coerceAtMost(max.x), + y = y.coerceAtLeast(min.y).coerceAtMost(max.y), + ) + } fun dotProduct(other: IVector3f<*>): Double { return other.x.toDouble() * x.toDouble() + other.y.toDouble() * y.toDouble() + other.z.toDouble() * z.toDouble() @@ -568,6 +741,155 @@ data class MutableVector3f(override var x: Float = 0f, override var y: Float = 0 } } +abstract class IVector3d> : IMatrixLike, IMatrixLikeDouble, IStruct3d { + override val columns = 1 + override val rows = 3 + + abstract val x: Double + abstract val y: Double + abstract val z: Double + + operator fun plus(other: IVector3d<*>) = make(x + other.x, y + other.y, z + other.z) + operator fun minus(other: IVector3d<*>) = make(x - other.x, y - other.y, z - other.z) + operator fun times(other: IVector3d<*>) = make(x * other.x, y * other.y, z * other.z) + operator fun div(other: IVector3d<*>) = make(x / other.x, y / other.y, z / other.z) + + operator fun plus(other: Double) = make(x + other, y + other, z + other) + operator fun minus(other: Double) = make(x - other, y - other, z - other) + operator fun times(other: Double) = make(x * other, y * other, z * other) + operator fun div(other: Double) = make(x / other, y / other, z / other) + + operator fun unaryMinus() = make(-x, -y, -z) + + val length get() = sqrt(x * x + y * y + z * z) + val lengthSquared get() = x * x + y * y + z * z + + val isFinite get() = x.isFinite() && y.isFinite() && z.isFinite() && !x.isNaN() && !y.isNaN() && !z.isNaN() + + val absoluteVector: Vector3d get() = Vector3d(x.absoluteValue, y.absoluteValue, z.absoluteValue) + + fun minimumPerComponent(other: IVector3d<*>): Vector3d { + return Vector3d( + x = x.coerceAtMost(other.x), + y = y.coerceAtMost(other.y), + ) + } + + fun maximumPerComponent(other: IVector3d<*>): Vector3d { + return Vector3d( + x = x.coerceAtLeast(other.x), + y = y.coerceAtLeast(other.y), + ) + } + + fun clampPerComponent(min: IVector3d<*>, max: IVector3d<*>): Vector3d { + return Vector3d( + x = x.coerceAtLeast(min.x).coerceAtMost(max.x), + y = y.coerceAtLeast(min.y).coerceAtMost(max.y), + ) + } + + fun dotProduct(other: IVector3d<*>): Double { + return other.x * x + other.y * y + other.z * z + } + + override fun get(row: Int, column: Int): Double { + if (column != 0) { + throw IndexOutOfBoundsException("Column must be 0 ($column given)") + } + + return when (row) { + 0 -> x + 1 -> y + 2 -> z + else -> throw IndexOutOfBoundsException("Row out of bounds: $row") + } + } + + fun rotateAroundThis(rotation: Double): Matrix4d { + val c = cos(rotation) + val s = sin(rotation) + val cInv = 1f - c + + return Matrix4d( + m00 = c + x * x * cInv, m01 = x * y * cInv - z * s, m02 = x * z * cInv + y * s, + m10 = y * x * cInv + z * s, m11 = c + y * y * cInv, m12 = y * z * cInv - x * s, + m20 = z * x * cInv - y * s, m21 = z * y * cInv + x * s, m22 = c + z * z * cInv, + ) + } + + operator fun times(other: IMatrixLikeDouble): T { + if (other.rows >= 4 && other.columns >= 4) { + val x = this.x * other[0, 0] + + this.y * other[0, 1] + + this.z * other[0, 2] + + other[0, 3] + + val y = this.x * other[1, 0] + + this.y * other[1, 1] + + this.z * other[1, 2] + + other[1, 3] + + val z = this.x * other[2, 0] + + this.y * other[2, 1] + + this.z * other[2, 2] + + other[2, 3] + + return make(x, y, z) + } else if (other.rows >= 3 && other.columns >= 3) { + val x = this.x * other[0, 0] + + this.y * other[0, 1] + + this.z * other[0, 2] + + val y = this.x * other[1, 0] + + this.y * other[1, 1] + + this.z * other[1, 2] + + val z = this.x * other[2, 0] + + this.y * other[2, 1] + + this.z * other[2, 2] + + return make(x, y, z) + } + + throw IllegalArgumentException("Incompatible matrix provided: ${other.rows} x ${other.columns}") + } + + protected abstract fun make(x: Double, y: Double, z: Double): T +} + +data class Vector3d(override val x: Double = 0.0, override val y: Double = 0.0, override val z: Double = 0.0) : IVector3d() { + override fun make(x: Double, y: Double, z: Double): Vector3d { + return Vector3d(x, y, z) + } + + fun toMutableVector(): MutableVector3d { + return MutableVector3d(x, y, z) + } + + companion object { + val UP = Vector3d(0.0, 1.0, 0.0) + val DOWN = Vector3d(0.0, -1.0, 0.0) + val LEFT = Vector3d(-1.0, 0.0, 0.0) + val RIGHT = Vector3d(1.0, 0.0, 0.0) + val FORWARD = Vector3d(0.0, 0.0, 1.0) + val BACKWARD = Vector3d(0.0, 0.0, -1.0) + } +} + +data class MutableVector3d(override var x: Double = 0.0, override var y: Double = 0.0, override var z: Double = 0.0) : IVector3d() { + fun toVector(): Vector3d { + return Vector3d(x, y, z) + } + + override fun make(x: Double, y: Double, z: Double): MutableVector3d { + this.x = x + this.y = y + this.z = z + return this + } +} + abstract class IVector4f> : IMatrixLike, IMatrixLikeFloat, IStruct4f { abstract val x: Float abstract val y: Float @@ -586,6 +908,31 @@ abstract class IVector4f> : IMatrixLike, IMatrixLikeFloat, IStr operator fun unaryMinus() = make(-x, -y, -z, -w) + val absoluteVector: Vector4f get() = Vector4f(x.absoluteValue, y.absoluteValue, z.absoluteValue, w.absoluteValue) + + val isFinite get() = x.isFinite() && y.isFinite() && z.isFinite() && w.isFinite() && !x.isNaN() && !y.isNaN() && !z.isNaN() && !w.isNaN() + + fun minimumPerComponent(other: IVector4f<*>): Vector4f { + return Vector4f( + x = x.coerceAtMost(other.x), + y = y.coerceAtMost(other.y), + ) + } + + fun maximumPerComponent(other: IVector4f<*>): Vector4f { + return Vector4f( + x = x.coerceAtLeast(other.x), + y = y.coerceAtLeast(other.y), + ) + } + + fun clampPerComponent(min: IVector4f<*>, max: IVector4f<*>): Vector4f { + return Vector4f( + x = x.coerceAtLeast(min.x).coerceAtMost(max.x), + y = y.coerceAtLeast(min.y).coerceAtMost(max.y), + ) + } + override val columns = 1 override val rows = 4