public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) { FPVector2 v1 = MathUtils.Mul(ref transform, _vertex1); FPVector2 v2 = MathUtils.Mul(ref transform, _vertex2); FPVector2 lower = FPVector2.Min(v1, v2); FPVector2 upper = FPVector2.Max(v1, v2); FPVector2 r = new FPVector2(Radius, Radius); aabb.LowerBound = lower - r; aabb.UpperBound = upper + r; }
/// <summary> /// Given a transform, compute the associated axis aligned bounding box for a child shape. /// </summary> /// <param name="aabb">The aabb results.</param> /// <param name="transform">The world transform of the shape.</param> /// <param name="childIndex">The child shape index.</param> public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) { FPVector2 lower = MathUtils.Mul(ref transform, Vertices[0]); FPVector2 upper = lower; for (int i = 1; i < Vertices.Count; ++i) { FPVector2 v = MathUtils.Mul(ref transform, Vertices[i]); lower = FPVector2.Min(lower, v); upper = FPVector2.Max(upper, v); } FPVector2 r = new FPVector2(Radius, Radius); aabb.LowerBound = lower - r; aabb.UpperBound = upper + r; }
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) { Debug.Assert(childIndex < Vertices.Count); int i1 = childIndex; int i2 = childIndex + 1; if (i2 == Vertices.Count) { i2 = 0; } FPVector2 v1 = MathUtils.Mul(ref transform, Vertices[i1]); FPVector2 v2 = MathUtils.Mul(ref transform, Vertices[i2]); aabb.LowerBound = FPVector2.Min(v1, v2); aabb.UpperBound = FPVector2.Max(v1, v2); }
public static FPVector2 Clamp(FPVector2 a, FPVector2 low, FPVector2 high) { return(FPVector2.Max(low, FPVector2.Min(a, high))); }
/// <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="callback">A callback class that is called for each proxy that is hit by the ray.</param> /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> public void RayCast(Func<RayCastInput, int, FP> callback, ref RayCastInput input) { FPVector2 p1 = input.Point1; FPVector2 p2 = input.Point2; FPVector2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. FPVector2 absV = MathUtils.Abs(new FPVector2(-r.y, r.x)); //FPE: Inlined the 'v' variable // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) FP maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { FPVector2 t = p1 + maxFraction * (p2 - p1); FPVector2.Min(ref p1, ref t, out segmentAABB.LowerBound); FPVector2.Max(ref p1, ref t, out segmentAABB.UpperBound); } _raycastStack.Clear(); _raycastStack.Push(_root); while (_raycastStack.Count > 0) { int nodeId = _raycastStack.Pop(); if (nodeId == NullNode) { continue; } TreeNode<T> node = _nodes[nodeId]; if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) FPVector2 c = node.AABB.Center; FPVector2 h = node.AABB.Extents; FP separation = FP.Abs(FPVector2.Dot(new FPVector2(-r.y, r.x), p1 - c)) - FPVector2.Dot(absV, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; subInput.MaxFraction = maxFraction; FP value = callback(subInput, nodeId); if (value == 0.0f) { // the client has terminated the raycast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; FPVector2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = FPVector2.Min(p1, t); segmentAABB.UpperBound = FPVector2.Max(p1, t); } } else { _raycastStack.Push(node.Child1); _raycastStack.Push(node.Child2); } } }