SapTest

import org.dyn4j.collision.broadphase.Sap
import org.dyn4j.dynamics.BodyFixture
import org.dyn4j.dynamics.Body
import org.dyn4j.geometry.Rectangle
import org.dyn4j.collision.broadphase.DynamicAABBTree

object SapTest {

  def main(args: Array[String]) {

    // create a BodyFixture to add to every Body
    val rectFixture = new BodyFixture(new Rectangle(32, 32))

    // first collidee
    val collidee1 = new Body
    collidee1.addFixture(rectFixture)

    // second collidee
    val collidee2 = new Body
    collidee2.addFixture(rectFixture)
    collidee2.translate(64, 0)

    // create AABBs for both collidees for output
    val aabbCollidee1 = collidee1.createAABB
    val aabbCollidee2 = collidee2.createAABB

    // add both collidees to the Sap broadphase.
    val sap = new Sap[Body, BodyFixture]
    sap.add(collidee1)
    sap.add(collidee2)

    // add both collidees to the DynamicAAB broadphase.
    val dynamicAABBTree = new DynamicAABBTree[Body, BodyFixture]
    dynamicAABBTree.add(collidee1)
    dynamicAABBTree.add(collidee2)

    // the collider I used overlapped the rightmost body in the Sap without touching its left side.
    // I'm testing collision with AABB to filter uninteresting collision pairs before the broadphase.
    val collider = new Body
    collider.addFixture(rectFixture)
    collider.translate(64 + 16, 0) // should overlap with collidee2. Touches the top, right and bottom side.
    var aabbCollider = collider.createAABB

    // do the checks
    var resultSap = sap.detect(aabbCollider)
    var resultDyn = dynamicAABBTree.detect(aabbCollider)
    var colliderOverlapsCollidee2 = aabbCollider overlaps aabbCollidee2

    // print results
    println("Test 1: Collider collides with top, right and bottom side of collidee 2.")
    println(s"collider  : $aabbCollider")
    println(s"collidee 1: $aabbCollidee1")
    println(s"collidee 2: $aabbCollidee2")
    println(s"sap.detect(aabbCollider) returned ${resultSap.size} results.")
    println(s"dynamicAABBTree.detect(aabbCollider) returned ${resultDyn.size} results.")
    println(s"aabbCollider.overlaps(aabbCollidee2) returned $colliderOverlapsCollidee2.")
    println

    //    Test 1: Collider collides with top, right and bottom side of collidee 2.
    //    collider  : AABB[Min=(64.0, -16.0)|Max=(96.0, 16.0)]
    //    collidee 1: AABB[Min=(-16.0, -16.0)|Max=(16.0, 16.0)]
    //    collidee 2: AABB[Min=(48.0, -16.0)|Max=(80.0, 16.0)]
    //    sap.detect(aabbCollider) returned 0 results.
    //    dynamicAABBTree.detect(aabbCollider) returned 1 results.
    //    aabbCollider.overlaps(aabbCollidee2) returned true.

    // translating collider so that it touches the left, top and bottom side of collidee2.
    collider.translate(-32, 0)
    aabbCollider = collider.createAABB // create translated AABB

    // do the checks again
    resultSap = sap.detect(aabbCollider)
    colliderOverlapsCollidee2 = aabbCollider overlaps aabbCollidee2

    // print results
    println("Test 2: Collider collides with left, top and bottom side of collidee 2.")
    println(s"collider  : $aabbCollider")
    println(s"collidee 1: $aabbCollidee1")
    println(s"collidee 2: $aabbCollidee2")
    println(s"sap.detect(aabbCollider) returned ${resultSap.size} results.")
    println(s"dynamicAABBTree.detect(aabbCollider) returned ${resultDyn.size} results.")
    println(s"aabbCollider.overlaps(aabbCollidee2) returned $colliderOverlapsCollidee2.")

    //    Test 2: Collider collides with left, top and bottom side of collidee 2.
    //    collider  : AABB[Min=(32.0, -16.0)|Max=(64.0, 16.0)]
    //    collidee 1: AABB[Min=(-16.0, -16.0)|Max=(16.0, 16.0)]
    //    collidee 2: AABB[Min=(48.0, -16.0)|Max=(80.0, 16.0)]
    //    sap.detect(aabbCollider) returned 1 results.
    //    dynamicAABBTree.detect(aabbCollider) returned 1 results.
    //    aabbCollider.overlaps(aabbCollidee2) returned true.

  }

}