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;
}
}
