/// <summary> /// Ray-cast against the proxies in the tree. This relies on the callback to perform a exact /// ray-cast in the case were the proxy contains a shape. The callback also performs the any /// collision filtering. This has performance roughly equal to k * log(n), where k is the number of /// collisions and n is the number of proxies in the tree. /// </summary> /// <param name="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> /// <param name="callback">a callback class that is called for each proxy that is hit by the ray.</param> public virtual void raycast(TreeRayCastCallback callback, RayCastInput input) { Vec2 p1 = input.p1; Vec2 p2 = input.p2; r.set_Renamed(p2).subLocal(p1); Debug.Assert(r.lengthSquared() > 0f); r.normalize(); // v is perpendicular to the segment. Vec2.crossToOutUnsafe(1f, r, v); absV.set_Renamed(v).absLocal(); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.maxFraction; // Build a bounding box for the segment. AABB segAABB = aabb; // Vec2 t = p1 + maxFraction * (p2 - p1); temp.set_Renamed(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1); Vec2.minToOut(p1, temp, segAABB.lowerBound); Vec2.maxToOut(p1, temp, segAABB.upperBound); intStack.push(m_root); while (intStack.Count > 0) { int nodeId = intStack.pop(); if (nodeId == TreeNode.NULL_NODE) { continue; } TreeNode node = m_nodes[nodeId]; if (!AABB.testOverlap(node.aabb, segAABB)) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) node.aabb.getCenterToOut(c); node.aabb.getExtentsToOut(h); temp.set_Renamed(p1).subLocal(c); float separation = MathUtils.abs(Vec2.dot(v, temp)) - Vec2.dot(absV, h); if (separation > 0.0f) { continue; } if (node.Leaf) { subInput.p1.set_Renamed(input.p1); subInput.p2.set_Renamed(input.p2); subInput.maxFraction = maxFraction; float value_Renamed = callback.raycastCallback(subInput, nodeId); if (value_Renamed == 0.0f) { // The client has terminated the ray cast. return; } if (value_Renamed > 0.0f) { // Update segment bounding box. maxFraction = value_Renamed; t.set_Renamed(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1); Vec2.minToOut(p1, t, segAABB.lowerBound); Vec2.maxToOut(p1, t, segAABB.upperBound); } } else { intStack.push(node.child1); intStack.push(node.child2); } } }
public void raycast(TreeRayCastCallback callback, RayCastInput input) { m_tree.raycast(callback, input); }
public void raycast(TreeRayCastCallback callback, RayCastInput input) { Vec2 p1 = input.p1; Vec2 p2 = input.p2; double p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y; double vx, vy; double rx, ry; double absVx, absVy; double cx, cy; double hx, hy; double tempx, tempy; r.x = p2x - p1x; r.y = p2y - p1y; r.normalize(); rx = r.x; ry = r.y; // v is perpendicular to the segment. vx = -1d * ry; vy = 1d * rx; absVx = MathUtils.abs(vx); absVy = MathUtils.abs(vy); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) double maxFraction = input.maxFraction; // Build a bounding box for the segment. 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 nodeStack.reset(); nodeStack.push(m_root); while (nodeStack.getCount() > 0) { DynamicTreeNode node = nodeStack.pop(); if (node == null) { continue; } 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) * .5d; cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5d; hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5d; hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5d; tempx = p1x - cx; tempy = p1y - cy; double separation = MathUtils.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy); if (separation > 0.0d) { continue; } if (node.isLeaf()) { subInput.p1.x = p1x; subInput.p1.y = p1y; subInput.p2.x = p2x; subInput.p2.y = p2y; subInput.maxFraction = maxFraction; double value = callback.raycastCallback(subInput, node.id); if (value == 0.0d) { // The client has terminated the ray cast. return; } if (value > 0.0d) { // 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 { nodeStack.push(node.child1); nodeStack.push(node.child2); } } }
/** * Ray-cast against the proxies in the tree. This relies on the callback to perform a exact * ray-cast in the case were the proxy contains a shape. The callback also performs the any * collision filtering. This has performance roughly equal to k * log(n), where k is the number of * collisions and n is the number of proxies in the tree. * * @param input the ray-cast input data. The ray : from p1 to p1 + maxFraction * (p2 - p1). * @param callback a callback class that is called for each proxy that is hit by the ray. */ public void raycast(TreeRayCastCallback callback, RayCastInput input) { m_tree.raycast(callback, input); }
public void raycast(TreeRayCastCallback callback, RayCastInput input) { Vec2 p1 = input.p1; 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; Debug.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. 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) { DynamicTreeNode node = nodeStack[--nodeStackIndex]; if (node == null) { continue; } 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]; Array.Copy(nodeStack, 0, newBuffer, 0, nodeStack.Length); nodeStack = newBuffer; } nodeStack[nodeStackIndex++] = node.child1; nodeStack[nodeStackIndex++] = node.child2; } } }