/// <summary>
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
/// This requires two ground anchors,
/// two dynamic body anchor points, max lengths for each side,
/// and a pulley ratio.
/// </summary>
/// <param name="bA">The first body.</param>
/// <param name="bB">The second body.</param>
/// <param name="groundA">The ground anchor for the first body.</param>
/// <param name="groundB">The ground anchor for the second body.</param>
/// <param name="anchorA">The first body anchor.</param>
/// <param name="anchorB">The second body anchor.</param>
/// <param name="ratio">The ratio.</param>
public FSPulleyJoint(FSBody bA, FSBody bB, FVector2 groundA, FVector2 groundB, FVector2 anchorA, FVector2 anchorB, float ratio)
: base(bA, bB)
{
JointType = JointType.Pulley;
GroundAnchorA = groundA;
GroundAnchorB = groundB;
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
Debug.Assert(ratio != 0.0f);
Debug.Assert(ratio > FSSettings.Epsilon);
Ratio = ratio;
FVector2 dA = BodyA.GetWorldPoint(anchorA) - groundA;
LengthA = dA.Length();
FVector2 dB = BodyB.GetWorldPoint(anchorB) - groundB;
LengthB = dB.Length();
m_constant = LengthA + ratio * LengthB;
_impulse = 0.0f;
}
/// <summary>Initializes this joint with the specified parameters.</summary>
internal void Initialize(
WorldPoint anchor,
Vector2 axis,
float frequency,
float dampingRatio,
float maxMotorTorque,
float motorSpeed,
bool enableMotor)
{
_localAnchorA = BodyA.GetLocalPoint(anchor);
_localAnchorB = BodyB.GetLocalPoint(anchor);
_localXAxisA = BodyA.GetLocalVector(axis);
_localYAxisA = Vector2Util.Cross(1.0f, ref _localXAxisA);
_frequency = frequency;
_dampingRatio = dampingRatio;
_maxMotorTorque = maxMotorTorque;
_motorSpeed = motorSpeed;
_enableMotor = enableMotor;
_impulse = 0;
_motorImpulse = 0;
_springImpulse = 0;
}
public void ApplyForce()
{
//Note: The spring force has a direction which is given by the vector between BodyB and BodyA
// The magnitude of the spring force is caculated using the Hooke's law
//Vector between the two masses attached to the spring
Vector2D s_vec = BodyB.Position - BodyA.Position;
if (s_vec.Length() == 0)
{
return;
}
//Distance between the two masses, i.e. the length of the spring
float lengthDifference = s_vec.Length() - RestLength;
//Compute the spring force based on Hooke's law
float force = Stiffness * -1 * lengthDifference * Dampen;
//Apply the spring force to the two bodies joined by the spring
appliedForce = s_vec * (force / s_vec.Length());
BodyA.SetForce(BodyA.Force - appliedForce);
BodyB.SetForce(BodyB.Force + appliedForce);
}
/// <summary>
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
/// This requires two ground anchors,
/// two dynamic body anchor points, max lengths for each side,
/// and a pulley ratio.
/// </summary>
/// <param name="bA">The first body.</param>
/// <param name="bB">The second body.</param>
/// <param name="groundA">The ground anchor for the first body.</param>
/// <param name="groundB">The ground anchor for the second body.</param>
/// <param name="anchorA">The first body anchor.</param>
/// <param name="anchorB">The second body anchor.</param>
/// <param name="ratio">The ratio.</param>
public PulleyJoint(Body bA, Body bB, Vector2 groundA, Vector2 groundB, Vector2 anchorA, Vector2 anchorB, float ratio)
: base(bA, bB)
{
JointType = JointType.Pulley;
GroundAnchorA = groundA;
GroundAnchorB = groundB;
LocalAnchorA = BodyA.GetLocalPoint(anchorA);
LocalAnchorB = BodyB.GetLocalPoint(anchorB);
Debug.Assert(ratio != 0.0f);
Debug.Assert(ratio > Settings.Epsilon);
Ratio = ratio;
Vector2 dA = anchorA - groundA;
LengthA = dA.Length();
Vector2 dB = anchorB - groundB;
LengthB = dB.Length();
_constant = LengthA + ratio * LengthB;
_impulse = 0.0f;
}
/// <inheritdoc/>
protected override bool OnApplyImpulse()
{
// Compute relative velocity.
Vector3F vA = BodyA.GetVelocityOfWorldPoint(_anchorAWorld);
Vector3F vB = BodyB.GetVelocityOfWorldPoint(_anchorBWorld);
Vector3F relativeVelocity = (vB - vA);
// Compute constraint impulse.
Vector3F impulse = _kInverse * (_targetVelocity - relativeVelocity - Softness / _deltaTime * _constraintImpulse);
// Impulse accumulation and clamping.
Vector3F oldConstraintImpulse = _constraintImpulse;
_constraintImpulse += impulse;
float impulseMagnitude = _constraintImpulse.Length;
float maxImpulseMagnitude = MaxForce * _deltaTime;
if (impulseMagnitude > maxImpulseMagnitude)
{
_constraintImpulse = _constraintImpulse / impulseMagnitude * maxImpulseMagnitude;
impulse = _constraintImpulse - oldConstraintImpulse;
}
// Apply impulses
BodyA.ApplyImpulse(-impulse, _anchorAWorld);
BodyB.ApplyImpulse(impulse, _anchorBWorld);
return(impulse.LengthSquared > Simulation.Settings.Constraints.MinConstraintImpulseSquared);
}
/// <summary>Initializes this joint with the specified parameters.</summary>
internal void Initialize(
WorldPoint anchor,
Vector2 axis,
float lowerTranslation = 0,
float upperTranslation = 0,
float maxMotorForce = 0,
float motorSpeed = 0,
bool enableLimit = false,
bool enableMotor = false)
{
_localAnchorA = BodyA.GetLocalPoint(anchor);
_localAnchorB = BodyB.GetLocalPoint(anchor);
_localXAxisA = BodyA.GetLocalVector(axis);
_localXAxisA.Normalize();
_localYAxisA = Vector2Util.Cross(1.0f, ref _localXAxisA);
_referenceAngle = BodyB.Angle - BodyA.Angle;
_lowerTranslation = lowerTranslation;
_upperTranslation = upperTranslation;
_maxMotorForce = maxMotorForce;
_motorSpeed = motorSpeed;
_enableLimit = enableLimit;
_enableMotor = enableMotor;
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
_limitState = LimitState.Inactive;
}
/// <summary>
/// This requires defining an
/// anchor shipPosition on both bodies and the non-zero length of the
/// distance joint. If you don't supply a length, the local anchor vectors2
/// is used so that the initial configuration can violate the constraint
/// slightly. This helps when saving and loading a game.
/// Warning Do not use a zero or short length.
/// </summary>
/// <param slotName="bodyA">The first body</param>
/// <param slotName="bodyB">The second body</param>
/// <param slotName="anchorA">The first body anchor</param>
/// <param slotName="anchorB">The second body anchor</param>
/// <param slotName="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
///
public DistanceJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB)
{
JointType = JointType.Distance;
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
Length = (BodyB.GetWorldPoint(ref anchorB) - BodyA.GetWorldPoint(ref anchorA)).Length();
}
/// Get the current length of the segment attached to bodyB.
public float GetCurrentLengthB()
{
var p = BodyB.GetWorldPoint(_localAnchorB);
var s = _groundAnchorB;
var d = p - s;
return(d.Length());
}
protected override void Initialise2(IntPtr world)
{
Vector3 anchorA = transform.position - BodyA.transform.position;
Vector3 anchorB = transform.position - BodyB.transform.position;
LPAPIJoint.CreateWeldJoint(world, BodyA.GetComponent <LPBody>().GetPtr(), BodyB.GetComponent <LPBody>().GetPtr()
, anchorA.x, anchorA.y, anchorB.x, anchorB.y, CollideConnected);
}
/// <summary>Initializes this joint with the specified parameters.</summary>
internal void Initialize(WorldPoint anchorA, WorldPoint anchorB, float length)
{
_localAnchorA = BodyA.GetLocalPoint(anchorA);
_localAnchorB = BodyB.GetLocalPoint(anchorB);
_maxLength = length;
_impulse = 0;
_length = 0;
}
public float GetCurrentLength()
{
var pA = BodyA.GetWorldPoint(_localAnchorA);
var pB = BodyB.GetWorldPoint(_localAnchorB);
var d = pB - pA;
var length = d.Length();
return(length);
}
/// <summary>
/// This requires defining an
/// anchor point on both bodies and the non-zero length of the
/// distance joint. If you don't supply a length, the local anchor points
/// is used so that the initial configuration can violate the constraint
/// slightly. This helps when saving and loading a game.
/// Warning Do not use a zero or short length.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="anchorA">The first body anchor</param>
/// <param name="anchorB">The second body anchor</param>
public DistanceJoint(EntityUid bodyA, EntityUid bodyB, Vector2 anchorA, Vector2 anchorB)
: base(bodyA, bodyB)
{
Length = MathF.Max(PhysicsConstants.LinearSlop, (BodyB.GetWorldPoint(anchorB) - BodyA.GetWorldPoint(anchorA)).Length);
_minLength = _length;
_maxLength = _length;
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
}
public override void ApplyForce(float dt)
{
Vector2 force = SpringForce(BodyA.Position, BodyB.Position, RestLength, SpringPower);
// modifier modifies the amount of force to use, if there is a static object then apply 100% force to the objects if they are not static then split the force 50/50
float modifier = (BodyA.Type == BodyTypeEnumRef.eType.Static || BodyB.Type == BodyTypeEnumRef.eType.Static) ? 1.0f : 0.5f;
BodyA.ApplyForce(-force * modifier, PhysicsBody.eForceMode.IMPULSE);
BodyB.ApplyForce(force * modifier, PhysicsBody.eForceMode.IMPULSE);
}
public FixedLineJoint(Body body, Vector2 worldAnchor, Vector2 axis)
: base(body)
{
JointType = JointType.FixedLine;
BodyB = BodyA;
LocalAnchorA = worldAnchor;
LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
LocalXAxis = axis;
}
/// <summary>Initializes this joint with the specified parameters.</summary>
internal void Initialize(WorldPoint anchor, float frequency, float dampingRatio)
{
_localAnchorA = BodyA.GetLocalPoint(anchor);
_localAnchorB = BodyB.GetLocalPoint(anchor);
_referenceAngle = BodyB.Angle - BodyA.Angle;
_frequency = frequency;
_dampingRatio = dampingRatio;
_impulse = Vector3.Zero;
}
/// Get the current joint translation, usually in meters.
public float GetJointTranslation()
{
var pA = BodyA.GetWorldPoint(LocalAnchorA);
var pB = BodyB.GetWorldPoint(LocalAnchorB);
var d = pB - pA;
var axis = BodyA.GetWorldVector(LocalXAxisA);
var translation = Vector2.Dot(d, axis);
return(translation);
}
/// <summary>Initializes the specified local anchor A.</summary>
/// <param name="anchorA">The world anchor point for the first body.</param>
/// <param name="anchorB">The world anchor point for the second body.</param>
/// <param name="frequency">The mass-spring-damper frequency in Hertz. A value of 0 disables softness.</param>
/// <param name="dampingRatio">The damping ratio. 0 = no damping, 1 = critical damping.</param>
internal void Initialize(
WorldPoint anchorA,
WorldPoint anchorB,
float frequency,
float dampingRatio)
{
_localAnchorA = BodyA.GetLocalPoint(anchorA);
_localAnchorB = BodyB.GetLocalPoint(anchorB);
_length = System.Math.Max(0.1f, WorldPoint.Distance(anchorA, anchorB));
_frequency = System.Math.Max(0, frequency);
_dampingRatio = System.Math.Max(0, dampingRatio);
}
public override float GetReactionTorque(float inv_dt)
{
Transform xf1;
BodyB.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor2 - BodyB.LocalCenter);
Vector2 P = _impulse * _J.LinearB;
float L = _impulse * _J.AngularB - MathUtils.Cross(r, P);
return(inv_dt * L);
}
/// <summary>
/// This requires defining an
/// anchor point on both bodies and the non-zero length of the
/// distance joint. If you don't supply a length, the local anchor points
/// is used so that the initial configuration can violate the constraint
/// slightly. This helps when saving and loading a game.
/// Warning Do not use a zero or short length.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="anchorA">The first body anchor</param>
/// <param name="anchorB">The second body anchor</param>
public DistanceJoint(PhysicsComponent bodyA, PhysicsComponent bodyB, Vector2 anchorA, Vector2 anchorB)
: base(bodyA, bodyB)
{
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
// TODO: Just pass this into the ctor.
var configManager = IoCManager.Resolve <IConfigurationManager>();
Length = MathF.Max(configManager.GetCVar(CVars.LinearSlop), (BodyB.GetWorldPoint(anchorB) - BodyA.GetWorldPoint(anchorA)).Length);
WarmStarting = configManager.GetCVar(CVars.WarmStarting);
_linearSlop = configManager.GetCVar(CVars.LinearSlop);
_minLength = _length;
_maxLength = _length;
}
/// <summary>Constructor for FrictionJoint.</summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="anchor"></param>
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
public FrictionJoint(Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false)
: base(bodyA, bodyB, JointType.Friction)
{
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(anchor);
LocalAnchorB = BodyB.GetLocalPoint(anchor);
}
else
{
LocalAnchorA = anchor;
LocalAnchorB = anchor;
}
}
public FrictionJoint(PhysicsComponent bodyA, PhysicsComponent bodyB, bool useWorldCoordinates = false)
: base(bodyA.Owner, bodyB.Owner)
{
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(Vector2.Zero);
LocalAnchorB = BodyB.GetLocalPoint(Vector2.Zero);
}
else
{
LocalAnchorA = Vector2.Zero;
LocalAnchorB = Vector2.Zero;
}
}
/// <summary>
/// Constructor for FrictionJoint.
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="anchor"></param>
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
public FrictionJoint(PhysicsComponent bodyA, PhysicsComponent bodyB, Vector2 anchor, bool useWorldCoordinates = false)
: base(bodyA.Owner.Uid, bodyB.Owner.Uid)
{
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(anchor);
LocalAnchorB = BodyB.GetLocalPoint(anchor);
}
else
{
LocalAnchorA = anchor;
LocalAnchorB = anchor;
}
}
internal MouseJoint(MouseJointDef def)
: base(def)
{
Target = def.Target;
_localAnchorB = MathUtils.MulT(BodyB.GetTransform(), Target);
MaxForce = def.MaxForce;
Stiffness = def.Stiffness;
Damping = def.Damping;
_impulse.SetZero();
_beta = 0.0f;
_gamma = 0.0f;
}
/// <summary>
/// This requires defining a line of
/// motion using an axis and an anchor point. The definition uses local
/// anchor points and a local axis so that the initial configuration
/// can violate the constraint slightly. The joint translation is zero
/// when the local anchor points coincide in world space. Using local
/// anchors and a local axis helps when saving and loading a game.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="worldAnchor">The anchor.</param>
/// <param name="axis">The axis.</param>
public FixedPrismaticJoint(Body body, Vector2 worldAnchor, Vector2 axis)
: base(body)
{
JointType = JointType.FixedPrismatic;
BodyB = BodyA;
LocalAnchorA = worldAnchor;
LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
_localXAxis1 = axis;
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_refAngle = BodyB.Rotation;
_limitState = LimitState.Inactive;
}
/// <summary>
/// This requires defining an
/// anchor point on both bodies and the non-zero length of the
/// distance joint. If you don't supply a length, the local anchor points
/// is used so that the initial configuration can violate the constraint
/// slightly. This helps when saving and loading a game.
/// Warning Do not use a zero or short length.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="anchorA">The first body anchor</param>
/// <param name="anchorB">The second body anchor</param>
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
public DistanceJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
JointType = JointType.Distance;
if (useWorldCoordinates)
{
LocalAnchorA = bodyA.GetLocalPoint(ref anchorA);
LocalAnchorB = bodyB.GetLocalPoint(ref anchorB);
Length = (anchorB - anchorA).Length();
}
else
{
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
Length = (BodyB.GetWorldPoint(ref anchorB) - BodyA.GetWorldPoint(ref anchorA)).Length();
}
}
/// <summary>Initializes the joint with the specified properties.</summary>
/// <param name="target">The initial world target point. This is assumed to coincide with the body anchor initially.</param>
/// <param name="maxForce">
/// The maximum constraint force that can be exerted to move the candidate body. Usually you will
/// express as some multiple of the weight (multiplier * mass * gravity).
/// </param>
/// <param name="frequency">The response speed in Hz.</param>
/// <param name="dampingRatio">The damping ratio. 0 = no damping, 1 = critical damping.</param>
internal void Initialize(WorldPoint target, float maxForce, float frequency, float dampingRatio)
{
System.Diagnostics.Debug.Assert(maxForce >= 0.0f);
System.Diagnostics.Debug.Assert(frequency >= 0.0f);
System.Diagnostics.Debug.Assert(dampingRatio >= 0.0f);
_targetA = target;
_localAnchorB = BodyB.GetLocalPoint(_targetA);
_maxForce = System.Math.Max(0, maxForce);
_impulse = Vector2.Zero;
_frequency = System.Math.Max(0, frequency);
_dampingRatio = System.Math.Max(0, dampingRatio);
_tmp.Beta = 0.0f;
_tmp.Gamma = 0.0f;
}
/// <summary>
/// Called when properties of this constraint were changed.
/// </summary>
protected virtual void OnChanged()
{
// TODO: A separate OnBodyA/BChanged would be helpful for composite joints.
// Wake up bodies.
if (Enabled)
{
if (BodyA != null)
{
BodyA.WakeUp();
}
if (BodyB != null)
{
BodyB.WakeUp();
}
}
}
/// <summary>
/// This requires a world target point,
/// tuning parameters, and the time step.
/// </summary>
/// <param name="def"></param>
public MouseJoint(Body bodyA, Body bodyB, Vector2 target)
: base(bodyA, bodyB)
{
JointType = JointType.Mouse;
Frequency = 5.0f;
DampingRatio = 0.7f;
Debug.Assert(target.IsValid());
//Debug.Assert(MathUtils.IsValid(def.MaxForce) && def.MaxForce >= 0.0f);
//Debug.Assert(MathUtils.IsValid(def.FrequencyHz) && def.FrequencyHz >= 0.0f);
//Debug.Assert(MathUtils.IsValid(def.DampingRatio) && def.DampingRatio >= 0.0f);
Transform xf1;
BodyB.GetTransform(out xf1);
LocalAnchorB = target;
LocalAnchorA = MathUtils.MultiplyT(ref xf1, LocalAnchorB);
}
internal MouseJoint(MouseJointDef def) : base(def)
{
Debug.Assert(def.Target.IsValid());
Debug.Assert(def.MaxForce.IsValid() && def.MaxForce >= 0.0f);
Debug.Assert(def.FrequencyHz.IsValid() && def.FrequencyHz >= 0.0f);
Debug.Assert(def.DampingRatio.IsValid() && def.DampingRatio >= 0.0f);
_targetA = def.Target;
_localAnchorB = MathUtils.MulT(BodyB.GetTransform(), _targetA);
_maxForce = def.MaxForce;
_impulse.SetZero();
_frequencyHz = def.FrequencyHz;
_dampingRatio = def.DampingRatio;
_beta = 0.0f;
_gamma = 0.0f;
}
/// <summary>
/// Called when the simulation wants this force effect to apply forces to rigid bodies.
/// </summary>
/// <remarks>
/// <para>
/// This method must be implemented in derived classes. This method is only called after the
/// force effect was added to a simulation and <see cref="ForceEffect.OnAddToSimulation"/> was
/// called.
/// </para>
/// <para>
/// This method is responsible for applying the forces of the effect to the rigid bodies. To
/// apply a force the methods <see cref="ForceEffect.AddForce(RigidBody, Vector3, Vector3)"/>,
/// <see cref="ForceEffect.AddForce(RigidBody, Vector3)"/> and/or
/// <see cref="ForceEffect.AddTorque(RigidBody, Vector3)"/> of the <see cref="ForceEffect"/>
/// base class must be used. Do not use the <strong>AddForce</strong>/<strong>AddTorque</strong>
/// methods of the <see cref="RigidBody"/> class.
/// </para>
/// </remarks>
protected override void OnApply()
{
Vector3 worldPosA = (BodyA != null) ? BodyA.Pose.ToWorldPosition(AttachmentPositionALocal) : AttachmentPositionALocal;
Vector3 velA = (BodyA != null) ? BodyA.GetVelocityOfLocalPoint(AttachmentPositionALocal) : Vector3.Zero;
Vector3 worldPosB = (BodyB != null) ? BodyB.Pose.ToWorldPosition(AttachmentPositionBLocal) : AttachmentPositionBLocal;
Vector3 velB = (BodyB != null) ? BodyB.GetVelocityOfLocalPoint(AttachmentPositionBLocal) : Vector3.Zero;
// Compute spring force.
Vector3 springVectorAToB = worldPosB - worldPosA;
float currentLength = springVectorAToB.Length;
if (!springVectorAToB.TryNormalize())
{
springVectorAToB = Vector3.UnitY;
}
Vector3 force = SpringConstant * (currentLength - Length) * springVectorAToB;
// Compute damping force.
Vector3 velRel = velA - velB;
force += -DampingConstant *Vector3.Dot(velRel, springVectorAToB) * springVectorAToB;
// Not needed anymore. Simulation.EvaluateForce automatically wakes up rigid bodies for big
// force changes.
//if (BodyA != null && BodyB != null)
//{
// // Make sure both are awake or sleep simultaneously.
// if (BodyA.IsSleeping && !BodyB.IsSleeping)
// BodyA.DeferSleep(Simulation.Settings.Timing.FixedTimeStep);
// if (!BodyA.IsSleeping && BodyB.IsSleeping)
// BodyB.DeferSleep(Simulation.Settings.Timing.FixedTimeStep);
//}
if (BodyA != null && BodyA.MotionType == MotionType.Dynamic)
{
AddForce(BodyA, force, worldPosA);
}
if (BodyB != null && BodyB.MotionType == MotionType.Dynamic)
{
AddForce(BodyB, -force, worldPosB);
}
}
