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