vlad.os/LuaCsForBarotraumaEP
0
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Server job assigning logic, submarine movement syncing, submarine collision improvements, spawnpoints in levels

Browse Source
This commit is contained in:
Regalis
2015-07-08 11:37:47 +03:00
parent 3af9b8183b
commit d56f7f3f77
155 changed files with 39772 additions and 261 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Farseer Physics Engine 3.5/Dynamics/Joints/AngleJoint.cs
+128
View File
@@ -0,0 +1,128 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*/
using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// Maintains a fixed angle between two bodies
/// </summary>
public class AngleJoint : Joint
{
private float _bias;
private float _jointError;
private float _massFactor;
private float _targetAngle;
internal AngleJoint()
{
JointType = JointType.Angle;
}
/// <summary>
/// Constructor for AngleJoint
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
public AngleJoint(Body bodyA, Body bodyB)
: base(bodyA, bodyB)
{
JointType = JointType.Angle;
BiasFactor = .2f;
MaxImpulse = float.MaxValue;
}
public override Vector2 WorldAnchorA
{
get { return BodyA.Position; }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.Position; }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
/// <summary>
/// The desired angle between BodyA and BodyB
/// </summary>
public float TargetAngle
{
get { return _targetAngle; }
set
{
if (value != _targetAngle)
{
_targetAngle = value;
WakeBodies();
}
}
}
/// <summary>
/// Gets or sets the bias factor.
/// Defaults to 0.2
/// </summary>
public float BiasFactor { get; set; }
/// <summary>
/// Gets or sets the maximum impulse
/// Defaults to float.MaxValue
/// </summary>
public float MaxImpulse { get; set; }
/// <summary>
/// Gets or sets the softness of the joint
/// Defaults to 0
/// </summary>
public float Softness { get; set; }
public override Vector2 GetReactionForce(float invDt)
{
//TODO
//return _inv_dt * _impulse;
return Vector2.Zero;
}
public override float GetReactionTorque(float invDt)
{
return 0;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
int indexA = BodyA.IslandIndex;
int indexB = BodyB.IslandIndex;
float aW = data.positions[indexA].a;
float bW = data.positions[indexB].a;
_jointError = (bW - aW - TargetAngle);
_bias = -BiasFactor * data.step.inv_dt * _jointError;
_massFactor = (1 - Softness) / (BodyA._invI + BodyB._invI);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
int indexA = BodyA.IslandIndex;
int indexB = BodyB.IslandIndex;
float p = (_bias - data.velocities[indexB].w + data.velocities[indexA].w) * _massFactor;
data.velocities[indexA].w -= BodyA._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
data.velocities[indexB].w += BodyB._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
//no position solving for this joint
return true;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/DistanceJoint.cs
+331
View File
@@ -0,0 +1,331 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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
{
// 1-D rained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
/// <summary>
/// A distance joint rains two points on two bodies
/// to remain at a fixed distance from each other. You can view
/// this as a massless, rigid rod.
/// </summary>
public class DistanceJoint : Joint
{
// Solver shared
private float _bias;
private float _gamma;
private float _impulse;
// Solver temp
private int _indexA;
private int _indexB;
private Vector2 _u;
private Vector2 _rA;
private Vector2 _rB;
private Vector2 _localCenterA;
private Vector2 _localCenterB;
private float _invMassA;
private float _invMassB;
private float _invIA;
private float _invIB;
private float _mass;
internal DistanceJoint()
{
JointType = JointType.Distance;
}
/// <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>
/// The local anchor point relative to bodyA's origin.
/// </summary>
public Vector2 LocalAnchorA { get; set; }
/// <summary>
/// The local anchor point relative to bodyB's origin.
/// </summary>
public Vector2 LocalAnchorB { get; set; }
public override sealed Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public override sealed Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
/// <summary>
/// The natural length between the anchor points.
/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
/// </summary>
public float Length { get; set; }
/// <summary>
/// The mass-spring-damper frequency in Hertz. A value of 0
/// disables softness.
/// </summary>
public float Frequency { get; set; }
/// <summary>
/// The damping ratio. 0 = no damping, 1 = critical damping.
/// </summary>
public float DampingRatio { get; set; }
/// <summary>
/// Get the reaction force given the inverse time step. Unit is N.
/// </summary>
/// <param name="invDt"></param>
/// <returns></returns>
public override Vector2 GetReactionForce(float invDt)
{
Vector2 F = (invDt * _impulse) * _u;
return F;
}
/// <summary>
/// Get the reaction torque given the inverse time step.
/// Unit is N*m. This is always zero for a distance joint.
/// </summary>
/// <param name="invDt"></param>
/// <returns></returns>
public override float GetReactionTorque(float invDt)
{
return 0.0f;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
_u = cB + _rB - cA - _rA;
// Handle singularity.
float length = _u.Length();
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.Zero;
}
float crAu = MathUtils.Cross(_rA, _u);
float crBu = MathUtils.Cross(_rB, _u);
float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu;
// Compute the effective mass matrix.
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Frequency > 0.0f)
{
float C = length - Length;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * _mass * DampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
float h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invMass += _gamma;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
}
else
{
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = _impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
// Cdot = dot(u, v + cross(w, r))
Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
float Cdot = Vector2.Dot(_u, vpB - vpA);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vector2 P = impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
if (Frequency > 0.0f)
{
// There is no position correction for soft distance constraints.
return true;
}
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 u = cB + rB - cA - rA;
float length = u.Length(); u.Normalize();
float C = length - Length;
C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float impulse = -_mass * C;
Vector2 P = impulse * u;
cA -= _invMassA * P;
aA -= _invIA * MathUtils.Cross(rA, P);
cB += _invMassB * P;
aB += _invIB * MathUtils.Cross(rB, P);
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return Math.Abs(C) < Settings.LinearSlop;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/FixedMouseJoint.cs
+261
View File
@@ -0,0 +1,261 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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.Diagnostics;
using FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// p = attached point, m = mouse point
// C = p - m
// Cdot = v
// = v + cross(w, r)
// J = [I r_skew]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
/// <summary>
/// A mouse joint is used to make a point on a body track a
/// specified world point. This a soft constraint with a maximum
/// force. This allows the constraint to stretch and without
/// applying huge forces.
/// NOTE: this joint is not documented in the manual because it was
/// developed to be used in the testbed. If you want to learn how to
/// use the mouse joint, look at the testbed.
/// </summary>
public class FixedMouseJoint : Joint
{
private Vector2 _worldAnchor;
private float _frequency;
private float _dampingRatio;
private float _beta;
// Solver shared
private Vector2 _impulse;
private float _maxForce;
private float _gamma;
// Solver temp
private int _indexA;
private Vector2 _rA;
private Vector2 _localCenterA;
private float _invMassA;
private float _invIA;
private Mat22 _mass;
private Vector2 _C;
/// <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;
MaxForce = 1000 * body.Mass;
Debug.Assert(worldAnchor.IsValid());
_worldAnchor = worldAnchor;
LocalAnchorA = MathUtils.MulT(BodyA._xf, worldAnchor);
}
/// <summary>
/// The local anchor point on BodyA
/// </summary>
public Vector2 LocalAnchorA { get; set; }
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
}
public override Vector2 WorldAnchorB
{
get { return _worldAnchor; }
set
{
WakeBodies();
_worldAnchor = value;
}
}
/// <summary>
/// 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).
/// </summary>
public float MaxForce
{
get { return _maxForce; }
set
{
Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f);
_maxForce = value;
}
}
/// <summary>
/// The response speed.
/// </summary>
public float Frequency
{
get { return _frequency; }
set
{
Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f);
_frequency = value;
}
}
/// <summary>
/// The damping ratio. 0 = no damping, 1 = critical damping.
/// </summary>
public float DampingRatio
{
get { return _dampingRatio; }
set
{
Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f);
_dampingRatio = value;
}
}
public override Vector2 GetReactionForce(float invDt)
{
return invDt * _impulse;
}
public override float GetReactionTorque(float invDt)
{
return invDt * 0.0f;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invIA = BodyA._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Rot qA = new Rot(aA);
float mass = BodyA.Mass;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * mass * DampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float h = data.step.dt;
Debug.Assert(d + h * k > Settings.Epsilon);
_gamma = h * (d + h * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = h * k * _gamma;
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
Mat22 K = new Mat22();
K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma;
K.ex.Y = -_invIA * _rA.X * _rA.Y;
K.ey.X = K.ex.Y;
K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma;
_mass = K.Inverse;
_C = cA + _rA - _worldAnchor;
_C *= _beta;
// Cheat with some damping
wA *= 0.98f;
if (Settings.EnableWarmstarting)
{
_impulse *= data.step.dtRatio;
vA += _invMassA * _impulse;
wA += _invIA * MathUtils.Cross(_rA, _impulse);
}
else
{
_impulse = Vector2.Zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
// Cdot = v + cross(w, r)
Vector2 Cdot = vA + MathUtils.Cross(wA, _rA);
Vector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse));
Vector2 oldImpulse = _impulse;
_impulse += impulse;
float maxImpulse = data.step.dt * MaxForce;
if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
_impulse *= maxImpulse / _impulse.Length();
}
impulse = _impulse - oldImpulse;
vA += _invMassA * impulse;
wA += _invIA * MathUtils.Cross(_rA, impulse);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
return true;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/FrictionJoint.cs
+272
View File
@@ -0,0 +1,272 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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 FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Point-to-point constraint
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
/// <summary>
/// Friction joint. This is used for top-down friction.
/// It provides 2D translational friction and angular friction.
/// </summary>
public class FrictionJoint : Joint
{
// Solver shared
private Vector2 _linearImpulse;
private float _angularImpulse;
// Solver temp
private int _indexA;
private int _indexB;
private Vector2 _rA;
private Vector2 _rB;
private Vector2 _localCenterA;
private Vector2 _localCenterB;
private float _invMassA;
private float _invMassB;
private float _invIA;
private float _invIB;
private float _angularMass;
private Mat22 _linearMass;
internal FrictionJoint()
{
JointType = JointType.Friction;
}
/// <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 = JointType.Friction;
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(anchor);
LocalAnchorB = BodyB.GetLocalPoint(anchor);
}
else
{
LocalAnchorA = anchor;
LocalAnchorB = anchor;
}
}
/// <summary>
/// The local anchor point on BodyA
/// </summary>
public Vector2 LocalAnchorA { get; set; }
/// <summary>
/// The local anchor point on BodyB
/// </summary>
public Vector2 LocalAnchorB { get; set; }
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
}
/// <summary>
/// The maximum friction force in N.
/// </summary>
public float MaxForce { get; set; }
/// <summary>
/// The maximum friction torque in N-m.
/// </summary>
public float MaxTorque { get; set; }
public override Vector2 GetReactionForce(float invDt)
{
return invDt * _linearImpulse;
}
public override float GetReactionTorque(float invDt)
{
return invDt * _angularImpulse;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Mat22 K = new Mat22();
K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y;
K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X;
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.step.dtRatio;
_angularImpulse *= data.step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
float h = data.step.dt;
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = h * MaxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 impulse = -MathUtils.Mul(ref _linearMass, Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = h * MaxForce;
if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(_rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(_rB, impulse);
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
return true;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/GearJoint.cs
+478
View File
@@ -0,0 +1,478 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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.Diagnostics;
using FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Gear Joint:
// C0 = (coordinate1 + ratio * coordinate2)_initial
// C = (coordinate1 + ratio * coordinate2) - C0 = 0
// J = [J1 ratio * J2]
// K = J * invM * JT
// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
//
// Revolute:
// coordinate = rotation
// Cdot = angularVelocity
// J = [0 0 1]
// K = J * invM * JT = invI
//
// Prismatic:
// coordinate = dot(p - pg, ug)
// Cdot = dot(v + cross(w, r), ug)
// J = [ug cross(r, ug)]
// K = J * invM * JT = invMass + invI * cross(r, ug)^2
/// <summary>
/// A gear joint is used to connect two joints together.
/// Either joint can be a revolute or prismatic joint.
/// You specify a gear ratio to bind the motions together:
/// <![CDATA[coordinate1 + ratio * coordinate2 = ant]]>
/// The ratio can be negative or positive. If one joint is a revolute joint
/// and the other joint is a prismatic joint, then the ratio will have units
/// of length or units of 1/length.
///
/// Warning: You have to manually destroy the gear joint if jointA or jointB is destroyed.
/// </summary>
public class GearJoint : Joint
{
private JointType _typeA;
private JointType _typeB;
private Body _bodyA;
private Body _bodyB;
private Body _bodyC;
private Body _bodyD;
// Solver shared
private Vector2 _localAnchorA;
private Vector2 _localAnchorB;
private Vector2 _localAnchorC;
private Vector2 _localAnchorD;
private Vector2 _localAxisC;
private Vector2 _localAxisD;
private float _referenceAngleA;
private float _referenceAngleB;
private float _constant;
private float _ratio;
private float _impulse;
// Solver temp
private int _indexA, _indexB, _indexC, _indexD;
private Vector2 _lcA, _lcB, _lcC, _lcD;
private float _mA, _mB, _mC, _mD;
private float _iA, _iB, _iC, _iD;
private Vector2 _JvAC, _JvBD;
private float _JwA, _JwB, _JwC, _JwD;
private float _mass;
/// <summary>
/// Requires two existing revolute or prismatic joints (any combination will work).
/// The provided joints must attach a dynamic body to a static body.
/// </summary>
/// <param name="jointA">The first joint.</param>
/// <param name="jointB">The second joint.</param>
/// <param name="ratio">The ratio.</param>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio = 1f)
{
JointType = JointType.Gear;
BodyA = bodyA;
BodyB = bodyB;
JointA = jointA;
JointB = jointB;
Ratio = ratio;
_typeA = jointA.JointType;
_typeB = jointB.JointType;
Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic);
Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
_bodyC = JointA.BodyA;
_bodyA = JointA.BodyB;
// Get geometry of joint1
Transform xfA = _bodyA._xf;
float aA = _bodyA._sweep.A;
Transform xfC = _bodyC._xf;
float aC = _bodyC._sweep.A;
if (_typeA == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointA;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC = Vector2.Zero;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointA;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxis;
Vector2 pC = _localAnchorC;
Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
coordinateA = Vector2.Dot(pA - pC, _localAxisC);
}
_bodyD = JointB.BodyA;
_bodyB = JointB.BodyB;
// Get geometry of joint2
Transform xfB = _bodyB._xf;
float aB = _bodyB._sweep.A;
Transform xfD = _bodyD._xf;
float aD = _bodyD._sweep.A;
if (_typeB == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointB;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD = Vector2.Zero;
coordinateB = aB - aD - _referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointB;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxis;
Vector2 pD = _localAnchorD;
Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
coordinateB = Vector2.Dot(pB - pD, _localAxisD);
}
_ratio = ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
public override Vector2 WorldAnchorA
{
get { return _bodyA.GetWorldPoint(_localAnchorA); }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
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>
/// The gear ratio.
/// </summary>
public float Ratio
{
get { return _ratio; }
set
{
Debug.Assert(MathUtils.IsValid(value));
_ratio = value;
}
}
/// <summary>
/// The first revolute/prismatic joint attached to the gear joint.
/// </summary>
public Joint JointA { get; private set; }
/// <summary>
/// The second revolute/prismatic joint attached to the gear joint.
/// </summary>
public Joint JointB { get; private set; }
public override Vector2 GetReactionForce(float invDt)
{
Vector2 P = _impulse * _JvAC;
return invDt * P;
}
public override float GetReactionTorque(float invDt)
{
float L = _impulse * _JwA;
return invDt * L;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = _bodyA.IslandIndex;
_indexB = _bodyB.IslandIndex;
_indexC = _bodyC.IslandIndex;
_indexD = _bodyD.IslandIndex;
_lcA = _bodyA._sweep.LocalCenter;
_lcB = _bodyB._sweep.LocalCenter;
_lcC = _bodyC._sweep.LocalCenter;
_lcD = _bodyD._sweep.LocalCenter;
_mA = _bodyA._invMass;
_mB = _bodyB._invMass;
_mC = _bodyC._invMass;
_mD = _bodyD._invMass;
_iA = _bodyA._invI;
_iB = _bodyB._invI;
_iC = _bodyC._invI;
_iD = _bodyD._invI;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float aC = data.positions[_indexC].a;
Vector2 vC = data.velocities[_indexC].v;
float wC = data.velocities[_indexC].w;
float aD = data.positions[_indexD].a;
Vector2 vD = data.velocities[_indexD].v;
float wD = data.velocities[_indexD].w;
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
_mass = 0.0f;
if (_typeA == JointType.Revolute)
{
_JvAC = Vector2.Zero;
_JwA = 1.0f;
_JwC = 1.0f;
_mass += _iA + _iC;
}
else
{
Vector2 u = MathUtils.Mul(qC, _localAxisC);
Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
_JvAC = u;
_JwC = MathUtils.Cross(rC, u);
_JwA = MathUtils.Cross(rA, u);
_mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA;
}
if (_typeB == JointType.Revolute)
{
_JvBD = Vector2.Zero;
_JwB = _ratio;
_JwD = _ratio;
_mass += _ratio * _ratio * (_iB + _iD);
}
else
{
Vector2 u = MathUtils.Mul(qD, _localAxisD);
Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
_JvBD = _ratio * u;
_JwD = _ratio * MathUtils.Cross(rD, u);
_JwB = _ratio * MathUtils.Cross(rB, u);
_mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB;
}
// Compute effective mass.
_mass = _mass > 0.0f ? 1.0f / _mass : 0.0f;
if (Settings.EnableWarmstarting)
{
vA += (_mA * _impulse) * _JvAC;
wA += _iA * _impulse * _JwA;
vB += (_mB * _impulse) * _JvBD;
wB += _iB * _impulse * _JwB;
vC -= (_mC * _impulse) * _JvAC;
wC -= _iC * _impulse * _JwC;
vD -= (_mD * _impulse) * _JvBD;
wD -= _iD * _impulse * _JwD;
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
data.velocities[_indexC].v = vC;
data.velocities[_indexC].w = wC;
data.velocities[_indexD].v = vD;
data.velocities[_indexD].w = wD;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Vector2 vC = data.velocities[_indexC].v;
float wC = data.velocities[_indexC].w;
Vector2 vD = data.velocities[_indexD].v;
float wD = data.velocities[_indexD].w;
float Cdot = Vector2.Dot(_JvAC, vA - vC) + Vector2.Dot(_JvBD, vB - vD);
Cdot += (_JwA * wA - _JwC * wC) + (_JwB * wB - _JwD * wD);
float impulse = -_mass * Cdot;
_impulse += impulse;
vA += (_mA * impulse) * _JvAC;
wA += _iA * impulse * _JwA;
vB += (_mB * impulse) * _JvBD;
wB += _iB * impulse * _JwB;
vC -= (_mC * impulse) * _JvAC;
wC -= _iC * impulse * _JwC;
vD -= (_mD * impulse) * _JvBD;
wD -= _iD * impulse * _JwD;
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
data.velocities[_indexC].v = vC;
data.velocities[_indexC].w = wC;
data.velocities[_indexD].v = vD;
data.velocities[_indexD].w = wD;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 cC = data.positions[_indexC].c;
float aC = data.positions[_indexC].a;
Vector2 cD = data.positions[_indexD].c;
float aD = data.positions[_indexD].a;
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
const float linearError = 0.0f;
float coordinateA, coordinateB;
Vector2 JvAC, JvBD;
float JwA, JwB, JwC, JwD;
float mass = 0.0f;
if (_typeA == JointType.Revolute)
{
JvAC = Vector2.Zero;
JwA = 1.0f;
JwC = 1.0f;
mass += _iA + _iC;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
Vector2 u = MathUtils.Mul(qC, _localAxisC);
Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
JvAC = u;
JwC = MathUtils.Cross(rC, u);
JwA = MathUtils.Cross(rA, u);
mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA;
Vector2 pC = _localAnchorC - _lcC;
Vector2 pA = MathUtils.MulT(qC, rA + (cA - cC));
coordinateA = Vector2.Dot(pA - pC, _localAxisC);
}
if (_typeB == JointType.Revolute)
{
JvBD = Vector2.Zero;
JwB = _ratio;
JwD = _ratio;
mass += _ratio * _ratio * (_iB + _iD);
coordinateB = aB - aD - _referenceAngleB;
}
else
{
Vector2 u = MathUtils.Mul(qD, _localAxisD);
Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
JvBD = _ratio * u;
JwD = _ratio * MathUtils.Cross(rD, u);
JwB = _ratio * MathUtils.Cross(rB, u);
mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB;
Vector2 pD = _localAnchorD - _lcD;
Vector2 pB = MathUtils.MulT(qD, rB + (cB - cD));
coordinateB = Vector2.Dot(pB - pD, _localAxisD);
}
float C = (coordinateA + _ratio * coordinateB) - _constant;
float impulse = 0.0f;
if (mass > 0.0f)
{
impulse = -C / mass;
}
cA += _mA * impulse * JvAC;
aA += _iA * impulse * JwA;
cB += _mB * impulse * JvBD;
aB += _iB * impulse * JwB;
cC -= _mC * impulse * JvAC;
aC -= _iC * impulse * JwC;
cD -= _mD * impulse * JvBD;
aD -= _iD * impulse * JwD;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
data.positions[_indexC].c = cC;
data.positions[_indexC].a = aC;
data.positions[_indexD].c = cD;
data.positions[_indexD].a = aD;
// TODO_ERIN not implemented
return linearError < Settings.LinearSlop;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/Joint.cs
+253
View File
@@ -0,0 +1,253 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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 Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
public enum JointType
{
Unknown,
Revolute,
Prismatic,
Distance,
Pulley,
//Mouse, <- We have fixed mouse
Gear,
Wheel,
Weld,
Friction,
Rope,
Motor,
//FPE note: From here on and down, it is only FPE joints
Angle,
FixedMouse,
FixedRevolute,
FixedDistance,
FixedLine,
FixedPrismatic,
FixedAngle,
FixedFriction,
}
public enum LimitState
{
Inactive,
AtLower,
AtUpper,
Equal,
}
/// <summary>
/// A joint edge is used to connect bodies and joints together
/// in a joint graph where each body is a node and each joint
/// is an edge. A joint edge belongs to a doubly linked list
/// maintained in each attached body. Each joint has two joint
/// nodes, one for each attached body.
/// </summary>
public sealed class JointEdge
{
/// <summary>
/// The joint.
/// </summary>
public Joint Joint;
/// <summary>
/// The next joint edge in the body's joint list.
/// </summary>
public JointEdge Next;
/// <summary>
/// Provides quick access to the other body attached.
/// </summary>
public Body Other;
/// <summary>
/// The previous joint edge in the body's joint list.
/// </summary>
public JointEdge Prev;
}
public abstract class Joint
{
private float _breakpoint;
private double _breakpointSquared;
/// <summary>
/// Indicate if this join is enabled or not. Disabling a joint
/// means it is still in the simulation, but inactive.
/// </summary>
public bool Enabled = true;
internal JointEdge EdgeA = new JointEdge();
internal JointEdge EdgeB = new JointEdge();
internal bool IslandFlag;
protected Joint()
{
Breakpoint = float.MaxValue;
//Connected bodies should not collide by default
CollideConnected = false;
}
protected Joint(Body bodyA, Body bodyB) : this()
{
//Can't connect a joint to the same body twice.
Debug.Assert(bodyA != bodyB);
BodyA = bodyA;
BodyB = bodyB;
}
/// <summary>
/// Constructor for fixed joint
/// </summary>
protected Joint(Body body) : this()
{
BodyA = body;
}
/// <summary>
/// Gets or sets the type of the joint.
/// </summary>
/// <value>The type of the joint.</value>
public JointType JointType { get; protected set; }
/// <summary>
/// Get the first body attached to this joint.
/// </summary>
public Body BodyA { get; internal set; }
/// <summary>
/// Get the second body attached to this joint.
/// </summary>
public Body BodyB { get; internal set; }
/// <summary>
/// Get the anchor point on bodyA in world coordinates.
/// On some joints, this value indicate the anchor point within the world.
/// </summary>
public abstract Vector2 WorldAnchorA { get; set; }
/// <summary>
/// Get the anchor point on bodyB in world coordinates.
/// On some joints, this value indicate the anchor point within the world.
/// </summary>
public abstract Vector2 WorldAnchorB { get; set; }
/// <summary>
/// Set the user data pointer.
/// </summary>
/// <value>The data.</value>
public object UserData { get; set; }
/// <summary>
/// Set this flag to true if the attached bodies should collide.
/// </summary>
public bool CollideConnected { get; set; }
/// <summary>
/// The Breakpoint simply indicates the maximum Value the JointError can be before it breaks.
/// The default value is float.MaxValue, which means it never breaks.
/// </summary>
public float Breakpoint
{
get { return _breakpoint; }
set
{
_breakpoint = value;
_breakpointSquared = _breakpoint * _breakpoint;
}
}
/// <summary>
/// Fires when the joint is broken.
/// </summary>
public event Action<Joint, float> Broke;
/// <summary>
/// Get the reaction force on body at the joint anchor in Newtons.
/// </summary>
/// <param name="invDt">The inverse delta time.</param>
public abstract Vector2 GetReactionForce(float invDt);
/// <summary>
/// Get the reaction torque on the body at the joint anchor in N*m.
/// </summary>
/// <param name="invDt">The inverse delta time.</param>
public abstract float GetReactionTorque(float invDt);
protected void WakeBodies()
{
if (BodyA != null)
BodyA.Awake = true;
if (BodyB != null)
BodyB.Awake = true;
}
/// <summary>
/// Return true if the joint is a fixed type.
/// </summary>
public bool IsFixedType()
{
return JointType == JointType.FixedRevolute ||
JointType == JointType.FixedDistance ||
JointType == JointType.FixedPrismatic ||
JointType == JointType.FixedLine ||
JointType == JointType.FixedMouse ||
JointType == JointType.FixedAngle ||
JointType == JointType.FixedFriction;
}
internal abstract void InitVelocityConstraints(ref SolverData data);
internal void Validate(float invDt)
{
if (!Enabled)
return;
float jointErrorSquared = GetReactionForce(invDt).LengthSquared();
if (Math.Abs(jointErrorSquared) <= _breakpointSquared)
return;
Enabled = false;
if (Broke != null)
Broke(this, (float)Math.Sqrt(jointErrorSquared));
}
internal abstract void SolveVelocityConstraints(ref SolverData data);
/// <summary>
/// Solves the position constraints.
/// </summary>
/// <param name="data"></param>
/// <returns>returns true if the position errors are within tolerance.</returns>
internal abstract bool SolvePositionConstraints(ref SolverData data);
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/MotorJoint.cs
+320
View File
@@ -0,0 +1,320 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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.Diagnostics;
using FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// A motor joint is used to control the relative motion
/// between two bodies. A typical usage is to control the movement
/// of a dynamic body with respect to the ground.
/// </summary>
public class MotorJoint : Joint
{
// Solver shared
private Vector2 _linearOffset;
private float _angularOffset;
private Vector2 _linearImpulse;
private float _angularImpulse;
private float _maxForce;
private float _maxTorque;
// Solver temp
private int _indexA;
private int _indexB;
private Vector2 _rA;
private Vector2 _rB;
private Vector2 _localCenterA;
private Vector2 _localCenterB;
private Vector2 _linearError;
private float _angularError;
private float _invMassA;
private float _invMassB;
private float _invIA;
private float _invIB;
private Mat22 _linearMass;
private float _angularMass;
internal MotorJoint()
{
JointType = JointType.Motor;
}
/// <summary>
/// Constructor for MotorJoint.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
public MotorJoint(Body bodyA, Body bodyB, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
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;
}
public override Vector2 WorldAnchorA
{
get { return BodyA.Position; }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.Position; }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
/// <summary>
/// The maximum amount of force that can be applied to BodyA
/// </summary>
public float MaxForce
{
set
{
Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f);
_maxForce = value;
}
get { return _maxForce; }
}
/// <summary>
/// The maximum amount of torque that can be applied to BodyA
/// </summary>
public float MaxTorque
{
set
{
Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f);
_maxTorque = value;
}
get { return _maxTorque; }
}
/// <summary>
/// The linear (translation) offset.
/// </summary>
public Vector2 LinearOffset
{
set
{
if (_linearOffset.X != value.X || _linearOffset.Y != value.Y)
{
WakeBodies();
_linearOffset = value;
}
}
get { return _linearOffset; }
}
/// <summary>
/// Get or set the angular offset.
/// </summary>
public float AngularOffset
{
set
{
if (_angularOffset != value)
{
WakeBodies();
_angularOffset = value;
}
}
get { return _angularOffset; }
}
//FPE note: Used for serialization.
internal float CorrectionFactor { get; set; }
public override Vector2 GetReactionForce(float invDt)
{
return invDt * _linearImpulse;
}
public override float GetReactionTorque(float invDt)
{
return invDt * _angularImpulse;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA);
Rot qB = new Rot(aB);
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, -_localCenterA);
_rB = MathUtils.Mul(qB, -_localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Mat22 K = new Mat22();
K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y;
K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X;
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
_linearError = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset);
_angularError = aB - aA - _angularOffset;
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.step.dtRatio;
_angularImpulse *= data.step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
float h = data.step.dt;
float inv_h = data.step.inv_dt;
// Solve angular friction
{
float Cdot = wB - wA + inv_h * CorrectionFactor * _angularError;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = h * _maxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA) + inv_h * CorrectionFactor * _linearError;
Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = h * _maxForce;
if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(_rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(_rB, impulse);
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
return true;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/PrismaticJoint.cs
+768
View File
@@ -0,0 +1,768 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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
{
// Linear constraint (point-to-line)
// d = p2 - p1 = x2 + r2 - x1 - r1
// C = dot(perp, d)
// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
//
// Angular constraint
// C = a2 - a1 + a_initial
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
//
// K = J * invM * JT
//
// J = [-a -s1 a s2]
// [0 -1 0 1]
// a = perp
// s1 = cross(d + r1, a) = cross(p2 - x1, a)
// s2 = cross(r2, a) = cross(p2 - x2, a)
// Motor/Limit linear constraint
// C = dot(ax1, d)
// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
// Block Solver
// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
// when the mass has poor distribution (leading to large torques about the joint anchor points).
//
// The Jacobian has 3 rows:
// J = [-uT -s1 uT s2] // linear
// [0 -1 0 1] // angular
// [-vT -a1 vT a2] // limit
//
// u = perp
// v = axis
// s1 = cross(d + r1, u), s2 = cross(r2, u)
// a1 = cross(d + r1, v), a2 = cross(r2, v)
// M * (v2 - v1) = JT * df
// J * v2 = bias
//
// v2 = v1 + invM * JT * df
// J * (v1 + invM * JT * df) = bias
// K * df = bias - J * v1 = -Cdot
// K = J * invM * JT
// Cdot = J * v1 - bias
//
// Now solve for f2.
// df = f2 - f1
// K * (f2 - f1) = -Cdot
// f2 = invK * (-Cdot) + f1
//
// Clamp accumulated limit impulse.
// lower: f2(3) = max(f2(3), 0)
// upper: f2(3) = min(f2(3), 0)
//
// Solve for correct f2(1:2)
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
//
// Now compute impulse to be applied:
// df = f2 - f1
/// <summary>
/// A prismatic joint. This joint provides one degree of freedom: translation
/// along an axis fixed in bodyA. Relative rotation is prevented. You can
/// use a joint limit to restrict the range of motion and a joint motor to
/// drive the motion or to model joint friction.
/// </summary>
public class PrismaticJoint : Joint
{
private Vector2 _localYAxisA;
private Vector3 _impulse;
private float _lowerTranslation;
private float _upperTranslation;
private float _maxMotorForce;
private float _motorSpeed;
private bool _enableLimit;
private bool _enableMotor;
private LimitState _limitState;
// Solver temp
private int _indexA;
private int _indexB;
private Vector2 _localCenterA;
private Vector2 _localCenterB;
private float _invMassA;
private float _invMassB;
private float _invIA;
private float _invIB;
private Vector2 _axis, _perp;
private float _s1, _s2;
private float _a1, _a2;
private Mat33 _K;
private float _motorMass;
private Vector2 _axis1;
internal PrismaticJoint()
{
JointType = JointType.Prismatic;
}
/// <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="anchorA">The first body anchor.</param>
/// <param name="anchorB">The second body anchor.</param>
/// <param name="axis">The axis.</param>
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
public PrismaticJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 axis, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
Initialize(anchorA, anchorB, axis, useWorldCoordinates);
}
public PrismaticJoint(Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
Initialize(anchor, anchor, axis, useWorldCoordinates);
}
private void Initialize(Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis, bool useWorldCoordinates)
{
JointType = JointType.Prismatic;
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(localAnchorA);
LocalAnchorB = BodyB.GetLocalPoint(localAnchorB);
}
else
{
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
}
Axis = axis; //FPE only: store the orignal value for use in Serialization
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
_limitState = LimitState.Inactive;
}
/// <summary>
/// The local anchor point on BodyA
/// </summary>
public Vector2 LocalAnchorA { get; set; }
/// <summary>
/// The local anchor point on BodyB
/// </summary>
public Vector2 LocalAnchorB { get; set; }
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
}
/// <summary>
/// Get the current joint translation, usually in meters.
/// </summary>
/// <value></value>
public float JointTranslation
{
get
{
Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA);
Vector2 axis = BodyA.GetWorldVector(LocalXAxis);
return Vector2.Dot(d, axis);
}
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
/// <value></value>
public float JointSpeed
{
get
{
Transform xf1, xf2;
BodyA.GetTransform(out xf1);
BodyB.GetTransform(out xf2);
Vector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - BodyA.LocalCenter);
Vector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - BodyB.LocalCenter);
Vector2 p1 = BodyA._sweep.C + r1;
Vector2 p2 = BodyB._sweep.C + r2;
Vector2 d = p2 - p1;
Vector2 axis = BodyA.GetWorldVector(LocalXAxis);
Vector2 v1 = BodyA._linearVelocity;
Vector2 v2 = BodyB._linearVelocity;
float w1 = BodyA._angularVelocity;
float w2 = BodyB._angularVelocity;
float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
return speed;
}
}
/// <summary>
/// Is the joint limit enabled?
/// </summary>
/// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
public bool LimitEnabled
{
get { return _enableLimit; }
set
{
Debug.Assert(BodyA.FixedRotation == false || BodyB.FixedRotation == false, "Warning: limits does currently not work with fixed rotation");
if (value != _enableLimit)
{
WakeBodies();
_enableLimit = value;
_impulse.Z = 0;
}
}
}
/// <summary>
/// Get the lower joint limit, usually in meters.
/// </summary>
/// <value></value>
public float LowerLimit
{
get { return _lowerTranslation; }
set
{
if (value != _lowerTranslation)
{
WakeBodies();
_lowerTranslation = value;
_impulse.Z = 0.0f;
}
}
}
/// <summary>
/// Get the upper joint limit, usually in meters.
/// </summary>
/// <value></value>
public float UpperLimit
{
get { return _upperTranslation; }
set
{
if (value != _upperTranslation)
{
WakeBodies();
_upperTranslation = value;
_impulse.Z = 0.0f;
}
}
}
/// <summary>
/// Set the joint limits, usually in meters.
/// </summary>
/// <param name="lower">The lower limit</param>
/// <param name="upper">The upper limit</param>
public void SetLimits(float lower, float upper)
{
if (upper != _upperTranslation || lower != _lowerTranslation)
{
WakeBodies();
_upperTranslation = upper;
_lowerTranslation = lower;
_impulse.Z = 0.0f;
}
}
/// <summary>
/// Is the joint motor enabled?
/// </summary>
/// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
WakeBodies();
_enableMotor = value;
}
}
/// <summary>
/// Set the motor speed, usually in meters per second.
/// </summary>
/// <value>The speed.</value>
public float MotorSpeed
{
set
{
WakeBodies();
_motorSpeed = value;
}
get { return _motorSpeed; }
}
/// <summary>
/// Set the maximum motor force, usually in N.
/// </summary>
/// <value>The force.</value>
public float MaxMotorForce
{
get { return _maxMotorForce; }
set
{
WakeBodies();
_maxMotorForce = value;
}
}
/// <summary>
/// Get the current motor impulse, usually in N.
/// </summary>
/// <value></value>
public float MotorImpulse { get; set; }
/// <summary>
/// Gets the motor force.
/// </summary>
/// <param name="invDt">The inverse delta time</param>
public float GetMotorForce(float invDt)
{
return invDt * MotorImpulse;
}
/// <summary>
/// The axis at which the joint moves.
/// </summary>
public Vector2 Axis
{
get { return _axis1; }
set
{
_axis1 = value;
LocalXAxis = BodyA.GetLocalVector(_axis1);
LocalXAxis.Normalize();
_localYAxisA = MathUtils.Cross(1.0f, LocalXAxis);
}
}
/// <summary>
/// The axis in local coordinates relative to BodyA
/// </summary>
public Vector2 LocalXAxis { get; private set; }
/// <summary>
/// The reference angle.
/// </summary>
public float ReferenceAngle { get; set; }
public override Vector2 GetReactionForce(float invDt)
{
return invDt * (_impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis);
}
public override float GetReactionTorque(float invDt)
{
return invDt * _impulse.Y;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective masses.
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = (cB - cA) + rB - rA;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Mul(qA, LocalXAxis);
_a1 = MathUtils.Cross(d + rA, _axis);
_a2 = MathUtils.Cross(rB, _axis);
_motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Mul(qA, _localYAxisA);
_s1 = MathUtils.Cross(d + rA, _perp);
_s2 = MathUtils.Cross(rB, _perp);
float k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
float k12 = iA * _s1 + iB * _s2;
float k13 = iA * _s1 * _a1 + iB * _s2 * _a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * _a1 + iB * _a2;
float k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
_K.ex = new Vector3(k11, k12, k13);
_K.ey = new Vector3(k12, k22, k23);
_K.ez = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
if (_enableMotor == false)
{
MotorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
MotorImpulse *= data.step.dtRatio;
Vector2 P = _impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis;
float LA = _impulse.X * _s1 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a1;
float LB = _impulse.X * _s2 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
_impulse = Vector3.Zero;
MotorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = MotorImpulse;
float maxImpulse = data.step.dt * _maxMotorForce;
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = MotorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float LA = impulse * _a1;
float LB = impulse * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
Vector2 Cdot1 = new Vector2();
Cdot1.X = Vector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA;
Cdot1.Y = wB - wA;
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
Cdot2 = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.ez.X, _K.ez.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float LA = df.X * _s1 + df.Y + df.Z * _a1;
float LB = df.X * _s2 + df.Y + df.Z * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float LA = df.X * _s1 + df.Y;
float LB = df.X * _s2 + df.Y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(rB, axis);
Vector2 perp = MathUtils.Mul(qA, _localYAxisA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(rB, perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.X = Vector2.Dot(perp, d);
C1.Y = aB - aA - ReferenceAngle;
float linearError = Math.Abs(C1.X);
float angularError = Math.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (_enableLimit)
{
float translation = Vector2.Dot(axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, Math.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k13 = iA * s1 * a1 + iB * s2 * a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new Vector3(k11, k12, k13);
K.ey = new Vector3(k12, k22, k23);
K.ez = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = K.Solve33(-C);
}
else
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new Vector2(k11, k12);
K.ey = new Vector2(k12, k22);
Vector2 impulse1 = K.Solve(-C1);
impulse = new Vector3();
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * perp + impulse.Z * axis;
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/PulleyJoint.cs
+396
View File
@@ -0,0 +1,396 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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
{
// Pulley:
// length1 = norm(p1 - s1)
// length2 = norm(p2 - s2)
// C0 = (length1 + ratio * length2)_initial
// C = C0 - (length1 + ratio * length2)
// u1 = (p1 - s1) / norm(p1 - s1)
// u2 = (p2 - s2) / norm(p2 - s2)
// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
/// <summary>
/// The pulley joint is connected to two bodies and two fixed world points.
/// The pulley supports a ratio such that:
/// <![CDATA[length1 + ratio * length2 <= constant]]>
/// Yes, the force transmitted is scaled by the ratio.
///
/// Warning: the pulley joint can get a bit squirrelly by itself. They often
/// work better when combined with prismatic joints. You should also cover the
/// the anchor points with static shapes to prevent one side from going to zero length.
/// </summary>
public class PulleyJoint : Joint
{
// Solver shared
private float _impulse;
// Solver temp
private int _indexA;
private int _indexB;
private Vector2 _uA;
private Vector2 _uB;
private Vector2 _rA;
private Vector2 _rB;
private Vector2 _localCenterA;
private Vector2 _localCenterB;
private float _invMassA;
private float _invMassB;
private float _invIA;
private float _invIB;
private float _mass;
internal PulleyJoint()
{
JointType = JointType.Pulley;
}
/// <summary>
/// Constructor for PulleyJoint.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="anchorA">The anchor on the first body.</param>
/// <param name="anchorB">The anchor on the second body.</param>
/// <param name="worldAnchorA">The world anchor for the first body.</param>
/// <param name="worldAnchorB">The world anchor for the second body.</param>
/// <param name="ratio">The ratio.</param>
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
public PulleyJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 worldAnchorA, Vector2 worldAnchorB, float ratio, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
JointType = JointType.Pulley;
WorldAnchorA = worldAnchorA;
WorldAnchorB = worldAnchorB;
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(anchorA);
LocalAnchorB = BodyB.GetLocalPoint(anchorB);
Vector2 dA = anchorA - worldAnchorA;
LengthA = dA.Length();
Vector2 dB = anchorB - worldAnchorB;
LengthB = dB.Length();
}
else
{
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
Vector2 dA = anchorA - BodyA.GetLocalPoint(worldAnchorA);
LengthA = dA.Length();
Vector2 dB = anchorB - BodyB.GetLocalPoint(worldAnchorB);
LengthB = dB.Length();
}
Debug.Assert(ratio != 0.0f);
Debug.Assert(ratio > Settings.Epsilon);
Ratio = ratio;
Constant = LengthA + ratio * LengthB;
_impulse = 0.0f;
}
/// <summary>
/// The local anchor point on BodyA
/// </summary>
public Vector2 LocalAnchorA { get; set; }
/// <summary>
/// The local anchor point on BodyB
/// </summary>
public Vector2 LocalAnchorB { get; set; }
/// <summary>
/// Get the first world anchor.
/// </summary>
/// <value></value>
public override sealed Vector2 WorldAnchorA { get; set; }
/// <summary>
/// Get the second world anchor.
/// </summary>
/// <value></value>
public override sealed Vector2 WorldAnchorB { get; set; }
/// <summary>
/// Get the current length of the segment attached to body1.
/// </summary>
/// <value></value>
public float LengthA { get; set; }
/// <summary>
/// Get the current length of the segment attached to body2.
/// </summary>
/// <value></value>
public float LengthB { get; set; }
/// <summary>
/// The current length between the anchor point on BodyA and WorldAnchorA
/// </summary>
public float CurrentLengthA
{
get
{
Vector2 p = BodyA.GetWorldPoint(LocalAnchorA);
Vector2 s = WorldAnchorA;
Vector2 d = p - s;
return d.Length();
}
}
/// <summary>
/// The current length between the anchor point on BodyB and WorldAnchorB
/// </summary>
public float CurrentLengthB
{
get
{
Vector2 p = BodyB.GetWorldPoint(LocalAnchorB);
Vector2 s = WorldAnchorB;
Vector2 d = p - s;
return d.Length();
}
}
/// <summary>
/// Get the pulley ratio.
/// </summary>
/// <value></value>
public float Ratio { get; set; }
//FPE note: Only used for serialization.
internal float Constant { get; set; }
public override Vector2 GetReactionForce(float invDt)
{
Vector2 P = _impulse * _uB;
return invDt * P;
}
public override float GetReactionTorque(float invDt)
{
return 0.0f;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// Get the pulley axes.
_uA = cA + _rA - WorldAnchorA;
_uB = cB + _rB - WorldAnchorB;
float lengthA = _uA.Length();
float lengthB = _uB.Length();
if (lengthA > 10.0f * Settings.LinearSlop)
{
_uA *= 1.0f / lengthA;
}
else
{
_uA = Vector2.Zero;
}
if (lengthB > 10.0f * Settings.LinearSlop)
{
_uB *= 1.0f / lengthB;
}
else
{
_uB = Vector2.Zero;
}
// Compute effective mass.
float ruA = MathUtils.Cross(_rA, _uA);
float ruB = MathUtils.Cross(_rB, _uB);
float mA = _invMassA + _invIA * ruA * ruA;
float mB = _invMassB + _invIB * ruB * ruB;
_mass = mA + Ratio * Ratio * mB;
if (_mass > 0.0f)
{
_mass = 1.0f / _mass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support variable time steps.
_impulse *= data.step.dtRatio;
// Warm starting.
Vector2 PA = -(_impulse) * _uA;
Vector2 PB = (-Ratio * _impulse) * _uB;
vA += _invMassA * PA;
wA += _invIA * MathUtils.Cross(_rA, PA);
vB += _invMassB * PB;
wB += _invIB * MathUtils.Cross(_rB, PB);
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
float Cdot = -Vector2.Dot(_uA, vpA) - Ratio * Vector2.Dot(_uB, vpB);
float impulse = -_mass * Cdot;
_impulse += impulse;
Vector2 PA = -impulse * _uA;
Vector2 PB = -Ratio * impulse * _uB;
vA += _invMassA * PA;
wA += _invIA * MathUtils.Cross(_rA, PA);
vB += _invMassB * PB;
wB += _invIB * MathUtils.Cross(_rB, PB);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// Get the pulley axes.
Vector2 uA = cA + rA - WorldAnchorA;
Vector2 uB = cB + rB - WorldAnchorB;
float lengthA = uA.Length();
float lengthB = uB.Length();
if (lengthA > 10.0f * Settings.LinearSlop)
{
uA *= 1.0f / lengthA;
}
else
{
uA = Vector2.Zero;
}
if (lengthB > 10.0f * Settings.LinearSlop)
{
uB *= 1.0f / lengthB;
}
else
{
uB = Vector2.Zero;
}
// Compute effective mass.
float ruA = MathUtils.Cross(rA, uA);
float ruB = MathUtils.Cross(rB, uB);
float mA = _invMassA + _invIA * ruA * ruA;
float mB = _invMassB + _invIB * ruB * ruB;
float mass = mA + Ratio * Ratio * mB;
if (mass > 0.0f)
{
mass = 1.0f / mass;
}
float C = Constant - lengthA - Ratio * lengthB;
float linearError = Math.Abs(C);
float impulse = -mass * C;
Vector2 PA = -impulse * uA;
Vector2 PB = -Ratio * impulse * uB;
cA += _invMassA * PA;
aA += _invIA * MathUtils.Cross(rA, PA);
cB += _invMassB * PB;
aB += _invIB * MathUtils.Cross(rB, PB);
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return linearError < Settings.LinearSlop;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/RevoluteJoint.cs
+619
View File
@@ -0,0 +1,619 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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 FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// A revolute joint constrains to bodies to share a common point while they
/// are free to rotate about the point. The relative rotation about the shared
/// point is the joint angle. You can limit the relative rotation with
/// a joint limit that specifies a lower and upper angle. You can use a motor
/// to drive the relative rotation about the shared point. A maximum motor torque
/// is provided so that infinite forces are not generated.
/// </summary>
public class RevoluteJoint : Joint
{
// Solver shared
private Vector3 _impulse;
private float _motorImpulse;
private bool _enableMotor;
private float _maxMotorTorque;
private float _motorSpeed;
private bool _enableLimit;
private float _referenceAngle;
private float _lowerAngle;
private float _upperAngle;
// Solver temp
private int _indexA;
private int _indexB;
private Vector2 _rA;
private Vector2 _rB;
private Vector2 _localCenterA;
private Vector2 _localCenterB;
private float _invMassA;
private float _invMassB;
private float _invIA;
private float _invIB;
private Mat33 _mass; // effective mass for point-to-point constraint.
private float _motorMass; // effective mass for motor/limit angular constraint.
private LimitState _limitState;
internal RevoluteJoint()
{
JointType = JointType.Revolute;
}
/// <summary>
/// Constructor of RevoluteJoint.
/// </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 anchor.</param>
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
public RevoluteJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
JointType = JointType.Revolute;
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(anchorA);
LocalAnchorB = BodyB.GetLocalPoint(anchorB);
}
else
{
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
}
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
_impulse = Vector3.Zero;
_limitState = LimitState.Inactive;
}
/// <summary>
/// Constructor of RevoluteJoint.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="anchor">The shared anchor.</param>
/// <param name="useWorldCoordinates"></param>
public RevoluteJoint(Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false)
: this(bodyA, bodyB, anchor, anchor, useWorldCoordinates)
{
}
/// <summary>
/// The local anchor point on BodyA
/// </summary>
public Vector2 LocalAnchorA { get; set; }
/// <summary>
/// The local anchor point on BodyB
/// </summary>
public Vector2 LocalAnchorB { get; set; }
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
}
/// <summary>
/// The referance angle computed as BodyB angle minus BodyA angle.
/// </summary>
public float ReferenceAngle
{
get { return _referenceAngle; }
set
{
WakeBodies();
_referenceAngle = value;
}
}
/// <summary>
/// Get the current joint angle in radians.
/// </summary>
public float JointAngle
{
get { return BodyB._sweep.A - BodyA._sweep.A - ReferenceAngle; }
}
/// <summary>
/// Get the current joint angle speed in radians per second.
/// </summary>
public float JointSpeed
{
get { return BodyB._angularVelocity - BodyA._angularVelocity; }
}
/// <summary>
/// Is the joint limit enabled?
/// </summary>
/// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
public bool LimitEnabled
{
get { return _enableLimit; }
set
{
if (_enableLimit != value)
{
WakeBodies();
_enableLimit = value;
_impulse.Z = 0.0f;
}
}
}
/// <summary>
/// Get the lower joint limit in radians.
/// </summary>
public float LowerLimit
{
get { return _lowerAngle; }
set
{
if (_lowerAngle != value)
{
WakeBodies();
_lowerAngle = value;
_impulse.Z = 0.0f;
}
}
}
/// <summary>
/// Get the upper joint limit in radians.
/// </summary>
public float UpperLimit
{
get { return _upperAngle; }
set
{
if (_upperAngle != value)
{
WakeBodies();
_upperAngle = value;
_impulse.Z = 0.0f;
}
}
}
/// <summary>
/// Set the joint limits, usually in meters.
/// </summary>
/// <param name="lower">The lower limit</param>
/// <param name="upper">The upper limit</param>
public void SetLimits(float lower, float upper)
{
if (lower != _lowerAngle || upper != _upperAngle)
{
WakeBodies();
_upperAngle = upper;
_lowerAngle = lower;
_impulse.Z = 0.0f;
}
}
/// <summary>
/// Is the joint motor enabled?
/// </summary>
/// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
WakeBodies();
_enableMotor = value;
}
}
/// <summary>
/// Get or set the motor speed in radians per second.
/// </summary>
public float MotorSpeed
{
set
{
WakeBodies();
_motorSpeed = value;
}
get { return _motorSpeed; }
}
/// <summary>
/// Get or set the maximum motor torque, usually in N-m.
/// </summary>
public float MaxMotorTorque
{
set
{
WakeBodies();
_maxMotorTorque = value;
}
get { return _maxMotorTorque; }
}
/// <summary>
/// Get or set the current motor impulse, usually in N-m.
/// </summary>
public float MotorImpulse
{
get { return _motorImpulse; }
set
{
WakeBodies();
_motorImpulse = value;
}
}
/// <summary>
/// Gets the motor torque in N-m.
/// </summary>
/// <param name="invDt">The inverse delta time</param>
public float GetMotorTorque(float invDt)
{
return invDt * _motorImpulse;
}
public override Vector2 GetReactionForce(float invDt)
{
Vector2 p = new Vector2(_impulse.X, _impulse.Y);
return invDt * p;
}
public override float GetReactionTorque(float invDt)
{
return invDt * _impulse.Z;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
_mass.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
_mass.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
_mass.ez.X = -_rA.Y * iA - _rB.Y * iB;
_mass.ex.Y = _mass.ey.X;
_mass.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
_mass.ez.Y = _rA.X * iA + _rB.X * iB;
_mass.ex.Z = _mass.ez.X;
_mass.ey.Z = _mass.ez.Y;
_mass.ez.Z = iA + iB;
_motorMass = iA + iB;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false || fixedRotation)
{
_motorImpulse = 0.0f;
}
if (_enableLimit && fixedRotation == false)
{
float jointAngle = aB - aA - ReferenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
_motorImpulse *= data.step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false)
{
float Cdot = wB - wA - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = data.step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = -_mass.Solve33(Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y);
Vector2 reduced = _mass.Solve22(rhs);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
else
{
_impulse += impulse;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y);
Vector2 reduced = _mass.Solve22(rhs);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
else
{
_impulse += impulse;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z);
}
else
{
// Solve point-to-point constraint
Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(_rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(_rB, impulse);
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float angularError = 0.0f;
float positionError;
bool fixedRotation = (_invIA + _invIB == 0.0f);
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
float angle = aB - aA - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
aA -= _invIA * limitImpulse;
aB += _invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 C = cB + rB - cA - rA;
positionError = C.Length();
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Mat22 K = new Mat22();
K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;
Vector2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * MathUtils.Cross(rA, impulse);
cB += mB * impulse;
aB += iB * MathUtils.Cross(rB, impulse);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/RopeJoint.cs
+291
View File
@@ -0,0 +1,291 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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 FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Limit:
// C = norm(pB - pA) - L
// u = (pB - pA) / norm(pB - pA)
// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))
// J = [-u -cross(rA, u) u cross(rB, u)]
// K = J * invM * JT
// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2
/// <summary>
/// A rope joint enforces a maximum distance between two points on two bodies. It has no other effect.
/// It can be used on ropes that are made up of several connected bodies, and if there is a need to support a heavy body.
/// This joint is used for stabiliation of heavy objects on soft constraint joints.
///
/// Warning: if you attempt to change the maximum length during the simulation you will get some non-physical behavior.
/// Use the DistanceJoint instead if you want to dynamically control the length.
/// </summary>
public class RopeJoint : Joint
{
// Solver shared
private float _impulse;
private float _length;
// Solver temp
private int _indexA;
private int _indexB;
private Vector2 _localCenterA;
private Vector2 _localCenterB;
private float _invMassA;
private float _invMassB;
private float _invIA;
private float _invIB;
private float _mass;
private Vector2 _rA, _rB;
private Vector2 _u;
internal RopeJoint()
{
JointType = JointType.Rope;
}
/// <summary>
/// Constructor for RopeJoint.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="anchorA">The anchor on the first body</param>
/// <param name="anchorB">The anchor on the second body</param>
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
public RopeJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
JointType = JointType.Rope;
if (useWorldCoordinates)
{
LocalAnchorA = bodyA.GetLocalPoint(anchorA);
LocalAnchorB = bodyB.GetLocalPoint(anchorB);
}
else
{
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
}
//FPE feature: Setting default MaxLength
Vector2 d = WorldAnchorB - WorldAnchorA;
MaxLength = d.Length();
}
/// <summary>
/// The local anchor point on BodyA
/// </summary>
public Vector2 LocalAnchorA { get; set; }
/// <summary>
/// The local anchor point on BodyB
/// </summary>
public Vector2 LocalAnchorB { get; set; }
public override sealed Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
}
public override sealed Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
}
/// <summary>
/// Get or set the maximum length of the rope.
/// By default, it is the distance between the two anchor points.
/// </summary>
public float MaxLength { get; set; }
/// <summary>
/// Gets the state of the joint.
/// </summary>
public LimitState State { get; private set; }
public override Vector2 GetReactionForce(float invDt)
{
return (invDt * _impulse) * _u;
}
public override float GetReactionTorque(float invDt)
{
return 0;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
_u = cB + _rB - cA - _rA;
_length = _u.Length();
float C = _length - MaxLength;
if (C > 0.0f)
{
State = LimitState.AtUpper;
}
else
{
State = LimitState.Inactive;
}
if (_length > Settings.LinearSlop)
{
_u *= 1.0f / _length;
}
else
{
_u = Vector2.Zero;
_mass = 0.0f;
_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = MathUtils.Cross(_rA, _u);
float crB = MathUtils.Cross(_rB, _u);
float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = _impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
// Cdot = dot(u, v + cross(w, r))
Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
float C = _length - MaxLength;
float Cdot = Vector2.Dot(_u, vpB - vpA);
// Predictive constraint.
if (C < 0.0f)
{
Cdot += data.step.inv_dt * C;
}
float impulse = -_mass * Cdot;
float oldImpulse = _impulse;
_impulse = Math.Min(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
Vector2 P = impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 u = cB + rB - cA - rA;
float length = u.Length(); u.Normalize();
float C = length - MaxLength;
C = MathUtils.Clamp(C, 0.0f, Settings.MaxLinearCorrection);
float impulse = -_mass * C;
Vector2 P = impulse * u;
cA -= _invMassA * P;
aA -= _invIA * MathUtils.Cross(rA, P);
cB += _invMassB * P;
aB += _invIB * MathUtils.Cross(rB, P);
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return length - MaxLength < Settings.LinearSlop;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/WeldJoint.cs
+388
View File
@@ -0,0 +1,388 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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 FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Point-to-point constraint
// C = p2 - p1
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// C = angle2 - angle1 - referenceAngle
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
/// <summary>
/// A weld joint essentially glues two bodies together. A weld joint may
/// distort somewhat because the island constraint solver is approximate.
///
/// The joint is soft constraint based, which means the two bodies will move
/// relative to each other, when a force is applied. To combine two bodies
/// in a rigid fashion, combine the fixtures to a single body instead.
/// </summary>
public class WeldJoint : Joint
{
// Solver shared
private Vector3 _impulse;
private float _gamma;
private float _bias;
// Solver temp
private int _indexA;
private int _indexB;
private Vector2 _rA;
private Vector2 _rB;
private Vector2 _localCenterA;
private Vector2 _localCenterB;
private float _invMassA;
private float _invMassB;
private float _invIA;
private float _invIB;
private Mat33 _mass;
internal WeldJoint()
{
JointType = JointType.Weld;
}
/// <summary>
/// You need to specify an anchor point where they are attached.
/// The position of the anchor point is important for computing the reaction torque.
/// </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 WeldJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
JointType = JointType.Weld;
if (useWorldCoordinates)
{
LocalAnchorA = bodyA.GetLocalPoint(anchorA);
LocalAnchorB = bodyB.GetLocalPoint(anchorB);
}
else
{
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
}
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
}
/// <summary>
/// The local anchor point on BodyA
/// </summary>
public Vector2 LocalAnchorA { get; set; }
/// <summary>
/// The local anchor point on BodyB
/// </summary>
public Vector2 LocalAnchorB { get; set; }
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
}
/// <summary>
/// The bodyB angle minus bodyA angle in the reference state (radians).
/// </summary>
public float ReferenceAngle { get; set; }
/// <summary>
/// The frequency of the joint. A higher frequency means a stiffer joint, but
/// a too high value can cause the joint to oscillate.
/// Default is 0, which means the joint does no spring calculations.
/// </summary>
public float FrequencyHz { get; set; }
/// <summary>
/// The damping on the joint. The damping is only used when
/// the joint has a frequency (> 0). A higher value means more damping.
/// </summary>
public float DampingRatio { get; set; }
public override Vector2 GetReactionForce(float invDt)
{
return invDt * new Vector2(_impulse.X, _impulse.Y);
}
public override float GetReactionTorque(float invDt)
{
return invDt * _impulse.Z;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Mat33 K = new Mat33();
K.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
K.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
K.ez.X = -_rA.Y * iA - _rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
K.ez.Y = _rA.X * iA + _rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
float invM = iA + iB;
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
float C = aB - aA - ReferenceAngle;
// Frequency
float omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
float d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
float k = m * omega * omega;
// magic formulas
float h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
if (FrequencyHz > 0.0f)
{
float Cdot2 = wB - wA;
float impulse2 = -_mass.ez.Z * (Cdot2 + _bias + _gamma * _impulse.Z);
_impulse.Z += impulse2;
wA -= iA * impulse2;
wB += iB * impulse2;
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 impulse1 = -MathUtils.Mul22(_mass, Cdot1);
_impulse.X += impulse1.X;
_impulse.Y += impulse1.Y;
Vector2 P = impulse1;
vA -= mA * P;
wA -= iA * MathUtils.Cross(_rA, P);
vB += mB * P;
wB += iB * MathUtils.Cross(_rB, P);
}
else
{
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = -MathUtils.Mul(_mass, Cdot);
_impulse += impulse;
Vector2 P = new Vector2(impulse.X, impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z);
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
float positionError, angularError;
Mat33 K = new Mat33();
K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
K.ez.X = -rA.Y * iA - rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
K.ez.Y = rA.X * iA + rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
Vector2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
Vector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
Vector2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - ReferenceAngle;
positionError = C1.Length();
angularError = Math.Abs(C2);
Vector3 C = new Vector3(C1.X, C1.Y, C2);
Vector3 impulse = -K.Solve33(C);
Vector2 P = new Vector2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
}
}
Farseer Physics Engine 3.5/Dynamics/Joints/WheelJoint.cs
+513
View File
@@ -0,0 +1,513 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* 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 FarseerPhysics.Common;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
// Linear constraint (point-to-line)
// d = pB - pA = xB + rB - xA - rA
// C = dot(ay, d)
// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
// Spring linear constraint
// C = dot(ax, d)
// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
// Motor rotational constraint
// Cdot = wB - wA
// J = [0 0 -1 0 0 1]
/// <summary>
/// A wheel joint. This joint provides two degrees of freedom: translation
/// along an axis fixed in bodyA and rotation in the plane. You can use a
/// joint limit to restrict the range of motion and a joint motor to drive
/// the rotation or to model rotational friction.
/// This joint is designed for vehicle suspensions.
/// </summary>
public class WheelJoint : Joint
{
// Solver shared
private Vector2 _localYAxis;
private float _impulse;
private float _motorImpulse;
private float _springImpulse;
private float _maxMotorTorque;
private float _motorSpeed;
private bool _enableMotor;
// Solver temp
private int _indexA;
private int _indexB;
private Vector2 _localCenterA;
private Vector2 _localCenterB;
private float _invMassA;
private float _invMassB;
private float _invIA;
private float _invIB;
private Vector2 _ax, _ay;
private float _sAx, _sBx;
private float _sAy, _sBy;
private float _mass;
private float _motorMass;
private float _springMass;
private float _bias;
private float _gamma;
private Vector2 _axis;
internal WheelJoint()
{
JointType = JointType.Wheel;
}
/// <summary>
/// Constructor for WheelJoint
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="anchor">The anchor point</param>
/// <param name="axis">The axis</param>
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
public WheelJoint(Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
JointType = JointType.Wheel;
if (useWorldCoordinates)
{
LocalAnchorA = bodyA.GetLocalPoint(anchor);
LocalAnchorB = bodyB.GetLocalPoint(anchor);
}
else
{
LocalAnchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(anchor));
LocalAnchorB = anchor;
}
Axis = axis; //FPE only: We maintain the original value as it is supposed to.
}
/// <summary>
/// The local anchor point on BodyA
/// </summary>
public Vector2 LocalAnchorA { get; set; }
/// <summary>
/// The local anchor point on BodyB
/// </summary>
public Vector2 LocalAnchorB { get; set; }
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
set { LocalAnchorA = BodyA.GetLocalPoint(value); }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.GetWorldPoint(LocalAnchorB); }
set { LocalAnchorB = BodyB.GetLocalPoint(value); }
}
/// <summary>
/// The axis at which the suspension moves.
/// </summary>
public Vector2 Axis
{
get { return _axis; }
set
{
_axis = value;
LocalXAxis = BodyA.GetLocalVector(_axis);
_localYAxis = MathUtils.Cross(1.0f, LocalXAxis);
}
}
/// <summary>
/// The axis in local coordinates relative to BodyA
/// </summary>
public Vector2 LocalXAxis { get; private set; }
/// <summary>
/// The desired motor speed in radians per second.
/// </summary>
public float MotorSpeed
{
get { return _motorSpeed; }
set
{
WakeBodies();
_motorSpeed = value;
}
}
/// <summary>
/// The maximum motor torque, usually in N-m.
/// </summary>
public float MaxMotorTorque
{
get { return _maxMotorTorque; }
set
{
WakeBodies();
_maxMotorTorque = value;
}
}
/// <summary>
/// Suspension frequency, zero indicates no suspension
/// </summary>
public float Frequency { get; set; }
/// <summary>
/// Suspension damping ratio, one indicates critical damping
/// </summary>
public float DampingRatio { get; set; }
/// <summary>
/// Gets the translation along the axis
/// </summary>
public float JointTranslation
{
get
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 pA = bA.GetWorldPoint(LocalAnchorA);
Vector2 pB = bB.GetWorldPoint(LocalAnchorB);
Vector2 d = pB - pA;
Vector2 axis = bA.GetWorldVector(LocalXAxis);
float translation = Vector2.Dot(d, axis);
return translation;
}
}
/// <summary>
/// Gets the angular velocity of the joint
/// </summary>
public float JointSpeed
{
get
{
float wA = BodyA.AngularVelocity;
float wB = BodyB.AngularVelocity;
return wB - wA;
}
}
/// <summary>
/// Enable/disable the joint motor.
/// </summary>
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
WakeBodies();
_enableMotor = value;
}
}
/// <summary>
/// Gets the torque of the motor
/// </summary>
/// <param name="invDt">inverse delta time</param>
public float GetMotorTorque(float invDt)
{
return invDt * _motorImpulse;
}
public override Vector2 GetReactionForce(float invDt)
{
return invDt * (_impulse * _ay + _springImpulse * _ax);
}
public override float GetReactionTorque(float invDt)
{
return invDt * _motorImpulse;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective masses.
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d1 = cB + rB - cA - rA;
// Point to line constraint
{
_ay = MathUtils.Mul(qA, _localYAxis);
_sAy = MathUtils.Cross(d1 + rA, _ay);
_sBy = MathUtils.Cross(rB, _ay);
_mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy;
if (_mass > 0.0f)
{
_mass = 1.0f / _mass;
}
}
// Spring constraint
_springMass = 0.0f;
_bias = 0.0f;
_gamma = 0.0f;
if (Frequency > 0.0f)
{
_ax = MathUtils.Mul(qA, LocalXAxis);
_sAx = MathUtils.Cross(d1 + rA, _ax);
_sBx = MathUtils.Cross(rB, _ax);
float invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx;
if (invMass > 0.0f)
{
_springMass = 1.0f / invMass;
float C = Vector2.Dot(d1, _ax);
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * _springMass * DampingRatio * omega;
// Spring stiffness
float k = _springMass * omega * omega;
// magic formulas
float h = data.step.dt;
_gamma = h * (d + h * k);
if (_gamma > 0.0f)
{
_gamma = 1.0f / _gamma;
}
_bias = C * h * k * _gamma;
_springMass = invMass + _gamma;
if (_springMass > 0.0f)
{
_springMass = 1.0f / _springMass;
}
}
}
else
{
_springImpulse = 0.0f;
}
// Rotational motor
if (_enableMotor)
{
_motorMass = iA + iB;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
else
{
_motorMass = 0.0f;
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
_springImpulse *= data.step.dtRatio;
_motorImpulse *= data.step.dtRatio;
Vector2 P = _impulse * _ay + _springImpulse * _ax;
float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse;
float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;
vA -= _invMassA * P;
wA -= _invIA * LA;
vB += _invMassB * P;
wB += _invIB * LB;
}
else
{
_impulse = 0.0f;
_springImpulse = 0.0f;
_motorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
// Solve spring constraint
{
float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
_springImpulse += impulse;
Vector2 P = impulse * _ax;
float LA = impulse * _sAx;
float LB = impulse * _sBx;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
// Solve rotational motor constraint
{
float Cdot = wB - wA - _motorSpeed;
float impulse = -_motorMass * Cdot;
float oldImpulse = _motorImpulse;
float maxImpulse = data.step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve point to line constraint
{
float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
float impulse = -_mass * Cdot;
_impulse += impulse;
Vector2 P = impulse * _ay;
float LA = impulse * _sAy;
float LB = impulse * _sBy;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = (cB - cA) + rB - rA;
Vector2 ay = MathUtils.Mul(qA, _localYAxis);
float sAy = MathUtils.Cross(d + rA, ay);
float sBy = MathUtils.Cross(rB, ay);
float C = Vector2.Dot(d, ay);
float k = _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy;
float impulse;
if (k != 0.0f)
{
impulse = -C / k;
}
else
{
impulse = 0.0f;
}
Vector2 P = impulse * ay;
float LA = impulse * sAy;
float LB = impulse * sBy;
cA -= _invMassA * P;
aA -= _invIA * LA;
cB += _invMassB * P;
aB += _invIB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return Math.Abs(C) <= Settings.LinearSlop;
}
}
}