internal void Initialize(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB) { ShapeA = shapeA; ShapeB = shapeB; int count = cache.Count; if (count == 1) { FaceType = Type.Points; Vector2 localPointA = ShapeA.GetVertex(cache.IndexA[0]); Vector2 localPointB = ShapeB.GetVertex(cache.IndexB[0]); Vector2 pointA = transformA.TransformPoint(localPointA); Vector2 pointB = transformB.TransformPoint(localPointB); Axis = pointB - pointA; Axis.Normalize(); } else if (cache.IndexB[0] == cache.IndexB[1]) { // Two points on A and one on B FaceType = Type.FaceA; Vector2 localPointA1 = ShapeA.GetVertex(cache.IndexA[0]); Vector2 localPointA2 = ShapeA.GetVertex(cache.IndexA[1]); Vector2 localPointB = ShapeB.GetVertex(cache.IndexB[0]); LocalPoint = 0.5f * (localPointA1 + localPointA2); Axis = (localPointA2 - localPointA1).CrossScalarPostMultiply(1.0f); Axis.Normalize(); Vector2 normal = transformA.TransformDirection(Axis); Vector2 pointA = transformA.TransformPoint(LocalPoint); Vector2 pointB = transformB.TransformPoint(localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { Axis = -Axis; } } else { // Two points on B and one or two points on A. // We ignore the second point on A. FaceType = Type.FaceB; Vector2 localPointA = shapeA.GetVertex(cache.IndexA[0]); Vector2 localPointB1 = shapeB.GetVertex(cache.IndexB[0]); Vector2 localPointB2 = shapeB.GetVertex(cache.IndexB[1]); LocalPoint = 0.5f * (localPointB1 + localPointB2); Axis = (localPointB2 - localPointB1).CrossScalarPostMultiply(1.0f); Axis.Normalize(); Vector2 normal = transformB.TransformDirection(Axis); Vector2 pointB = transformB.TransformPoint(LocalPoint); Vector2 pointA = transformA.TransformPoint(localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { Axis = -Axis; } } }
public override bool TestPoint(Transform xf, Vector2 p) { Vector2 center = xf.position + xf.TransformDirection(_position); Vector2 d = p - center; return(Vector2.Dot(d, d) <= _radius * _radius); }
internal bool SynchronizeFixtures() { Transform xf1 = new Transform(); xf1.rotation = Box2DNet.Common.Math.AngleToRotation(_sweep.A0); //xf1.R = new Mat22(_sweep.A0); xf1.position = _sweep.C0 - xf1.TransformDirection(_sweep.LocalCenter); bool inRange = true; for (Fixture f = _fixtureList; f != null; f = f.Next) { inRange = f.Synchronize(_world._broadPhase, xf1, _xf); if (inRange == false) { break; } } if (inRange == false) { _flags |= BodyFlags.Frozen; _linearVelocity = Vector2.Zero; _angularVelocity = 0.0f; // Failure return(false); } // Success return(true); }
public override void ComputeAABB(out AABB aabb, Transform xf) { aabb = new AABB(); Vector2 p = xf.position + xf.TransformDirection(_position); aabb.LowerBound = new Vector2(p.X - _radius, p.Y - _radius); aabb.UpperBound = new Vector2(p.X + _radius, p.Y + _radius); }
internal float Evaluate(Transform TransformA, Transform TransformB) { switch (FaceType) { case Type.Points: { Vector2 axisA = TransformA.InverseTransformDirection(Axis); Vector2 axisB = TransformB.InverseTransformDirection(-Axis); Vector2 localPointA = ShapeA.GetSupportVertex(axisA); Vector2 localPointB = ShapeB.GetSupportVertex(axisB); Vector2 pointA = TransformA.TransformPoint(localPointA); Vector2 pointB = TransformB.TransformPoint(localPointB); float separation = Vector2.Dot(pointB - pointA, Axis); return(separation); } case Type.FaceA: { Vector2 normal = TransformA.TransformDirection(Axis); Vector2 pointA = TransformA.TransformPoint(LocalPoint); Vector2 axisB = TransformB.InverseTransformDirection(-normal); Vector2 localPointB = ShapeB.GetSupportVertex(axisB); Vector2 pointB = TransformB.TransformPoint(localPointB); float separation = Vector2.Dot(pointB - pointA, normal); return(separation); } case Type.FaceB: { Vector2 normal = TransformB.TransformDirection(Axis); Vector2 pointB = TransformB.TransformPoint(LocalPoint); Vector2 axisA = TransformA.InverseTransformDirection(-normal); Vector2 localPointA = ShapeA.GetSupportVertex(axisA); Vector2 pointA = TransformA.TransformPoint(localPointA); float separation = Vector2.Dot(pointA - pointB, normal); return(separation); } default: Box2DNetDebug.Assert(false); return(0.0f); } }
public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) { int count1 = poly1._vertexCount; Vector2[] normals1 = poly1._normals; int count2 = poly2._vertexCount; Vector2[] vertices2 = poly2._vertices; Vector2[] normals2 = poly2._normals; Box2DNetDebug.Assert(0 <= edge1 && edge1 < count1); // Get the normal of the reference edge in poly2's frame. Vector2 normal1 = xf2.InverseTransformDirection(xf1.TransformDirection(normals1[edge1])); // Find the incident edge on poly2. int index = 0; float minDot = Settings.FLT_MAX; for (int i = 0; i < count2; ++i) { float dot = Vector2.Dot(normal1, normals2[i]); if (dot < minDot) { minDot = dot; index = i; } } // Build the clip vertices for the incident edge. int i1 = index; int i2 = i1 + 1 < count2 ? i1 + 1 : 0; c = new ClipVertex[2]; c[0].V = Common.Math.Mul(xf2, vertices2[i1]); c[0].ID.Features.ReferenceEdge = (byte)edge1; c[0].ID.Features.IncidentEdge = (byte)i1; c[0].ID.Features.IncidentVertex = 0; c[1].V = Common.Math.Mul(xf2, vertices2[i2]); c[1].ID.Features.ReferenceEdge = (byte)edge1; c[1].ID.Features.IncidentEdge = (byte)i2; c[1].ID.Features.IncidentVertex = 1; }
// 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(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda) { lambda = 0f; normal = Vector2.Zero; Vector2 position = xf.position + xf.TransformDirection(_position); Vector2 s = segment.P1 - position; float b = Vector2.Dot(s, s) - _radius * _radius; // Does the segment start inside the circle? if (b < 0.0f) { lambda = 0f; return(SegmentCollide.StartInsideCollide); } // Solve quadratic equation. Vector2 r = segment.P2 - segment.P1; float c = Vector2.Dot(s, r); float rr = Vector2.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); }
/// <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, Vector2 center, float angle) { SetAsBox(hx, hy); Transform xf = new Transform(); xf.position = center; xf.rotation = Box2DNet.Common.Math.AngleToRotation(angle); // xf.R = new Mat22(angle); //Debug.Log(string.Format("xf.position = ({0},{1}) xf.rotation = ({2},{3},{4},{5})", xf.position.x, xf.position.y, xf.rotation.x, xf.rotation.y, xf.rotation.z, xf.rotation.w)); // Transform vertices and normals. for (int i = 0; i < _vertexCount; ++i) { _vertices[i] = xf.TransformPoint(_vertices[i]); _normals[i] = xf.TransformDirection(_normals[i]); } }
/// <summary> /// Find the separation between poly1 and poly2 for a give edge normal on poly1. /// </summary> public static float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) { int count1 = poly1._vertexCount; Vector2[] vertices1 = poly1._vertices; Vector2[] normals1 = poly1._normals; int count2 = poly2._vertexCount; Vector2[] vertices2 = poly2._vertices; //This fixed it, not sure why it was broken if (normals1.Length - 1 <= edge1 || edge1 < 0) { return(0); } // Convert normal from poly1's frame into poly2's frame. Vector2 normal1World = xf1.TransformDirection(normals1[edge1]); Vector2 normal1 = xf2.InverseTransformDirection(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 = Vector2.Dot(vertices2[i], normal1); if (dot < minDot) { minDot = dot; index = i; } } Vector2 v1 = xf1.TransformPoint(vertices1[edge1]); Vector2 v2 = xf2.TransformPoint(vertices2[index]); float separation = Vector2.Dot(v2 - v1, normal1World); return(separation); }
internal override void InitVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; _localCenter1 = b1.GetLocalCenter(); _localCenter2 = b2.GetLocalCenter(); Transform xf1 = b1.GetTransform(); Transform xf2 = b2.GetTransform(); // Compute the effective masses. Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1); Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2); Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1; _invMass1 = b1._invMass; _invI1 = b1._invI; _invMass2 = b2._invMass; _invI2 = b2._invI; // Compute motor Jacobian and effective mass. { _axis = xf1.TransformDirection(_localXAxis1); _a1 = (d + r1).Cross(_axis); _a2 = r2.Cross(_axis); _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2; _motorMass = 1.0f / _motorMass; } // Prismatic constraint. { _perp = xf1.TransformDirection(_localYAxis1); _s1 = (d + r1).Cross(_perp); _s2 = r2.Cross(_perp); float m1 = _invMass1, m2 = _invMass2; float i1 = _invI1, i2 = _invI2; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2; float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; _K.Col1 = new Vector2(k11, k12); _K.Col2 = new Vector2(k12, k22); } // Compute motor and limit terms. if (_enableLimit) { float jointTranslation = Vector2.Dot(_axis, d); if (Box2DNet.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { _limitState = LimitState.EqualLimits; } else if (jointTranslation <= _lowerTranslation) { if (_limitState != LimitState.AtLowerLimit) { _limitState = LimitState.AtLowerLimit; _impulse.Y = 0.0f; } } else if (jointTranslation >= _upperTranslation) { if (_limitState != LimitState.AtUpperLimit) { _limitState = LimitState.AtUpperLimit; _impulse.Y = 0.0f; } } else { _limitState = LimitState.InactiveLimit; _impulse.Y = 0.0f; } } else { _limitState = LimitState.InactiveLimit; } if (_enableMotor == false) { _motorImpulse = 0.0f; } if (step.WarmStarting) { // Account for variable time step. _impulse *= step.DtRatio; _motorImpulse *= step.DtRatio; Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis; float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1; float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2; b1._linearVelocity -= _invMass1 * P; b1._angularVelocity -= _invI1 * L1; b2._linearVelocity += _invMass2 * P; b2._angularVelocity += _invI2 * L2; } else { _impulse = Vector2.Zero; _motorImpulse = 0.0f; } }
public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda) { lambda = 0f; normal = Vector2.Zero; float lower = 0.0f, upper = maxLambda; Vector2 p1 = xf.InverseTransformDirection(segment.P1 - xf.position); Vector2 p2 = xf.InverseTransformDirection(segment.P2 - xf.position); Vector2 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 = Vector2.Dot(_normals[i], _vertices[i] - p1); float denominator = Vector2.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); } } if (index >= 0) { lambda = lower; normal = xf.TransformDirection(_normals[index]); return(SegmentCollide.HitCollide); } lambda = 0f; return(SegmentCollide.StartInsideCollide); }
internal void ComputeTransform(ref Transform xf, Vector2 center, Vector2 localCenter, float angle) { xf.rotation = Box2DNet.Common.Math.AngleToRotation(angle); //xf.R = new Mat22(angle); xf.position = center - xf.TransformDirection(localCenter); }
/// 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, Transform xfA, float radiusA, Transform xfB, float radiusB) { if (manifold.PointCount == 0) { return; } switch (manifold.Type) { case ManifoldType.Circles: { Vector2 pointA = xfA.TransformPoint(manifold.LocalPoint); Vector2 pointB = xfB.TransformPoint(manifold.Points[0].LocalPoint); Vector2 normal = new Vector2(1.0f, 0.0f); if ((pointA - pointB).LengthSquared() > (Box2DNet.Common.Math.Epsilon * Box2DNet.Common.Math.Epsilon)) { normal = pointB - pointA; normal.Normalize(); } Normal = normal; Vector2 cA = pointA + radiusA * normal; Vector2 cB = pointB - radiusB * normal; Points[0] = 0.5f * (cA + cB); } break; case ManifoldType.FaceA: { Vector2 normal = xfA.TransformDirection(manifold.LocalPlaneNormal); Vector2 planePoint = xfA.TransformPoint(manifold.LocalPoint); // Ensure normal points from A to B. Normal = normal; for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = xfB.TransformPoint(manifold.Points[i].LocalPoint); Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Vector2 cB = clipPoint - radiusB * normal; Points[i] = 0.5f * (cA + cB); } } break; case ManifoldType.FaceB: { Vector2 normal = xfB.TransformDirection(manifold.LocalPlaneNormal); Vector2 planePoint = xfB.TransformPoint(manifold.LocalPoint); // Ensure normal points from A to B. Normal = -normal; for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = xfA.TransformPoint(manifold.Points[i].LocalPoint); Vector2 cA = clipPoint - radiusA * normal; Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Points[i] = 0.5f * (cA + cB); } } break; } }
/// <summary> /// Get the world coordinates of a vector given the local coordinates. /// </summary> /// <param name="localVector">A vector fixed in the body.</param> /// <returns>Return the same vector expressed in world coordinates.</returns> public Vector2 GetWorldVector(Vector2 localVector) { return(_xf.TransformDirection(localVector)); }