917 lines
22 KiB
Kotlin
917 lines
22 KiB
Kotlin
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<Body>(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")
|
|
}
|
|
}
|