350 lines
13 KiB
C#
350 lines
13 KiB
C#
|
/*
|
||
|
|
* Farseer Physics Engine based on Box2D.XNA port:
|
||
|
|
* Copyright (c) 2010 Ian Qvist
|
||
|
|
*
|
||
|
|
* Box2D.XNA port of Box2D:
|
||
|
|
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
|
||
|
|
*
|
||
|
|
* Original source Box2D:
|
||
|
|
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
|
||
|
|
*
|
||
|
|
* This software is provided 'as-is', without any express or implied
|
||
|
|
* warranty. In no event will the authors be held liable for any damages
|
||
|
|
* arising from the use of this software.
|
||
|
|
* Permission is granted to anyone to use this software for any purpose,
|
||
|
|
* including commercial applications, and to alter it and redistribute it
|
||
|
|
* freely, subject to the following restrictions:
|
||
|
|
* 1. The origin of this software must not be misrepresented; you must not
|
||
|
|
* claim that you wrote the original software. If you use this software
|
||
|
|
* in a product, an acknowledgment in the product documentation would be
|
||
|
|
* appreciated but is not required.
|
||
|
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||
|
|
* misrepresented as being the original software.
|
||
|
|
* 3. This notice may not be removed or altered from any source distribution.
|
||
|
|
*/
|
||
|
|
|
||
|
|
using System.Diagnostics;
|
||
|
|
using FarseerPhysics.Common;
|
||
|
|
using Microsoft.Xna.Framework;
|
||
|
|
|
||
|
|
namespace FarseerPhysics.Dynamics.Joints
|
||
|
|
{
|
||
|
|
/// <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:
|
||
|
|
/// 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 The revolute and prismatic joints must be attached to
|
||
|
|
/// fixed bodies (which must be body1 on those joints).
|
||
|
|
/// </summary>
|
||
|
|
public class GearJoint : Joint
|
||
|
|
{
|
||
|
|
private Jacobian _J;
|
||
|
|
|
||
|
|
private float _ant;
|
||
|
|
private FixedPrismaticJoint _fixedPrismatic1;
|
||
|
|
private FixedPrismaticJoint _fixedPrismatic2;
|
||
|
|
private FixedRevoluteJoint _fixedRevolute1;
|
||
|
|
private FixedRevoluteJoint _fixedRevolute2;
|
||
|
|
private float _impulse;
|
||
|
|
private float _mass;
|
||
|
|
private PrismaticJoint _prismatic1;
|
||
|
|
private PrismaticJoint _prismatic2;
|
||
|
|
private RevoluteJoint _revolute1;
|
||
|
|
private RevoluteJoint _revolute2;
|
||
|
|
|
||
|
|
/// <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>
|
||
|
|
public GearJoint(Joint jointA, Joint jointB, float ratio)
|
||
|
|
: base(jointA.BodyA, jointA.BodyB)
|
||
|
|
{
|
||
|
|
JointType = JointType.Gear;
|
||
|
|
JointA = jointA;
|
||
|
|
JointB = jointB;
|
||
|
|
Ratio = ratio;
|
||
|
|
|
||
|
|
JointType type1 = jointA.JointType;
|
||
|
|
JointType type2 = jointB.JointType;
|
||
|
|
|
||
|
|
// Make sure its the right kind of joint
|
||
|
|
Debug.Assert(type1 == JointType.Revolute ||
|
||
|
|
type1 == JointType.Prismatic ||
|
||
|
|
type1 == JointType.FixedRevolute ||
|
||
|
|
type1 == JointType.FixedPrismatic);
|
||
|
|
Debug.Assert(type2 == JointType.Revolute ||
|
||
|
|
type2 == JointType.Prismatic ||
|
||
|
|
type2 == JointType.FixedRevolute ||
|
||
|
|
type2 == JointType.FixedPrismatic);
|
||
|
|
|
||
|
|
// In the case of a prismatic and revolute joint, the first body must be static.
|
||
|
|
if (type1 == JointType.Revolute || type1 == JointType.Prismatic)
|
||
|
|
Debug.Assert(jointA.BodyA.BodyType == BodyType.Static);
|
||
|
|
if (type2 == JointType.Revolute || type2 == JointType.Prismatic)
|
||
|
|
Debug.Assert(jointB.BodyA.BodyType == BodyType.Static);
|
||
|
|
|
||
|
|
float coordinate1 = 0.0f, coordinate2 = 0.0f;
|
||
|
|
|
||
|
|
switch (type1)
|
||
|
|
{
|
||
|
|
case JointType.Revolute:
|
||
|
|
BodyA = jointA.BodyB;
|
||
|
|
_revolute1 = (RevoluteJoint)jointA;
|
||
|
|
LocalAnchor1 = _revolute1.LocalAnchorB;
|
||
|
|
coordinate1 = _revolute1.JointAngle;
|
||
|
|
break;
|
||
|
|
case JointType.Prismatic:
|
||
|
|
BodyA = jointA.BodyB;
|
||
|
|
_prismatic1 = (PrismaticJoint)jointA;
|
||
|
|
LocalAnchor1 = _prismatic1.LocalAnchorB;
|
||
|
|
coordinate1 = _prismatic1.JointTranslation;
|
||
|
|
break;
|
||
|
|
case JointType.FixedRevolute:
|
||
|
|
BodyA = jointA.BodyA;
|
||
|
|
_fixedRevolute1 = (FixedRevoluteJoint)jointA;
|
||
|
|
LocalAnchor1 = _fixedRevolute1.LocalAnchorA;
|
||
|
|
coordinate1 = _fixedRevolute1.JointAngle;
|
||
|
|
break;
|
||
|
|
case JointType.FixedPrismatic:
|
||
|
|
BodyA = jointA.BodyA;
|
||
|
|
_fixedPrismatic1 = (FixedPrismaticJoint)jointA;
|
||
|
|
LocalAnchor1 = _fixedPrismatic1.LocalAnchorA;
|
||
|
|
coordinate1 = _fixedPrismatic1.JointTranslation;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (type2)
|
||
|
|
{
|
||
|
|
case JointType.Revolute:
|
||
|
|
BodyB = jointB.BodyB;
|
||
|
|
_revolute2 = (RevoluteJoint)jointB;
|
||
|
|
LocalAnchor2 = _revolute2.LocalAnchorB;
|
||
|
|
coordinate2 = _revolute2.JointAngle;
|
||
|
|
break;
|
||
|
|
case JointType.Prismatic:
|
||
|
|
BodyB = jointB.BodyB;
|
||
|
|
_prismatic2 = (PrismaticJoint)jointB;
|
||
|
|
LocalAnchor2 = _prismatic2.LocalAnchorB;
|
||
|
|
coordinate2 = _prismatic2.JointTranslation;
|
||
|
|
break;
|
||
|
|
case JointType.FixedRevolute:
|
||
|
|
BodyB = jointB.BodyA;
|
||
|
|
_fixedRevolute2 = (FixedRevoluteJoint)jointB;
|
||
|
|
LocalAnchor2 = _fixedRevolute2.LocalAnchorA;
|
||
|
|
coordinate2 = _fixedRevolute2.JointAngle;
|
||
|
|
break;
|
||
|
|
case JointType.FixedPrismatic:
|
||
|
|
BodyB = jointB.BodyA;
|
||
|
|
_fixedPrismatic2 = (FixedPrismaticJoint)jointB;
|
||
|
|
LocalAnchor2 = _fixedPrismatic2.LocalAnchorA;
|
||
|
|
coordinate2 = _fixedPrismatic2.JointTranslation;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
_ant = coordinate1 + Ratio * coordinate2;
|
||
|
|
}
|
||
|
|
|
||
|
|
public override Vector2 WorldAnchorA
|
||
|
|
{
|
||
|
|
get { return BodyA.GetWorldPoint(LocalAnchor1); }
|
||
|
|
}
|
||
|
|
|
||
|
|
public override Vector2 WorldAnchorB
|
||
|
|
{
|
||
|
|
get { return BodyB.GetWorldPoint(LocalAnchor2); }
|
||
|
|
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// The gear ratio.
|
||
|
|
/// </summary>
|
||
|
|
public float Ratio { get; set; }
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// The first revolute/prismatic joint attached to the gear joint.
|
||
|
|
/// </summary>
|
||
|
|
public Joint JointA { get; set; }
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// The second revolute/prismatic joint attached to the gear joint.
|
||
|
|
/// </summary>
|
||
|
|
public Joint JointB { get; set; }
|
||
|
|
|
||
|
|
public Vector2 LocalAnchor1 { get; private set; }
|
||
|
|
public Vector2 LocalAnchor2 { get; private set; }
|
||
|
|
|
||
|
|
public override Vector2 GetReactionForce(float inv_dt)
|
||
|
|
{
|
||
|
|
Vector2 P = _impulse * _J.LinearB;
|
||
|
|
return inv_dt * P;
|
||
|
|
}
|
||
|
|
|
||
|
|
public override float GetReactionTorque(float inv_dt)
|
||
|
|
{
|
||
|
|
Transform xf1;
|
||
|
|
BodyB.GetTransform(out xf1);
|
||
|
|
|
||
|
|
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor2 - BodyB.LocalCenter);
|
||
|
|
Vector2 P = _impulse * _J.LinearB;
|
||
|
|
float L = _impulse * _J.AngularB - MathUtils.Cross(r, P);
|
||
|
|
return inv_dt * L;
|
||
|
|
}
|
||
|
|
|
||
|
|
internal override void InitVelocityConstraints(ref TimeStep step)
|
||
|
|
{
|
||
|
|
Body b1 = BodyA;
|
||
|
|
Body b2 = BodyB;
|
||
|
|
|
||
|
|
float K = 0.0f;
|
||
|
|
_J.SetZero();
|
||
|
|
|
||
|
|
if (_revolute1 != null || _fixedRevolute1 != null)
|
||
|
|
{
|
||
|
|
_J.AngularA = -1.0f;
|
||
|
|
K += b1.InvI;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
Vector2 ug;
|
||
|
|
if (_prismatic1 != null)
|
||
|
|
ug = _prismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
|
||
|
|
else
|
||
|
|
ug = _fixedPrismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
|
||
|
|
|
||
|
|
Transform xf1 /*, xfg1*/;
|
||
|
|
b1.GetTransform(out xf1);
|
||
|
|
//g1.GetTransform(out xfg1);
|
||
|
|
|
||
|
|
|
||
|
|
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor1 - b1.LocalCenter);
|
||
|
|
float crug = MathUtils.Cross(r, ug);
|
||
|
|
_J.LinearA = -ug;
|
||
|
|
_J.AngularA = -crug;
|
||
|
|
K += b1.InvMass + b1.InvI * crug * crug;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_revolute2 != null || _fixedRevolute2 != null)
|
||
|
|
{
|
||
|
|
_J.AngularB = -Ratio;
|
||
|
|
K += Ratio * Ratio * b2.InvI;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
Vector2 ug;
|
||
|
|
if (_prismatic2 != null)
|
||
|
|
ug = _prismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
|
||
|
|
else
|
||
|
|
ug = _fixedPrismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
|
||
|
|
|
||
|
|
Transform /*xfg1,*/ xf2;
|
||
|
|
//g1.GetTransform(out xfg1);
|
||
|
|
b2.GetTransform(out xf2);
|
||
|
|
|
||
|
|
Vector2 r = MathUtils.Multiply(ref xf2.R, LocalAnchor2 - b2.LocalCenter);
|
||
|
|
float crug = MathUtils.Cross(r, ug);
|
||
|
|
_J.LinearB = -Ratio * ug;
|
||
|
|
_J.AngularB = -Ratio * crug;
|
||
|
|
K += Ratio * Ratio * (b2.InvMass + b2.InvI * crug * crug);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Compute effective mass.
|
||
|
|
Debug.Assert(K > 0.0f);
|
||
|
|
_mass = K > 0.0f ? 1.0f / K : 0.0f;
|
||
|
|
|
||
|
|
if (Settings.EnableWarmstarting)
|
||
|
|
{
|
||
|
|
// Warm starting.
|
||
|
|
b1.LinearVelocityInternal += b1.InvMass * _impulse * _J.LinearA;
|
||
|
|
b1.AngularVelocityInternal += b1.InvI * _impulse * _J.AngularA;
|
||
|
|
b2.LinearVelocityInternal += b2.InvMass * _impulse * _J.LinearB;
|
||
|
|
b2.AngularVelocityInternal += b2.InvI * _impulse * _J.AngularB;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
_impulse = 0.0f;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
internal override void SolveVelocityConstraints(ref TimeStep step)
|
||
|
|
{
|
||
|
|
Body b1 = BodyA;
|
||
|
|
Body b2 = BodyB;
|
||
|
|
|
||
|
|
float Cdot = _J.Compute(b1.LinearVelocityInternal, b1.AngularVelocityInternal,
|
||
|
|
b2.LinearVelocityInternal, b2.AngularVelocityInternal);
|
||
|
|
|
||
|
|
float impulse = _mass * (-Cdot);
|
||
|
|
_impulse += impulse;
|
||
|
|
|
||
|
|
b1.LinearVelocityInternal += b1.InvMass * impulse * _J.LinearA;
|
||
|
|
b1.AngularVelocityInternal += b1.InvI * impulse * _J.AngularA;
|
||
|
|
b2.LinearVelocityInternal += b2.InvMass * impulse * _J.LinearB;
|
||
|
|
b2.AngularVelocityInternal += b2.InvI * impulse * _J.AngularB;
|
||
|
|
}
|
||
|
|
|
||
|
|
internal override bool SolvePositionConstraints()
|
||
|
|
{
|
||
|
|
const float linearError = 0.0f;
|
||
|
|
|
||
|
|
Body b1 = BodyA;
|
||
|
|
Body b2 = BodyB;
|
||
|
|
|
||
|
|
float coordinate1 = 0.0f, coordinate2 = 0.0f;
|
||
|
|
if (_revolute1 != null)
|
||
|
|
{
|
||
|
|
coordinate1 = _revolute1.JointAngle;
|
||
|
|
}
|
||
|
|
else if (_fixedRevolute1 != null)
|
||
|
|
{
|
||
|
|
coordinate1 = _fixedRevolute1.JointAngle;
|
||
|
|
}
|
||
|
|
else if (_prismatic1 != null)
|
||
|
|
{
|
||
|
|
coordinate1 = _prismatic1.JointTranslation;
|
||
|
|
}
|
||
|
|
else if (_fixedPrismatic1 != null)
|
||
|
|
{
|
||
|
|
coordinate1 = _fixedPrismatic1.JointTranslation;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_revolute2 != null)
|
||
|
|
{
|
||
|
|
coordinate2 = _revolute2.JointAngle;
|
||
|
|
}
|
||
|
|
else if (_fixedRevolute2 != null)
|
||
|
|
{
|
||
|
|
coordinate2 = _fixedRevolute2.JointAngle;
|
||
|
|
}
|
||
|
|
else if (_prismatic2 != null)
|
||
|
|
{
|
||
|
|
coordinate2 = _prismatic2.JointTranslation;
|
||
|
|
}
|
||
|
|
else if (_fixedPrismatic2 != null)
|
||
|
|
{
|
||
|
|
coordinate2 = _fixedPrismatic2.JointTranslation;
|
||
|
|
}
|
||
|
|
|
||
|
|
float C = _ant - (coordinate1 + Ratio * coordinate2);
|
||
|
|
|
||
|
|
float impulse = _mass * (-C);
|
||
|
|
|
||
|
|
b1.Sweep.C += b1.InvMass * impulse * _J.LinearA;
|
||
|
|
b1.Sweep.A += b1.InvI * impulse * _J.AngularA;
|
||
|
|
b2.Sweep.C += b2.InvMass * impulse * _J.LinearB;
|
||
|
|
b2.Sweep.A += b2.InvI * impulse * _J.AngularB;
|
||
|
|
|
||
|
|
b1.SynchronizeTransform();
|
||
|
|
b2.SynchronizeTransform();
|
||
|
|
|
||
|
|
// TODO_ERIN not implemented
|
||
|
|
return linearError < Settings.LinearSlop;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|