337 lines
8.4 KiB
Kotlin
337 lines
8.4 KiB
Kotlin
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)
|
|
}
|
|
}
|