Examples with AABB spacegraph.space2d.phys.collision.AABB used on opensource projects

Example 11 with AABB

use of spacegraph.space2d.phys.collision.AABB in project narchy by automenta.

the class Explosion method explodeBlastRadius.

public static void explodeBlastRadius(World world, Vec2 center, float blastRadius, float blastPower) {
    final float m_blastRadiusSq = blastRadius * blastRadius;
    // find all bodies with fixtures in blast radius AABB
    QueryCallback queryCallback = new QueryCallback() {

        @Override
        public boolean reportFixture(Fixture fixture) {
            Body body = fixture.getBody();
            Vec2 bodyCom = body.getWorldCenter();
            // ignore bodies outside the blast range
            if ((bodyCom.sub(center)).lengthSquared() < m_blastRadiusSq) {
                applyBlastImpulse(body, center, bodyCom, blastPower);
                return true;
            }
            return false;
        }
    };
    world.queryAABB(queryCallback, new AABB(center.sub(new Vec2(blastRadius, blastRadius)), center.add(new Vec2(blastRadius, blastRadius))));
}

Example 12 with AABB

use of spacegraph.space2d.phys.collision.AABB in project narchy by automenta.

the class DynamicTree method insertLeaf.

private final void insertLeaf(int leaf_index) {
    DynamicTreeNode leaf = node[leaf_index];
    if (m_root == null) {
        m_root = leaf;
        m_root.parent = null;
        return;
    }
    // find the best sibling
    AABB leafAABB = leaf.aabb;
    DynamicTreeNode index = m_root;
    while (index.child1 != null) {
        final DynamicTreeNode node = index;
        DynamicTreeNode child1 = node.child1;
        DynamicTreeNode child2 = node.child2;
        float area = node.aabb.getPerimeter();
        combinedAABB.combine(node.aabb, leafAABB);
        float combinedArea = combinedAABB.getPerimeter();
        // Cost of creating a new parent for this node and the new leaf
        float cost = 2.0f * combinedArea;
        // Minimum cost of pushing the leaf further down the tree
        float inheritanceCost = 2.0f * (combinedArea - area);
        // Cost of descending into child1
        float cost1;
        if (child1.child1 == null) {
            combinedAABB.combine(leafAABB, child1.aabb);
            cost1 = combinedAABB.getPerimeter() + inheritanceCost;
        } else {
            combinedAABB.combine(leafAABB, child1.aabb);
            float oldArea = child1.aabb.getPerimeter();
            float newArea = combinedAABB.getPerimeter();
            cost1 = (newArea - oldArea) + inheritanceCost;
        }
        // Cost of descending into child2
        float cost2;
        if (child2.child1 == null) {
            combinedAABB.combine(leafAABB, child2.aabb);
            cost2 = combinedAABB.getPerimeter() + inheritanceCost;
        } else {
            combinedAABB.combine(leafAABB, child2.aabb);
            float oldArea = child2.aabb.getPerimeter();
            float newArea = combinedAABB.getPerimeter();
            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;
        }
    }
    DynamicTreeNode sibling = index;
    DynamicTreeNode oldParent = node[sibling.id].parent;
    final DynamicTreeNode newParent = allocateNode();
    newParent.parent = oldParent;
    newParent.data = null;
    newParent.aabb.combine(leafAABB, sibling.aabb);
    newParent.height = sibling.height + 1;
    if (oldParent != null) {
        // The sibling was not the root.
        if (oldParent.child1 == sibling) {
            oldParent.child1 = newParent;
        } else {
            oldParent.child2 = newParent;
        }
        newParent.child1 = sibling;
        newParent.child2 = leaf;
        sibling.parent = newParent;
        leaf.parent = newParent;
    } else {
        // The sibling was the root.
        newParent.child1 = sibling;
        newParent.child2 = leaf;
        sibling.parent = newParent;
        leaf.parent = newParent;
        m_root = newParent;
    }
    // Walk back up the tree fixing heights and AABBs
    index = leaf.parent;
    while (index != null) {
        index = balance(index);
        DynamicTreeNode child1 = index.child1;
        DynamicTreeNode child2 = index.child2;
        assert (child1 != null);
        assert (child2 != null);
        index.height = 1 + MathUtils.max(child1.height, child2.height);
        index.aabb.combine(child1.aabb, child2.aabb);
        index = index.parent;
    }
// validate();
}

Example 13 with AABB

use of spacegraph.space2d.phys.collision.AABB in project narchy by automenta.

the class DynamicTree method rebuildBottomUp.

/**
 * Build an optimal tree. Very expensive. For testing.
 */
public void rebuildBottomUp() {
    int[] nodes = new int[m_nodeCount];
    int count = 0;
    // Build array of leaves. Free the rest.
    for (int i = 0; i < m_nodeCapacity; ++i) {
        if (node[i].height < 0) {
            // free node in pool
            continue;
        }
        DynamicTreeNode node = this.node[i];
        if (node.child1 == null) {
            node.parent = null;
            nodes[count] = i;
            ++count;
        } else {
            freeNode(node);
        }
    }
    AABB b = new AABB();
    while (count > 1) {
        float minCost = Float.MAX_VALUE;
        int iMin = -1, jMin = -1;
        for (int i = 0; i < count; ++i) {
            AABB aabbi = node[nodes[i]].aabb;
            for (int j = i + 1; j < count; ++j) {
                AABB aabbj = node[nodes[j]].aabb;
                b.combine(aabbi, aabbj);
                float cost = b.getPerimeter();
                if (cost < minCost) {
                    iMin = i;
                    jMin = j;
                    minCost = cost;
                }
            }
        }
        int index1 = nodes[iMin];
        int index2 = nodes[jMin];
        DynamicTreeNode child1 = node[index1];
        DynamicTreeNode child2 = node[index2];
        DynamicTreeNode parent = allocateNode();
        parent.child1 = child1;
        parent.child2 = child2;
        parent.height = 1 + MathUtils.max(child1.height, child2.height);
        parent.aabb.combine(child1.aabb, child2.aabb);
        parent.parent = null;
        child1.parent = parent;
        child2.parent = parent;
        nodes[jMin] = nodes[count - 1];
        nodes[iMin] = parent.id;
        --count;
    }
    m_root = node[nodes[0]];
    validate();
}

