public override void ComputeAABB(out AABB aabb, XForm transform) { aabb = default(AABB); Vec2 vec = transform.Position + Box2DX.Common.Math.Mul(transform.R, this._localPosition); aabb.LowerBound.Set(vec.X - this._radius, vec.Y - this._radius); aabb.UpperBound.Set(vec.X + this._radius, vec.Y + this._radius); }
/// <summary> /// Find the separation between poly1 and poly2 for a give edge normal on poly1. /// </summary> public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2) { int count1 = poly1._vertexCount; Vec2[] vertices1 = poly1._vertices; Vec2[] normals1 = poly1._normals; int count2 = poly2._vertexCount; Vec2[] vertices2 = poly2._vertices; Box2DXDebug.Assert(0 <= edge1 && edge1 < count1); // Convert normal from poly1's frame into poly2's frame. Vec2 normal1World = Common.Math.Mul(xf1.R, normals1[edge1]); Vec2 normal1 = Common.Math.MulT(xf2.R, normal1World); // Find support vertex on poly2 for -normal. int index = 0; float minDot = Common.Settings.FLT_MAX; for (int i = 0; i < count2; ++i) { float dot = Vec2.Dot(vertices2[i], normal1); if (dot < minDot) { minDot = dot; index = i; } } Vec2 v1 = Common.Math.Mul(xf1, vertices1[edge1]); Vec2 v2 = Common.Math.Mul(xf2, vertices2[index]); float separation = Vec2.Dot(v2 - v1, normal1World); return separation; }
public float T0; //time interval = [T0,1], where T0 is in [0,1] /// <summary> /// Get the interpolated transform at a specific time. /// </summary> /// <param name="alpha">Alpha is a factor in [0,1], where 0 indicates t0.</param> public void GetTransform(out XForm xf, float alpha) { xf = new XForm(); xf.Position = (1.0f - alpha) * C0 + alpha * C; float angle = (1.0f - alpha) * A0 + alpha * A; xf.R.Set(angle); // Shift to origin xf.Position -= Common.Math.Mul(xf.R, LocalCenter); }
public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2) { aabb = default(AABB); Vec2 a = transform1.Position + Box2DX.Common.Math.Mul(transform1.R, this._localPosition); Vec2 b = transform2.Position + Box2DX.Common.Math.Mul(transform2.R, this._localPosition); Vec2 vec = Box2DX.Common.Math.Min(a, b); Vec2 vec2 = Box2DX.Common.Math.Max(a, b); aabb.LowerBound.Set(vec.X - this._radius, vec.Y - this._radius); aabb.UpperBound.Set(vec2.X + this._radius, vec2.Y + this._radius); }
/// <summary> /// Build vertices to represent an oriented box. /// </summary> /// <param name="hx">The half-width</param> /// <param name="hy">The half-height.</param> /// <param name="center">The center of the box in local coordinates.</param> /// <param name="angle">The rotation of the box in local coordinates.</param> public void SetAsBox(float hx, float hy, Vec2 center, float angle) { SetAsBox(hx, hy); XForm xf = new XForm(); xf.Position = center; xf.R.Set(angle); for (int i = 0; i < VertexCount; ++i) { Vertices[i] = Common.Math.Mul(xf, Vertices[i]); } }
public void GetXForm(out XForm xf, float t) { xf = default(XForm); if (1f - this.T0 > Math.FLOAT32_EPSILON) { float num = (t - this.T0) / (1f - this.T0); xf.Position = (1f - num) * this.C0 + num * this.C; float angle = (1f - num) * this.A0 + num * this.A; xf.R.Set(angle); } else { xf.Position = this.C; xf.R.Set(this.A); } xf.Position -= Math.Mul(xf.R, this.LocalCenter); }
public static void CollideCircles(ref Manifold manifold, CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2) { manifold.PointCount = 0; Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition()); Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition()); Vec2 d = p2 - p1; float distSqr = Vec2.Dot(d, d); float r1 = circle1.GetRadius(); float r2 = circle2.GetRadius(); float radiusSum = r1 + r2; if (distSqr > radiusSum * radiusSum) { return; } float separation; if (distSqr < Common.Settings.FLT_EPSILON) { separation = -radiusSum; manifold.Normal.Set(0.0f, 1.0f); } else { float dist = Common.Math.Sqrt(distSqr); separation = dist - radiusSum; float a = 1.0f / dist; manifold.Normal.X = a * d.X; manifold.Normal.Y = a * d.Y; } manifold.PointCount = 1; manifold.Points[0].ID.Key = 0; manifold.Points[0].Separation = separation; p1 += r1 * manifold.Normal; p2 -= r2 * manifold.Normal; Vec2 p = 0.5f * (p1 + p2); manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, p); manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, p); }
// Collision Detection in Interactive 3D Environments by Gino van den Bergen // From Section 3.1.2 // x = s + a * r // norm(x) = radius public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda) { lambda = 0f; normal = Vec2.Zero; Vec2 position = transform.Position + Common.Math.Mul(transform.R, _localPosition); Vec2 s = segment.P1 - position; float b = Vec2.Dot(s, s) - _radius * _radius; // Does the segment start inside the circle? if (b < 0.0f) { lambda = 0f; return SegmentCollide.StartInsideCollide; } // Solve quadratic equation. Vec2 r = segment.P2 - segment.P1; float c = Vec2.Dot(s, r); float rr = Vec2.Dot(r, r); float sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < 0.0f || rr < Common.Settings.FLT_EPSILON) { return SegmentCollide.MissCollide; } // Find the point of intersection of the line with the circle. float a = -(c + Common.Math.Sqrt(sigma)); // Is the intersection point on the segment? if (0.0f <= a && a <= maxLambda * rr) { a /= rr; lambda = a; normal = s + a * r; normal.Normalize(); return SegmentCollide.HitCollide; } return SegmentCollide.MissCollide; }
public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda) { lambda = 0f; normal = Vec2.Zero; Vec2 v = transform.Position + Box2DX.Common.Math.Mul(transform.R, this._localPosition); Vec2 vec = segment.P1 - v; float num = Vec2.Dot(vec, vec) - this._radius * this._radius; SegmentCollide result; if (num < 0f) { lambda = 0f; result = SegmentCollide.StartInsideCollide; } else { Vec2 vec2 = segment.P2 - segment.P1; float num2 = Vec2.Dot(vec, vec2); float num3 = Vec2.Dot(vec2, vec2); float num4 = num2 * num2 - num3 * num; if (num4 < 0f || num3 < Settings.FLT_EPSILON) { result = SegmentCollide.MissCollide; } else { float num5 = -(num2 + Box2DX.Common.Math.Sqrt(num4)); if (0f <= num5 && num5 <= maxLambda * num3) { num5 /= num3; lambda = num5; normal = vec + num5 * vec2; normal.Normalize(); result = SegmentCollide.HitCollide; } else { result = SegmentCollide.MissCollide; } } } return result; }
/// <summary> /// Get the interpolated transform at a specific time. /// </summary> /// <param name="t">The normalized time in [0,1].</param> public void GetXForm(out XForm xf, float t) { xf = new XForm(); // center = p + R * LocalCenter if (1.0f - T0 > Math.FLOAT32_EPSILON) { float alpha = (t - T0) / (1.0f - T0); xf.Position = (1.0f - alpha) * C0 + alpha * C; float angle = (1.0f - alpha) * A0 + alpha * A; xf.R.Set(angle); } else { xf.Position = C; xf.R.Set(A); } // Shift to origin xf.Position -= Math.Mul(xf.R, LocalCenter); }
public static void CollideCircles(ref Manifold manifold, CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2) { manifold.PointCount = 0; Vec2 p1 = Common.Math.Mul(xf1, circle1._position); Vec2 p2 = Common.Math.Mul(xf2, circle2._position); Vec2 d = p2 - p1; float distSqr = Vec2.Dot(d, d); float radius = circle1._radius + circle2._radius; if (distSqr > radius * radius) { return; } manifold.Type = ManifoldType.Circles; manifold.LocalPoint = circle1._position; manifold.LocalPlaneNormal.SetZero(); manifold.PointCount = 1; manifold.Points[0].LocalPoint = circle2._position; manifold.Points[0].ID.Key = 0; }
internal void RefilterProxy(BroadPhase broadPhase, XForm transform) { if (this._proxyId != PairManager.NullProxy) { broadPhase.DestroyProxy((int)this._proxyId); AABB aabb; this.ComputeAABB(out aabb, transform); bool flag = broadPhase.InRange(aabb); if (flag) { this._proxyId = broadPhase.CreateProxy(aabb, this); } else { this._proxyId = PairManager.NullProxy; } } }
internal void CreateProxy(BroadPhase broadPhase, XForm transform) { Box2DXDebug.Assert(this._proxyId == PairManager.NullProxy); AABB aabb; this.ComputeAABB(out aabb, transform); bool flag = broadPhase.InRange(aabb); Box2DXDebug.Assert(flag); if (flag) { this._proxyId = broadPhase.CreateProxy(aabb, this); } else { this._proxyId = PairManager.NullProxy; } }
public override void ComputeAABB(out AABB aabb, XForm xf) { Mat22 R = Common.Math.Mul(xf.R, _obb.R); Mat22 absR = Common.Math.Abs(R); Vec2 h = Common.Math.Mul(absR, _obb.Extents); Vec2 position = xf.Position + Common.Math.Mul(xf.R, _obb.Center); aabb.LowerBound = position - h; aabb.UpperBound = position + h; }
public override bool TestPoint(XForm xf, Vec2 p) { Vec2 pLocal = Common.Math.MulT(xf.R, p - xf.Position); for (int i = 0; i < _vertexCount; ++i) { float dot = Vec2.Dot(_normals[i], pLocal - _vertices[i]); if (dot > 0.0f) { return false; } } return true; }
/// <summary> /// Get the centroid and apply the supplied transform. /// </summary> public Vec2 Centroid(XForm xf) { return Common.Math.Mul(xf, _centroid); }
private static void CollideCircles(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) { Collision.Collision.CollideCircles(ref manifold, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2); }
internal bool Synchronize(BroadPhase broadPhase, XForm transform1, XForm transform2) { bool result; if (this._proxyId == PairManager.NullProxy) { result = false; } else { AABB aabb; this.ComputeSweptAABB(out aabb, transform1, transform2); if (broadPhase.InRange(aabb)) { broadPhase.MoveProxy((int)this._proxyId, aabb); result = true; } else { result = false; } } return result; }
internal void ComputeXForm(ref XForm xf, Vec2 center, Vec2 localCenter, float angle) { xf.R.Set(angle); xf.Position = center - Box2DX.Common.Math.Mul(xf.R, localCenter); }
public override void ComputeAABB(out AABB aabb, XForm transform) { aabb = new AABB(); Vec2 p = transform.Position + Common.Math.Mul(transform.R, _localPosition); aabb.LowerBound.Set(p.X - _radius, p.Y - _radius); aabb.UpperBound.Set(p.X + _radius, p.Y + _radius); }
/// <summary> /// Get the first vertex and apply the supplied transform. /// </summary> public Vec2 GetFirstVertex(XForm xf) { return Common.Math.Mul(xf, _coreVertices[0]); }
/// Evaluate the manifold with supplied transforms. This assumes /// modest motion from the original state. This does not change the /// point count, impulses, etc. The radii must come from the shapes /// that generated the manifold. public void Initialize(Manifold manifold, XForm xfA, float radiusA, XForm xfB, float radiusB) { if (manifold.PointCount == 0) { return; } switch (manifold.Type) { case ManifoldType.Circles: { Vec2 pointA = Common.Math.Mul(xfA, manifold.LocalPoint); Vec2 pointB = Common.Math.Mul(xfB, manifold.Points[0].LocalPoint); Vec2 normal = new Vec2(1.0f, 0.0f); if (Vec2.DistanceSquared(pointA, pointB) > Common.Settings.FLT_EPSILON_SQUARED) { normal = pointB - pointA; normal.Normalize(); } Normal = normal; Vec2 cA = pointA + radiusA * normal; Vec2 cB = pointB - radiusB * normal; Points[0] = 0.5f * (cA + cB); } break; case ManifoldType.FaceA: { Vec2 normal = Common.Math.Mul(xfA.R, manifold.LocalPlaneNormal); Vec2 planePoint = Common.Math.Mul(xfA, manifold.LocalPoint); // Ensure normal points from A to B. Normal = normal; for (int i = 0; i < manifold.PointCount; ++i) { Vec2 clipPoint = Common.Math.Mul(xfB, manifold.Points[i].LocalPoint); Vec2 cA = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal; Vec2 cB = clipPoint - radiusB * normal; Points[i] = 0.5f * (cA + cB); } } break; case ManifoldType.FaceB: { Vec2 normal = Common.Math.Mul(xfB.R, manifold.LocalPlaneNormal); Vec2 planePoint = Common.Math.Mul(xfB, manifold.LocalPoint); // Ensure normal points from A to B. Normal = -normal; for (int i = 0; i < manifold.PointCount; ++i) { Vec2 clipPoint = Common.Math.Mul(xfA, manifold.Points[i].LocalPoint); Vec2 cA = clipPoint - radiusA * normal; Vec2 cB = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal; Points[i] = 0.5f * (cA + cB); } } break; } }
/// <summary> /// Get the support point in the given world direction. /// Use the supplied transform. /// </summary> public Vec2 Support(XForm xf, Vec2 d) { Vec2 dLocal = Common.Math.MulT(xf.R, d); int bestIndex = 0; float bestValue = Vec2.Dot(_coreVertices[0], dLocal); for (int i = 1; i < _vertexCount; ++i) { float value = Vec2.Dot(_coreVertices[i], dLocal); if (value > bestValue) { bestIndex = i; bestValue = value; } } return Common.Math.Mul(xf, _coreVertices[bestIndex]); }
public abstract void ComputeAABB(out AABB aabb, XForm xf);
public override SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda) { lambda = 0f; normal = Vec2.Zero; float lower = 0.0f, upper = maxLambda; Vec2 p1 = Common.Math.MulT(xf.R, segment.P1 - xf.Position); Vec2 p2 = Common.Math.MulT(xf.R, segment.P2 - xf.Position); Vec2 d = p2 - p1; int index = -1; for (int i = 0; i < _vertexCount; ++i) { // p = p1 + a * d // dot(normal, p - v) = 0 // dot(normal, p1 - v) + a * dot(normal, d) = 0 float numerator = Vec2.Dot(_normals[i], _vertices[i] - p1); float denominator = Vec2.Dot(_normals[i], d); if (denominator == 0.0f) { if (numerator < 0.0f) { return SegmentCollide.MissCollide; } } else { // Note: we want this predicate without division: // lower < numerator / denominator, where denominator < 0 // Since denominator < 0, we have to flip the inequality: // lower < numerator / denominator <==> denominator * lower > numerator. if (denominator < 0.0f && numerator < lower * denominator) { // Increase lower. // The segment enters this half-space. lower = numerator / denominator; index = i; } else if (denominator > 0.0f && numerator < upper * denominator) { // Decrease upper. // The segment exits this half-space. upper = numerator / denominator; } } if (upper < lower) { return SegmentCollide.MissCollide; } } Box2DXDebug.Assert(0.0f <= lower && lower <= maxLambda); if (index >= 0) { lambda = lower; normal = Common.Math.Mul(xf.R, _normals[index]); return SegmentCollide.HitCollide; } lambda = 0f; return SegmentCollide.StartInsideCollide; }
public abstract void ComputeSweptAABB(out AABB aabb, XForm xf1, XForm xf2);
public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2) { AABB aabb1, aabb2; ComputeAABB(out aabb1, transform1); ComputeAABB(out aabb2, transform2); aabb.LowerBound = Common.Math.Min(aabb1.LowerBound, aabb2.LowerBound); aabb.UpperBound = Common.Math.Max(aabb1.UpperBound, aabb2.UpperBound); }
public abstract bool TestPoint(XForm xf, Vec2 p);
/// <summary> /// Draw a transform. Choose your own length scale. /// </summary> /// <param name="xf">A transform.</param> public abstract void DrawXForm(XForm xf);
public abstract SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda);