public void Set(Vec2 v1, Vec2 v2) { _v1 = v1; _v2 = v2; _direction = _v2 - _v1; _length = _direction.Normalize(); _normal = Vec2.Cross(_direction, 1.0f); _cornerDir1 = _normal; _cornerDir2 = -1.0f * _normal; }
public LineJoint() { Body ground = null; { PolygonDef sd = new PolygonDef(); sd.SetAsBox(50.0f, 10.0f); BodyDef bd = new BodyDef(); bd.Position.Set(0.0f, -10.0f); ground = _world.CreateBody(bd); ground.CreateShape(sd); } { PolygonDef sd = new PolygonDef(); sd.SetAsBox(0.5f, 2.0f); sd.Density = 1.0f; BodyDef bd = new BodyDef(); bd.Position.Set(0.0f, 7.0f); Body body = _world.CreateBody(bd); body.CreateShape(sd); body.SetMassFromShapes(); LineJointDef jd = new LineJointDef(); Vec2 axis = new Vec2(2.0f, 1.0f); axis.Normalize(); jd.Initialize(ground, body, new Vec2(0.0f, 8.5f), axis); jd.motorSpeed = 0.0f; jd.maxMotorForce = 100.0f; jd.enableMotor = true; jd.lowerTranslation = -4.0f; jd.upperTranslation = 4.0f; jd.enableLimit = true; _world.CreateJoint(jd); } }
/// 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 Manifold.ManifoldType.Circles: { Vec2 pointA = Math.Mul(xfA, manifold.LocalPoint); Vec2 pointB = Math.Mul(xfB, manifold.Points[0].LocalPoint); Vec2 normal = new Vec2(1.0f, 0.0f); if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON * Settings.FLT_EPSILON) { 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 Manifold.ManifoldType.FaceA: { Vec2 normal = Math.Mul(xfA.R, manifold.LocalPlaneNormal); Vec2 planePoint = Math.Mul(xfA, manifold.LocalPoint); // Ensure normal points from A to B. Normal = normal; for (int i = 0; i < manifold.PointCount; ++i) { Vec2 clipPoint = 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 Manifold.ManifoldType.FaceB: { Vec2 normal = Math.Mul(xfB.R, manifold.LocalPlaneNormal); Vec2 planePoint = Math.Mul(xfB, manifold.LocalPoint); // Ensure normal points from A to B. Normal = -normal; for (int i = 0; i < manifold.PointCount; ++i) { Vec2 clipPoint = 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; } }
// 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; }
internal void Initialize(ContactConstraint cc) { Box2DXDebug.Assert(cc.PointCount > 0); switch (cc.Type) { case ManifoldType.Circles: { Vec2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint); Vec2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint); if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON_SQUARED) { Normal = pointB - pointA; Normal.Normalize(); } else { Normal.Set(1.0f, 0.0f); } Points[0] = 0.5f * (pointA + pointB); Separations[0] = Vec2.Dot(pointB - pointA, Normal) - cc.Radius; } break; case ManifoldType.FaceA: { Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal); Vec2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint); for (int i = 0; i < cc.PointCount; ++i) { Vec2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint); Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Points[i] = clipPoint; } } break; case ManifoldType.FaceB: { Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal); Vec2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint); for (int i = 0; i < cc.PointCount; ++i) { Vec2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint); Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Points[i] = clipPoint; } // Ensure normal points from A to B Normal = -Normal; } break; } }
public override void Step(Settings settings) { base.Step(settings); float segmentLength = 30.0f; Segment segment; Vec2 laserStart = new Vec2(5.0f - 0.1f, 0.0f); Vec2 laserDir = new Vec2(segmentLength, 0.0f); segment.P1 = laserBody.GetWorldPoint(laserStart); segment.P2 = laserBody.GetWorldVector(laserDir); segment.P2 += segment.P1; for (int rebounds = 0; rebounds < 10; rebounds++) { float lambda = 1; Vec2 normal; Shape shape = _world.RaycastOne(segment, out lambda, out normal, false, null); Color laserColor = new Color(255, 0, 0); if (shape != null) { _debugDraw.DrawSegment(segment.P1, (1 - lambda) * segment.P1 + lambda * segment.P2, laserColor); } else { _debugDraw.DrawSegment(segment.P1, segment.P2, laserColor); break; } //Bounce segmentLength *= (1 - lambda); if (segmentLength <= Box2DX.Common.Settings.FLT_EPSILON) break; laserStart = (1 - lambda) * segment.P1 + lambda * segment.P2; laserDir = segment.P2 - segment.P1; laserDir.Normalize(); laserDir = laserDir - 2 * Vec2.Dot(laserDir, normal) * normal; segment.P1 = laserStart - 0.1f * laserDir; segment.P2 = laserStart + segmentLength * laserDir; } }