507 lines
17 KiB
C#
507 lines
17 KiB
C#
/*
|
|
* Farseer Physics Engine based on Box2D.XNA port:
|
|
* Copyright (c) 2010 Ian Qvist
|
|
*
|
|
* Box2D.XNA port of Box2D:
|
|
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
|
|
*
|
|
* Original source Box2D:
|
|
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
|
|
*
|
|
* This software is provided 'as-is', without any express or implied
|
|
* warranty. In no event will the authors be held liable for any damages
|
|
* arising from the use of this software.
|
|
* Permission is granted to anyone to use this software for any purpose,
|
|
* including commercial applications, and to alter it and redistribute it
|
|
* freely, subject to the following restrictions:
|
|
* 1. The origin of this software must not be misrepresented; you must not
|
|
* claim that you wrote the original software. If you use this software
|
|
* in a product, an acknowledgment in the product documentation would be
|
|
* appreciated but is not required.
|
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
|
* misrepresented as being the original software.
|
|
* 3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
using System;
|
|
using System.Diagnostics;
|
|
using FarseerPhysics.Common;
|
|
using Microsoft.Xna.Framework;
|
|
|
|
namespace FarseerPhysics.Dynamics.Joints
|
|
{
|
|
/// <summary>
|
|
/// The pulley joint is connected to two bodies and two fixed ground points.
|
|
/// The pulley supports a ratio such that:
|
|
/// length1 + ratio * length2 <!--<-->= ant
|
|
/// Yes, the force transmitted is scaled by the ratio.
|
|
/// The pulley also enforces a maximum length limit on both sides. This is
|
|
/// useful to prevent one side of the pulley hitting the top.
|
|
/// </summary>
|
|
public class PulleyJoint : Joint
|
|
{
|
|
/// <summary>
|
|
/// Get the first ground anchor.
|
|
/// </summary>
|
|
/// <value></value>
|
|
public Vector2 GroundAnchorA;
|
|
|
|
/// <summary>
|
|
/// Get the second ground anchor.
|
|
/// </summary>
|
|
/// <value></value>
|
|
public Vector2 GroundAnchorB;
|
|
|
|
public Vector2 LocalAnchorA;
|
|
public Vector2 LocalAnchorB;
|
|
|
|
public float MinPulleyLength = 2.0f;
|
|
private float _ant;
|
|
private float _impulse;
|
|
private float _lengthA;
|
|
private float _lengthB;
|
|
private float _limitImpulse1;
|
|
private float _limitImpulse2;
|
|
private float _limitMass1;
|
|
private float _limitMass2;
|
|
private LimitState _limitState1;
|
|
private LimitState _limitState2;
|
|
private float _maxLengthA;
|
|
private float _maxLengthB;
|
|
|
|
// Effective masses
|
|
private float _pulleyMass;
|
|
private LimitState _state;
|
|
private Vector2 _u1;
|
|
private Vector2 _u2;
|
|
|
|
internal PulleyJoint()
|
|
{
|
|
JointType = JointType.Pulley;
|
|
}
|
|
|
|
/// <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="bodyA">The first body.</param>
|
|
/// <param name="bodyB">The second body.</param>
|
|
/// <param name="groundAnchorA">The ground anchor for the first body.</param>
|
|
/// <param name="groundAnchorB">The ground anchor for the second body.</param>
|
|
/// <param name="localAnchorA">The first body anchor.</param>
|
|
/// <param name="localAnchorB">The second body anchor.</param>
|
|
/// <param name="ratio">The ratio.</param>
|
|
public PulleyJoint(Body bodyA, Body bodyB,
|
|
Vector2 groundAnchorA, Vector2 groundAnchorB,
|
|
Vector2 localAnchorA, Vector2 localAnchorB,
|
|
float ratio)
|
|
: base(bodyA, bodyB)
|
|
{
|
|
JointType = JointType.Pulley;
|
|
|
|
GroundAnchorA = groundAnchorA;
|
|
GroundAnchorB = groundAnchorB;
|
|
LocalAnchorA = localAnchorA;
|
|
LocalAnchorB = localAnchorB;
|
|
|
|
Vector2 d1 = BodyA.GetWorldPoint(localAnchorA) - groundAnchorA;
|
|
_lengthA = d1.Length();
|
|
|
|
Vector2 d2 = BodyB.GetWorldPoint(localAnchorB) - groundAnchorB;
|
|
_lengthB = d2.Length();
|
|
|
|
Debug.Assert(ratio != 0.0f);
|
|
Debug.Assert(ratio > Settings.Epsilon);
|
|
Ratio = ratio;
|
|
|
|
float C = _lengthA + Ratio * _lengthB;
|
|
|
|
MaxLengthA = C - Ratio * MinPulleyLength;
|
|
MaxLengthB = (C - MinPulleyLength) / Ratio;
|
|
|
|
_ant = _lengthA + Ratio * _lengthB;
|
|
|
|
MaxLengthA = Math.Min(MaxLengthA, _ant - Ratio * MinPulleyLength);
|
|
MaxLengthB = Math.Min(MaxLengthB, (_ant - MinPulleyLength) / Ratio);
|
|
|
|
_impulse = 0.0f;
|
|
_limitImpulse1 = 0.0f;
|
|
_limitImpulse2 = 0.0f;
|
|
}
|
|
|
|
public override Vector2 WorldAnchorA
|
|
{
|
|
get { return BodyA.GetWorldPoint(LocalAnchorA); }
|
|
}
|
|
|
|
public override Vector2 WorldAnchorB
|
|
{
|
|
get { return BodyB.GetWorldPoint(LocalAnchorB); }
|
|
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the current length of the segment attached to body1.
|
|
/// </summary>
|
|
/// <value></value>
|
|
public float LengthA
|
|
{
|
|
get
|
|
{
|
|
Vector2 d = BodyA.GetWorldPoint(LocalAnchorA) - GroundAnchorA;
|
|
return d.Length();
|
|
}
|
|
set { _lengthA = value; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the current length of the segment attached to body2.
|
|
/// </summary>
|
|
/// <value></value>
|
|
public float LengthB
|
|
{
|
|
get
|
|
{
|
|
Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - GroundAnchorB;
|
|
return d.Length();
|
|
}
|
|
set { _lengthB = value; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the pulley ratio.
|
|
/// </summary>
|
|
/// <value></value>
|
|
public float Ratio { get; set; }
|
|
|
|
public float MaxLengthA
|
|
{
|
|
get { return _maxLengthA; }
|
|
set { _maxLengthA = value; }
|
|
}
|
|
|
|
public float MaxLengthB
|
|
{
|
|
get { return _maxLengthB; }
|
|
set { _maxLengthB = value; }
|
|
}
|
|
|
|
public override Vector2 GetReactionForce(float inv_dt)
|
|
{
|
|
Vector2 P = _impulse * _u2;
|
|
return inv_dt * P;
|
|
}
|
|
|
|
public override float GetReactionTorque(float inv_dt)
|
|
{
|
|
return 0.0f;
|
|
}
|
|
|
|
internal override void InitVelocityConstraints(ref TimeStep step)
|
|
{
|
|
Body b1 = BodyA;
|
|
Body b2 = BodyB;
|
|
|
|
Transform xf1, xf2;
|
|
b1.GetTransform(out xf1);
|
|
b2.GetTransform(out xf2);
|
|
|
|
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
|
|
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
|
|
|
|
Vector2 p1 = b1.Sweep.C + r1;
|
|
Vector2 p2 = b2.Sweep.C + r2;
|
|
|
|
Vector2 s1 = GroundAnchorA;
|
|
Vector2 s2 = GroundAnchorB;
|
|
|
|
// Get the pulley axes.
|
|
_u1 = p1 - s1;
|
|
_u2 = p2 - s2;
|
|
|
|
float length1 = _u1.Length();
|
|
float length2 = _u2.Length();
|
|
|
|
if (length1 > Settings.LinearSlop)
|
|
{
|
|
_u1 *= 1.0f / length1;
|
|
}
|
|
else
|
|
{
|
|
_u1 = Vector2.Zero;
|
|
}
|
|
|
|
if (length2 > Settings.LinearSlop)
|
|
{
|
|
_u2 *= 1.0f / length2;
|
|
}
|
|
else
|
|
{
|
|
_u2 = Vector2.Zero;
|
|
}
|
|
|
|
float C = _ant - length1 - Ratio * length2;
|
|
if (C > 0.0f)
|
|
{
|
|
_state = LimitState.Inactive;
|
|
_impulse = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
_state = LimitState.AtUpper;
|
|
}
|
|
|
|
if (length1 < MaxLengthA)
|
|
{
|
|
_limitState1 = LimitState.Inactive;
|
|
_limitImpulse1 = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
_limitState1 = LimitState.AtUpper;
|
|
}
|
|
|
|
if (length2 < MaxLengthB)
|
|
{
|
|
_limitState2 = LimitState.Inactive;
|
|
_limitImpulse2 = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
_limitState2 = LimitState.AtUpper;
|
|
}
|
|
|
|
// Compute effective mass.
|
|
float cr1u1 = MathUtils.Cross(r1, _u1);
|
|
float cr2u2 = MathUtils.Cross(r2, _u2);
|
|
|
|
_limitMass1 = b1.InvMass + b1.InvI * cr1u1 * cr1u1;
|
|
_limitMass2 = b2.InvMass + b2.InvI * cr2u2 * cr2u2;
|
|
_pulleyMass = _limitMass1 + Ratio * Ratio * _limitMass2;
|
|
Debug.Assert(_limitMass1 > Settings.Epsilon);
|
|
Debug.Assert(_limitMass2 > Settings.Epsilon);
|
|
Debug.Assert(_pulleyMass > Settings.Epsilon);
|
|
_limitMass1 = 1.0f / _limitMass1;
|
|
_limitMass2 = 1.0f / _limitMass2;
|
|
_pulleyMass = 1.0f / _pulleyMass;
|
|
|
|
if (Settings.EnableWarmstarting)
|
|
{
|
|
// Scale impulses to support variable time steps.
|
|
_impulse *= step.dtRatio;
|
|
_limitImpulse1 *= step.dtRatio;
|
|
_limitImpulse2 *= step.dtRatio;
|
|
|
|
// Warm starting.
|
|
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
|
|
Vector2 P2 = (-Ratio * _impulse - _limitImpulse2) * _u2;
|
|
b1.LinearVelocityInternal += b1.InvMass * P1;
|
|
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
|
|
b2.LinearVelocityInternal += b2.InvMass * P2;
|
|
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
|
|
}
|
|
else
|
|
{
|
|
_impulse = 0.0f;
|
|
_limitImpulse1 = 0.0f;
|
|
_limitImpulse2 = 0.0f;
|
|
}
|
|
}
|
|
|
|
internal override void SolveVelocityConstraints(ref TimeStep step)
|
|
{
|
|
Body b1 = BodyA;
|
|
Body b2 = BodyB;
|
|
|
|
Transform xf1, xf2;
|
|
b1.GetTransform(out xf1);
|
|
b2.GetTransform(out xf2);
|
|
|
|
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
|
|
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
|
|
|
|
if (_state == LimitState.AtUpper)
|
|
{
|
|
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
|
|
Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
|
|
|
|
float Cdot = -Vector2.Dot(_u1, v1) - Ratio * Vector2.Dot(_u2, v2);
|
|
float impulse = _pulleyMass * (-Cdot);
|
|
float oldImpulse = _impulse;
|
|
_impulse = Math.Max(0.0f, _impulse + impulse);
|
|
impulse = _impulse - oldImpulse;
|
|
|
|
Vector2 P1 = -impulse * _u1;
|
|
Vector2 P2 = -Ratio * impulse * _u2;
|
|
b1.LinearVelocityInternal += b1.InvMass * P1;
|
|
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
|
|
b2.LinearVelocityInternal += b2.InvMass * P2;
|
|
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
|
|
}
|
|
|
|
if (_limitState1 == LimitState.AtUpper)
|
|
{
|
|
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
|
|
|
|
float Cdot = -Vector2.Dot(_u1, v1);
|
|
float impulse = -_limitMass1 * Cdot;
|
|
float oldImpulse = _limitImpulse1;
|
|
_limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
|
|
impulse = _limitImpulse1 - oldImpulse;
|
|
|
|
Vector2 P1 = -impulse * _u1;
|
|
b1.LinearVelocityInternal += b1.InvMass * P1;
|
|
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
|
|
}
|
|
|
|
if (_limitState2 == LimitState.AtUpper)
|
|
{
|
|
Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
|
|
|
|
float Cdot = -Vector2.Dot(_u2, v2);
|
|
float impulse = -_limitMass2 * Cdot;
|
|
float oldImpulse = _limitImpulse2;
|
|
_limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
|
|
impulse = _limitImpulse2 - oldImpulse;
|
|
|
|
Vector2 P2 = -impulse * _u2;
|
|
b2.LinearVelocityInternal += b2.InvMass * P2;
|
|
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
|
|
}
|
|
}
|
|
|
|
internal override bool SolvePositionConstraints()
|
|
{
|
|
Body b1 = BodyA;
|
|
Body b2 = BodyB;
|
|
|
|
Vector2 s1 = GroundAnchorA;
|
|
Vector2 s2 = GroundAnchorB;
|
|
|
|
float linearError = 0.0f;
|
|
|
|
if (_state == LimitState.AtUpper)
|
|
{
|
|
Transform xf1, xf2;
|
|
b1.GetTransform(out xf1);
|
|
b2.GetTransform(out xf2);
|
|
|
|
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
|
|
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
|
|
|
|
Vector2 p1 = b1.Sweep.C + r1;
|
|
Vector2 p2 = b2.Sweep.C + r2;
|
|
|
|
// Get the pulley axes.
|
|
_u1 = p1 - s1;
|
|
_u2 = p2 - s2;
|
|
|
|
float length1 = _u1.Length();
|
|
float length2 = _u2.Length();
|
|
|
|
if (length1 > Settings.LinearSlop)
|
|
{
|
|
_u1 *= 1.0f / length1;
|
|
}
|
|
else
|
|
{
|
|
_u1 = Vector2.Zero;
|
|
}
|
|
|
|
if (length2 > Settings.LinearSlop)
|
|
{
|
|
_u2 *= 1.0f / length2;
|
|
}
|
|
else
|
|
{
|
|
_u2 = Vector2.Zero;
|
|
}
|
|
|
|
float C = _ant - length1 - Ratio * length2;
|
|
linearError = Math.Max(linearError, -C);
|
|
|
|
C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
|
|
float impulse = -_pulleyMass * C;
|
|
|
|
Vector2 P1 = -impulse * _u1;
|
|
Vector2 P2 = -Ratio * impulse * _u2;
|
|
|
|
b1.Sweep.C += b1.InvMass * P1;
|
|
b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1);
|
|
b2.Sweep.C += b2.InvMass * P2;
|
|
b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2);
|
|
|
|
b1.SynchronizeTransform();
|
|
b2.SynchronizeTransform();
|
|
}
|
|
|
|
if (_limitState1 == LimitState.AtUpper)
|
|
{
|
|
Transform xf1;
|
|
b1.GetTransform(out xf1);
|
|
|
|
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
|
|
Vector2 p1 = b1.Sweep.C + r1;
|
|
|
|
_u1 = p1 - s1;
|
|
float length1 = _u1.Length();
|
|
|
|
if (length1 > Settings.LinearSlop)
|
|
{
|
|
_u1 *= 1.0f / length1;
|
|
}
|
|
else
|
|
{
|
|
_u1 = Vector2.Zero;
|
|
}
|
|
|
|
float C = MaxLengthA - length1;
|
|
linearError = Math.Max(linearError, -C);
|
|
C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
|
|
float impulse = -_limitMass1 * C;
|
|
|
|
Vector2 P1 = -impulse * _u1;
|
|
b1.Sweep.C += b1.InvMass * P1;
|
|
b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1);
|
|
|
|
b1.SynchronizeTransform();
|
|
}
|
|
|
|
if (_limitState2 == LimitState.AtUpper)
|
|
{
|
|
Transform xf2;
|
|
b2.GetTransform(out xf2);
|
|
|
|
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
|
|
Vector2 p2 = b2.Sweep.C + r2;
|
|
|
|
_u2 = p2 - s2;
|
|
float length2 = _u2.Length();
|
|
|
|
if (length2 > Settings.LinearSlop)
|
|
{
|
|
_u2 *= 1.0f / length2;
|
|
}
|
|
else
|
|
{
|
|
_u2 = Vector2.Zero;
|
|
}
|
|
|
|
float C = MaxLengthB - length2;
|
|
linearError = Math.Max(linearError, -C);
|
|
C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
|
|
float impulse = -_limitMass2 * C;
|
|
|
|
Vector2 P2 = -impulse * _u2;
|
|
b2.Sweep.C += b2.InvMass * P2;
|
|
b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2);
|
|
|
|
b2.SynchronizeTransform();
|
|
}
|
|
|
|
return linearError < Settings.LinearSlop;
|
|
}
|
|
}
|
|
} |