use of org.jbox2d.collision.AABB in project libgdx by libgdx.
the class DynamicTree method raycast.
@Override
public void raycast(TreeRayCastCallback callback, RayCastInput input) {
final Vec2 p1 = input.p1;
final Vec2 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 = MathUtils.abs(vx);
absVy = MathUtils.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
nodeStackIndex = 0;
nodeStack[nodeStackIndex++] = m_root;
while (nodeStackIndex > 0) {
final DynamicTreeNode node = nodeStack[--nodeStackIndex];
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 = MathUtils.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 (nodeStack.length - nodeStackIndex - 2 <= 0) {
DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.length * 2];
System.arraycopy(nodeStack, 0, newBuffer, 0, nodeStack.length);
nodeStack = newBuffer;
}
nodeStack[nodeStackIndex++] = node.child1;
nodeStack[nodeStackIndex++] = node.child2;
}
}
}
use of org.jbox2d.collision.AABB in project libgdx by libgdx.
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 (m_nodes[i].height < 0) {
// free node in pool
continue;
}
DynamicTreeNode node = m_nodes[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 = m_nodes[nodes[i]].aabb;
for (int j = i + 1; j < count; ++j) {
AABB aabbj = m_nodes[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 = m_nodes[index1];
DynamicTreeNode child2 = m_nodes[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 = m_nodes[nodes[0]];
validate();
}
use of org.jbox2d.collision.AABB in project libgdx by libgdx.
the class DynamicTreeFlatNodes method createProxy.
@Override
public final int createProxy(final AABB aabb, Object userData) {
final int node = allocateNode();
// Fatten the aabb
final AABB nodeAABB = m_aabb[node];
nodeAABB.lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension;
nodeAABB.lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension;
nodeAABB.upperBound.x = aabb.upperBound.x + Settings.aabbExtension;
nodeAABB.upperBound.y = aabb.upperBound.y + Settings.aabbExtension;
m_userData[node] = userData;
insertLeaf(node);
return node;
}
use of org.jbox2d.collision.AABB in project libgdx by libgdx.
the class DynamicTreeFlatNodes method insertLeaf.
private final void insertLeaf(int leaf) {
if (m_root == NULL_NODE) {
m_root = leaf;
m_parent[m_root] = NULL_NODE;
return;
}
// find the best sibling
AABB leafAABB = m_aabb[leaf];
int index = m_root;
while (m_child1[index] != NULL_NODE) {
final int node = index;
int child1 = m_child1[node];
int child2 = m_child2[node];
final AABB nodeAABB = m_aabb[node];
float area = nodeAABB.getPerimeter();
combinedAABB.combine(nodeAABB, 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;
AABB child1AABB = m_aabb[child1];
if (m_child1[child1] == NULL_NODE) {
combinedAABB.combine(leafAABB, child1AABB);
cost1 = combinedAABB.getPerimeter() + inheritanceCost;
} else {
combinedAABB.combine(leafAABB, child1AABB);
float oldArea = child1AABB.getPerimeter();
float newArea = combinedAABB.getPerimeter();
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
float cost2;
AABB child2AABB = m_aabb[child2];
if (m_child1[child2] == NULL_NODE) {
combinedAABB.combine(leafAABB, child2AABB);
cost2 = combinedAABB.getPerimeter() + inheritanceCost;
} else {
combinedAABB.combine(leafAABB, child2AABB);
float oldArea = child2AABB.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;
}
}
int sibling = index;
int oldParent = m_parent[sibling];
final int newParent = allocateNode();
m_parent[newParent] = oldParent;
m_userData[newParent] = null;
m_aabb[newParent].combine(leafAABB, m_aabb[sibling]);
m_height[newParent] = m_height[sibling] + 1;
if (oldParent != NULL_NODE) {
// The sibling was not the root.
if (m_child1[oldParent] == sibling) {
m_child1[oldParent] = newParent;
} else {
m_child2[oldParent] = newParent;
}
m_child1[newParent] = sibling;
m_child2[newParent] = leaf;
m_parent[sibling] = newParent;
m_parent[leaf] = newParent;
} else {
// The sibling was the root.
m_child1[newParent] = sibling;
m_child2[newParent] = leaf;
m_parent[sibling] = newParent;
m_parent[leaf] = newParent;
m_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = m_parent[leaf];
while (index != NULL_NODE) {
index = balance(index);
int child1 = m_child1[index];
int child2 = m_child2[index];
assert (child1 != NULL_NODE);
assert (child2 != NULL_NODE);
m_height[index] = 1 + MathUtils.max(m_height[child1], m_height[child2]);
m_aabb[index].combine(m_aabb[child1], m_aabb[child2]);
index = m_parent[index];
}
// validate();
}
use of org.jbox2d.collision.AABB in project libgdx by libgdx.
the class DynamicTreeFlatNodes method expandBuffers.
private void expandBuffers(int oldSize, int newSize) {
m_aabb = BufferUtils.reallocateBuffer(AABB.class, m_aabb, oldSize, newSize);
m_userData = BufferUtils.reallocateBuffer(Object.class, m_userData, oldSize, newSize);
m_parent = BufferUtils.reallocateBuffer(m_parent, oldSize, newSize);
m_child1 = BufferUtils.reallocateBuffer(m_child1, oldSize, newSize);
m_child2 = BufferUtils.reallocateBuffer(m_child2, oldSize, newSize);
m_height = BufferUtils.reallocateBuffer(m_height, oldSize, newSize);
// Build a linked list for the free list.
for (int i = oldSize; i < newSize; i++) {
m_aabb[i] = new AABB();
m_parent[i] = (i == newSize - 1) ? NULL_NODE : i + 1;
m_height[i] = -1;
m_child1[i] = -1;
m_child2[i] = -1;
}
m_freeList = oldSize;
}
Aggregations