internal PrismaticJoint(PrismaticJointDef def)
: base(def)
{
LocalAnchorA = def.LocalAnchorA;
LocalAnchorB = def.LocalAnchorB;
LocalXAxisA = def.LocalAxisA;
LocalXAxisA.Normalize();
LocalYAxisA = MathUtils.Cross(1.0f, LocalXAxisA);
ReferenceAngle = def.ReferenceAngle;
_impulse.SetZero();
_axialMass = 0.0f;
_motorImpulse = 0.0f;
_lowerImpulse = 0.0f;
_upperImpulse = 0.0f;
_lowerTranslation = def.LowerTranslation;
_upperTranslation = def.UpperTranslation;
Debug.Assert(_lowerTranslation <= _upperTranslation);
_maxMotorForce = def.MaxMotorForce;
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_translation = 0.0f;
_axis.SetZero();
_perp.SetZero();
}
private void DrawWorld()
{
World.DrawDebugData();
if (TestSettings.ContactPoint)
{
const float impulseScale = 0.1f;
const float axisScale = 0.3f;
var debugDraw = TestSettings.WorldDrawer;
for (var i = 0; i < _pointsCount; ++i)
{
var point = _points[i];
if (point.State == PointState.AddState)
{
// Add
debugDraw.DrawPoint(point.Position, 10f, Color.FromArgb(77, 242, 77));
}
else if (point.State == PointState.PersistState)
{
// Persist
debugDraw.DrawPoint(point.Position, 5f, Color.FromArgb(77, 77, 242));
}
if (TestSettings.ContactNormals)
{
var p1 = point.Position;
var p2 = p1 + axisScale * point.Normal;
debugDraw.DrawSegment(p1, p2, Color.FromArgb(230, 230, 230));
}
else if (TestSettings.ContactImpulse)
{
var p1 = point.Position;
var p2 = p1 + impulseScale * point.NormalImpulse * point.Normal;
debugDraw.DrawSegment(p1, p2, Color.FromArgb(230, 230, 77));
}
if (TestSettings.FrictionImpulse)
{
var tangent = MathUtils.Cross(point.Normal, 1.0f);
var p1 = point.Position;
var p2 = p1 + impulseScale * point.TangentImpulse * tangent;
debugDraw.DrawSegment(p1, p2, Color.FromArgb(230, 230, 77));
}
}
}
if (BombSpawning)
{
Drawer.DrawPoint(BombSpawnPoint, 4.0f, Color.Blue);
Drawer.DrawSegment(MouseWorld, BombSpawnPoint, Color.FromArgb(203, 203, 203));
}
}
private void DrawWorld()
{
World.DrawDebugData();
if (TestSettings.ContactPoint)
{
const float impulseScale = 0.1f;
const float axisScale = 0.3f;
var debugDraw = TestSettings.WorldDrawer;
for (var i = 0; i < _pointsCount; ++i)
{
var point = _points[i];
if (point.State == PointState.AddState)
{
// Add
debugDraw.DrawPoint(point.Position, 10f, _c1);
}
else if (point.State == PointState.PersistState)
{
// Persist
debugDraw.DrawPoint(point.Position, 5f, _c2);
}
if (TestSettings.ContactNormals)
{
var p1 = point.Position;
var p2 = p1 + axisScale * point.Normal;
debugDraw.DrawSegment(p1, p2, _c3);
}
else if (TestSettings.ContactImpulse)
{
var p1 = point.Position;
var p2 = p1 + impulseScale * point.NormalImpulse * point.Normal;
debugDraw.DrawSegment(p1, p2, _c4);
}
if (TestSettings.FrictionImpulse)
{
var tangent = MathUtils.Cross(point.Normal, 1.0f);
var p1 = point.Position;
var p2 = p1 + impulseScale * point.TangentImpulse * tangent;
debugDraw.DrawSegment(p1, p2, _c4);
}
}
}
}
/// Get the current joint translation speed, usually in meters per second.
public float GetJointSpeed()
{
var bA = BodyA;
var bB = BodyB;
var rA = MathUtils.Mul(bA.Transform.Rotation, LocalAnchorA - bA.Sweep.LocalCenter);
var rB = MathUtils.Mul(bB.Transform.Rotation, LocalAnchorB - bB.Sweep.LocalCenter);
var p1 = bA.Sweep.C + rA;
var p2 = bB.Sweep.C + rB;
var d = p2 - p1;
var axis = MathUtils.Mul(bA.Transform.Rotation, LocalXAxisA);
var vA = bA.LinearVelocity;
var vB = bB.LinearVelocity;
var wA = bA.AngularVelocity;
var wB = bB.AngularVelocity;
var speed = Vector2.Dot(d, MathUtils.Cross(wA, axis))
+ Vector2.Dot(axis, vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA));
return(speed);
}