/// <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>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;
}
/// <summary>
/// Constructor for MotorJoint.
/// </summary>
/// <param slotName="bodyA">The first body</param>
/// <param slotName="bodyB">The second body</param>
/// <param slotName="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
///
public MotorJoint(Body bodyA, Body bodyB)
: base(bodyA, bodyB)
{
bool useWorldCoordinates = false;
JointType = JointType.Motor;
Vector2 xB = BodyB.Position;
if (useWorldCoordinates)
{
_linearOffset = BodyA.GetLocalPoint(xB);
}
else
{
_linearOffset = xB;
}
//Defaults
_angularOffset = 0.0f;
_maxForce = 1.0f;
_maxTorque = 1.0f;
CorrectionFactor = 0.3f;
_angularOffset = BodyB.Rotation - BodyA.Rotation;
}
/// <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);
}
protected override void Initialise2(IntPtr world)
{
bool isArev = false;
bool isBrev = false;
if (JointA != null && JointB != null &&
(JointA.GetType() == typeof(LPJointRevolute) || JointA.GetType() == typeof(LPJointPrismatic)) &&
(JointB.GetType() == typeof(LPJointRevolute) || JointB.GetType() == typeof(LPJointPrismatic)))
{
if (JointA.GetType() == typeof(LPJointRevolute))
{
isArev = true;
}
if (JointB.GetType() == typeof(LPJointRevolute))
{
isBrev = true;
}
ThingPtr = LPAPIJoint.CreateGearJoint(world, BodyA.GetComponent <LPBody>().GetPtr(), BodyB.GetComponent <LPBody>().GetPtr()
, JointA.GetPtr(), isArev, JointB.GetPtr(), isBrev, Ratio, CollideConnected);
}
else
{
Debug.LogError("This Gear Joint must be assigned 2 Other Joints in order to be created"
+ ", also both joints need to be a type of either LPJointRevolute or LPJointPrismatic");
}
}
/// <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();
}
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 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;
}
/// Get the current length of the segment attached to bodyA.
public float GetCurrentLengthA()
{
var p = BodyA.GetWorldPoint(_localAnchorA);
var s = _groundAnchorA;
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;
ThingPtr = LPAPIJoint.CreateWeldJoint(world, BodyA.GetComponent <LPBody>().GetPtr(), BodyB.GetComponent <LPBody>().GetPtr()
, anchorA.x, anchorA.y, anchorB.x, anchorB.y, CollideConnected);
}
protected override void Initialise2(IntPtr world)
{
ThingPtr = LPAPIJoint.CreateMouseJoint(world, BodyA.GetComponent <LPBody>().GetPtr(), BodyB.GetComponent <LPBody>().GetPtr()
, BodyB.transform.position.x, BodyB.transform.position.y, true);
LPAPIJoint.SetMouseJointMaxForce(ThingPtr, MaximumForce);
LPAPIJoint.SetMouseJointDampingRatio(ThingPtr, DampingRatio);
LPAPIJoint.SetMouseJointFrequency(ThingPtr, Frequency);
}
public float GetCurrentLength()
{
var pA = BodyA.GetWorldPoint(_localAnchorA);
var pB = BodyB.GetWorldPoint(_localAnchorB);
var d = pB - pA;
var length = d.Length();
return(length);
}
/// <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 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);
}
protected override void Initialise2(IntPtr world)
{
//float lenght = (BodyA.transform.position-BodyB.transform.position).magnitude;
ThingPtr = LPAPIJoint.CreateDistanceJoint(world, BodyA.GetComponent <LPBody>().GetPtr(), BodyB.GetComponent <LPBody>().GetPtr()
, BodyAAnchorOffset.x, BodyAAnchorOffset.y
, BodyBAnchorOffset.x, BodyBAnchorOffset.y, lenght, CollideConnected);
LPAPIJoint.SetDistanceJointDampingRatio(ThingPtr, Dampingratio);
LPAPIJoint.SetDistanceJointFrequency(ThingPtr, FrequencyInHertz);
}
/// <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;
}
protected override void Initialise2(IntPtr world)
{
Vector3 anchorA = Vector3.zero;
Vector3 anchorB = Vector3.zero;
ThingPtr = LPAPIJoint.CreateFrictionJoint(world, BodyA.GetComponent <LPBody>().GetPtr(), BodyB.GetComponent <LPBody>().GetPtr()
, anchorA.x, anchorA.y, anchorB.x, anchorB.y, CollideConnected);
LPAPIJoint.SetFrictionJointMaxForce(ThingPtr, MaximumForce);
LPAPIJoint.SetFrictionJointMaxTorque(ThingPtr, MaximumTorque);
}
/// 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);
}
/// <summary>Initializes this joint with the specified parameters.</summary>
internal void Initialize(float maxForce, float maxTorque, float correctionFactor)
{
_linearOffset = BodyA.GetLocalPoint(BodyB.Position);
_angularOffset = BodyB.Angle - BodyA.Angle;
_maxForce = maxForce;
_maxTorque = maxTorque;
_correctionFactor = correctionFactor;
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
protected override void Initialise2(IntPtr world)
{
Vector3 groundA = (transform.position + new Vector3(BodyAGroundOffset.x, BodyAGroundOffset.y));
Vector3 groundB = (transform.position + new Vector3(BodyBGroundOffset.x, BodyBGroundOffset.y));
float lenghtA = (BodyA.transform.position - groundA).magnitude;
float lenghtB = (BodyB.transform.position - groundB).magnitude;
ThingPtr = LPAPIJoint.CreatePulleyJoint(world, BodyA.GetComponent <LPBody>().GetPtr(), BodyB.GetComponent <LPBody>().GetPtr()
, groundA.x, groundA.y, groundB.x, groundB.y
, BodyAAnchorOffset.x, BodyAAnchorOffset.y
, BodyBAnchorOffset.x, BodyBAnchorOffset.y, ratio, lenghtA, lenghtB, CollideConnected);
}
/// <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="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second body anchor.</param>
/// <param name="axis">The axis.</param>
public PrismaticJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis)
: base(bodyA, bodyB)
{
JointType = JointType.Prismatic;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
_localXAxis1 = BodyA.GetLocalVector(axis);
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_refAngle = BodyB.Rotation - BodyA.Rotation;
_limitState = LimitState.Inactive;
}
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(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;
}
}
/// <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;
}
}
/// <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="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second body anchor.</param>
/// <param name="axis">The axis.</param>
public FSPrismaticJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB, FVector2 axis)
: base(bodyA, bodyB)
{
JointType = JointType.Prismatic;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
_localXAxisA = BodyA.GetLocalVector(axis);
_localXAxisA.Normalize();
_localYAxisA = MathUtils.Cross(1.0f, _localXAxisA);
m_referenceAngle = BodyB.Rotation - BodyA.Rotation;
_limitState = LimitState.Inactive;
}
public PrismaticJoint(Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis)
: base(bodyA, bodyB)
{
JointType = JointType.Prismatic;
LocalAnchorA = bodyA.GetLocalPoint(anchor);
LocalAnchorB = bodyB.GetLocalPoint(anchor);
_localXAxisA = BodyA.GetLocalVector(axis);
_localXAxisA.Normalize();
_localYAxisA = MathUtils.Cross(1.0f, _localXAxisA);
_referenceAngle = BodyB.Rotation - BodyA.Rotation;
_limitState = LimitState.Inactive;
}
private Mat22 _mass; // effective mass for point-to-point constraint.
/// <summary>
/// This requires a world target point,
/// tuning parameters, and the time step.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="target">The target.</param>
public FixedMouseJoint(Body body, Vector2 target)
: base(body)
{
JointType = JointType.FixedMouse;
Frequency = 5.0f;
DampingRatio = 0.7f;
Debug.Assert(target.IsValid());
Transform xf1;
BodyA.GetTransform(out xf1);
LocalAnchorB = target;
LocalAnchorA = MathUtils.MultiplyT(ref xf1, LocalAnchorB);
}
/// <summary>
/// This requires a world target point,
/// tuning parameters, and the time step.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="worldAnchor">The target.</param>
public FixedMouseJoint(Body body, Vector2 worldAnchor)
: base(body)
{
JointType = JointType.FixedMouse;
Frequency = 5.0f;
DampingRatio = 0.7f;
Debug.Assert(worldAnchor.IsValid());
Transform xf1;
BodyA.GetTransform(out xf1);
_worldAnchor = worldAnchor;
LocalAnchorA = BodyA.GetLocalPoint(worldAnchor);
}