Example 14 with AABB

use of spacegraph.space2d.phys.collision.AABB in project narchy by automenta.

the class DynamicTree method raycast.

@Override
public void raycast(TreeRayCastCallback callback, RayCastInput input) {
    final Tuple2f p1 = input.p1;
    final Tuple2f p2 = input.p2;
    float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
    float vx, vy;
    float rx, ry;
    float absVx, absVy;
    float cx, cy;
    float hx, hy;
    float tempx, tempy;
    r.x = p2x - p1x;
    r.y = p2y - p1y;
    assert ((r.x * r.x + r.y * r.y) > 0f);
    r.normalize();
    rx = r.x;
    ry = r.y;
    // v is perpendicular to the segment.
    vx = -1f * ry;
    vy = 1f * rx;
    absVx = Math.abs(vx);
    absVy = Math.abs(vy);
    // Separating axis for segment (Gino, p80).
    // |dot(v, p1 - c)| > dot(|v|, h)
    float maxFraction = input.maxFraction;
    // Build a bounding box for the segment.
    final AABB segAABB = aabb;
    // Vec2 t = p1 + maxFraction * (p2 - p1);
    // before inline
    // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
    // Vec2.minToOut(p1, temp, segAABB.lowerBound);
    // Vec2.maxToOut(p1, temp, segAABB.upperBound);
    tempx = (p2x - p1x) * maxFraction + p1x;
    tempy = (p2y - p1y) * maxFraction + p1y;
    segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
    segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
    segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
    segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
    // end inline
    stackPtr = 0;
    stack[stackPtr++] = m_root;
    while (stackPtr > 0) {
        final DynamicTreeNode node = stack[--stackPtr];
        if (node == null) {
            continue;
        }
        final AABB nodeAABB = node.aabb;
        if (!AABB.testOverlap(nodeAABB, segAABB)) {
            continue;
        }
        // Separating axis for segment (Gino, p80).
        // |dot(v, p1 - c)| > dot(|v|, h)
        // node.aabb.getCenterToOut(c);
        // node.aabb.getExtentsToOut(h);
        cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5f;
        cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5f;
        hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5f;
        hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5f;
        tempx = p1x - cx;
        tempy = p1y - cy;
        float separation = Math.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
        if (separation > 0.0f) {
            continue;
        }
        if (node.child1 == null) {
            subInput.p1.x = p1x;
            subInput.p1.y = p1y;
            subInput.p2.x = p2x;
            subInput.p2.y = p2y;
            subInput.maxFraction = maxFraction;
            float value = callback.raycastCallback(subInput, node.id);
            if (value == 0.0f) {
                // The client has terminated the ray cast.
                return;
            }
            if (value > 0.0f) {
                // Update segment bounding box.
                maxFraction = value;
                // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
                // Vec2.minToOut(p1, temp, segAABB.lowerBound);
                // Vec2.maxToOut(p1, temp, segAABB.upperBound);
                tempx = (p2x - p1x) * maxFraction + p1x;
                tempy = (p2y - p1y) * maxFraction + p1y;
                segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
                segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
                segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
                segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
            }
        } else {
            if (stack.length - stackPtr - 2 <= 0) {
                DynamicTreeNode[] newBuffer = new DynamicTreeNode[stack.length * 2];
                System.arraycopy(stack, 0, newBuffer, 0, stack.length);
                stack = newBuffer;
            }
            stack[stackPtr++] = node.child1;
            stack[stackPtr++] = node.child2;
        }
    }
}

Example 15 with AABB

use of spacegraph.space2d.phys.collision.AABB in project narchy by automenta.

the class Fixture method synchronize.

/**
 * Internal method
 *
 * @param broadPhase
 * @param xf1
 * @param xf2
 */
protected void synchronize(BroadPhase broadPhase, final Transform transform1, final Transform transform2) {
    if (m_proxyCount == 0) {
        return;
    }
    for (int i = 0; i < m_proxyCount; ++i) {
        FixtureProxy proxy = proxies[i];
        // Compute an AABB that covers the swept shape (may miss some rotation effect).
        final AABB aabb1 = pool1;
        final AABB aab = pool2;
        shape.computeAABB(aabb1, transform1, proxy.childIndex);
        shape.computeAABB(aab, transform2, proxy.childIndex);
        proxy.aabb.lowerBound.x = aabb1.lowerBound.x < aab.lowerBound.x ? aabb1.lowerBound.x : aab.lowerBound.x;
        proxy.aabb.lowerBound.y = aabb1.lowerBound.y < aab.lowerBound.y ? aabb1.lowerBound.y : aab.lowerBound.y;
        proxy.aabb.upperBound.x = aabb1.upperBound.x > aab.upperBound.x ? aabb1.upperBound.x : aab.upperBound.x;
        proxy.aabb.upperBound.y = aabb1.upperBound.y > aab.upperBound.y ? aabb1.upperBound.y : aab.upperBound.y;
        displacement.x = transform2.pos.x - transform1.pos.x;
        displacement.y = transform2.pos.y - transform1.pos.y;
        broadPhase.moveProxy(proxy.id, proxy.aabb, displacement);
    }
}