151 lines
3.9 KiB
Kotlin
151 lines
3.9 KiB
Kotlin
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
|
|
}
|
|
}
|