public void Add(Joint joint)
{
if (JointCount >= JointCapacity)
return;
// Debug.Assert(JointCount < JointCapacity);
_joints[JointCount++] = joint;
}
public static GearJoint CreateGearJoint(World world, Joint jointA, Joint jointB, float ratio)
{
GearJoint gearJoint = new GearJoint(jointA, jointB, ratio);
world.AddJoint(gearJoint);
return(gearJoint);
}
internal void DestroyJoint()
{
if (this.joint == null) return;
Scene.PhysicsWorld.RemoveJoint(this.joint);
this.joint.Broke -= this.joint_Broke;
this.joint = null;
}
public ConstructElement(ScrapGame game, Segment entity, Construct construct, Point offSet, Joint rootJoint)
{
branchJoints = new List<Joint>();
this.rootJoint = rootJoint;
this.segment = entity;
this.game = game;
this.construct = construct;
this.offSet = offSet;
}
public Balance(World worldFarseer, Vector2 posDepart, Texture2D tex)
: base(worldFarseer, posDepart, tex)
{
objPhys.IsStatic = false;
objPhys.BodyType = BodyType.Dynamic;
objPhys.AngularDamping = 0.8f;
objPhys.Inertia = 2;
Body rotor = BodyFactory.CreateCircle(worldFarseer, 0.1f, 3f);
pivot = JointFactory.CreateRevoluteJoint(worldFarseer,objPhys,rotor,objPhys.Position);
}
public override void LoadContent()
{
base.LoadContent();
World.Gravity = new Vector2(0, 140);
projection = Matrix.CreateOrthographicOffCenter(0f, ScreenManager.GraphicsDevice.Viewport.Width, ScreenManager.GraphicsDevice.Viewport.Height, 0f, 0f, 1f);
hookpointmanager = new HookPointManager(World);
MaterialType hookpointmaterial = MaterialType.Blank;
swingerbody = BodyFactory.CreateBody(World, new Vector2(3, 10));
swinger = FixtureFactory.AttachCircle(1, 40, swingerbody);
swinger.Body.BodyType = BodyType.Dynamic;
swinger.Friction = 0;
body3 = BodyFactory.CreateBody(World, new Vector2(-4, -10));
body3.IsStatic = true;
CircleShape circleShape3 = new CircleShape(.4f, .3f);
Fixture fixture3 = body3.CreateFixture(circleShape3, hookpointmaterial);
body3.IgnoreGravity = true;
fixture3.Body.BodyType = BodyType.Static;
body2 = BodyFactory.CreateBody(World, new Vector2(-8, -10));
body2.IsStatic = true;
CircleShape circleShape2 = new CircleShape(.4f, .3f);
fixture2 = body2.CreateFixture(circleShape2, hookpointmaterial);
body2.IgnoreGravity = true;
fixture2.Body.BodyType = BodyType.Static;
joint1 = JointFactory.CreateSliderJoint(World, swingerbody, fixture3.Body, Vector2.Zero, Vector2.Zero, 4, 5);
_border = new Border(World, this, ScreenManager.GraphicsDevice.Viewport);
//SetUserAgent(swingerbody, 100f, 100f);
// create sprite based on body
swingersprite = new Sprite(ScreenManager.Assets.TextureFromShape(swingerbody.FixtureList[0].Shape,
MaterialType.Dots,
Color.Red, 2f));
body3sprite = new Sprite(ScreenManager.Assets.TextureFromShape(swingerbody.FixtureList[0].Shape,
MaterialType.Dots,
Color.Red, 2f));
body2sprite = new Sprite(ScreenManager.Assets.TextureFromShape(swingerbody.FixtureList[0].Shape,
MaterialType.Dots,
Color.Red, 2f));
}
public Wheel(ScrapGame game, Vector2 position)
: base(game)
{
texture = game.Content.Load<Texture2D>("wheel");
wheel = BodyFactory.CreateCircle(game.world, .49f, 1f, this);
wheel.Restitution = .5f;
wheel.BodyType = BodyType.Dynamic;
wheel.Position = position;
wheel.Friction = .9f;
//wheel.CollisionCategories = new Category();
body = BodyFactory.CreateRectangle(game.world, .5f, .5f, .5f, this);
body.Position = position;
body.BodyType = BodyType.Dynamic;
body.Restitution = .5f;
body.Friction = .9f;
bodyJoint = JointFactory.CreateRevoluteJoint(game.world, body, wheel, new Vector2(0, 0), new Vector2(0, 0));
}
public DriveWheel(ScrapGame game, Vector2 position)
: base(game)
{
texture = game.Content.Load<Texture2D>("wheel");
wheel = BodyFactory.CreateCircle(game.world, .49f, 2f,this);
wheel.Restitution = 1f;
wheel.BodyType = BodyType.Dynamic;
wheel.Position = position;
wheel.Friction = 2f;
wheel.UserData = this;
body = BodyFactory.CreateRectangle(game.world, .1f, .1f, 2f);
body.Position = position;
body.BodyType = BodyType.Dynamic;
//body.IgnoreCCD = true;
wheelHubJoint = JointFactory.CreateRevoluteJoint(game.world, body, wheel, new Vector2(0, 0));
}
/// <summary>
/// Create a joint to constrain bodies together. This may cause the connected bodies to cease colliding.
/// </summary>
/// <param name="joint">The joint.</param>
public void AddJoint(Joint joint)
{
//You are adding the same joint twice?
Debug.Assert(!_jointAddList.Contains(joint));
if (!_jointAddList.Contains(joint))
_jointAddList.Add(joint);
}
private void RemoveJoint(Joint joint, bool doCheck)
{
if (doCheck)
{
Debug.Assert(!_jointRemoveList.Contains(joint),
"The joint is already marked for removal. You are removing the joint more than once.");
}
if (!_jointRemoveList.Contains(joint))
_jointRemoveList.Add(joint);
}
private void SerializeJoint(Joint joint)
{
if (joint.IsFixedType())
return;
_writer.WriteStartElement("Joint");
_writer.WriteAttributeString("Type", joint.JointType.ToString());
WriteElement("BodyA", FindBodyIndex(joint.BodyA));
WriteElement("BodyB", FindBodyIndex(joint.BodyB));
WriteElement("CollideConnected", joint.CollideConnected);
WriteElement("Breakpoint", joint.Breakpoint);
if (joint.UserData != null)
{
_writer.WriteStartElement("UserData");
WriteDynamicType(joint.UserData.GetType(), joint.UserData);
_writer.WriteEndElement();
}
switch (joint.JointType)
{
case JointType.Distance:
{
DistanceJoint djd = (DistanceJoint)joint;
WriteElement("DampingRatio", djd.DampingRatio);
WriteElement("FrequencyHz", djd.Frequency);
WriteElement("Length", djd.Length);
WriteElement("LocalAnchorA", djd.LocalAnchorA);
WriteElement("LocalAnchorB", djd.LocalAnchorB);
}
break;
case JointType.Friction:
{
FrictionJoint fjd = (FrictionJoint)joint;
WriteElement("LocalAnchorA", fjd.LocalAnchorA);
WriteElement("LocalAnchorB", fjd.LocalAnchorB);
WriteElement("MaxForce", fjd.MaxForce);
WriteElement("MaxTorque", fjd.MaxTorque);
}
break;
case JointType.Gear:
throw new Exception("Gear joint not supported by serialization");
case JointType.Line:
{
LineJoint ljd = (LineJoint)joint;
WriteElement("EnableMotor", ljd.MotorEnabled);
WriteElement("LocalAnchorA", ljd.LocalAnchorA);
WriteElement("LocalAnchorB", ljd.LocalAnchorB);
WriteElement("MotorSpeed", ljd.MotorSpeed);
WriteElement("DampingRatio", ljd.DampingRatio);
WriteElement("MaxMotorTorque", ljd.MaxMotorTorque);
WriteElement("FrequencyHz", ljd.Frequency);
WriteElement("LocalXAxis", ljd.LocalXAxis);
}
break;
case JointType.Prismatic:
{
PrismaticJoint pjd = (PrismaticJoint)joint;
//NOTE: Does not conform with Box2DScene
WriteElement("EnableLimit", pjd.LimitEnabled);
WriteElement("EnableMotor", pjd.MotorEnabled);
WriteElement("LocalAnchorA", pjd.LocalAnchorA);
WriteElement("LocalAnchorB", pjd.LocalAnchorB);
WriteElement("LocalXAxis1", pjd.LocalXAxis1);
WriteElement("LowerTranslation", pjd.LowerLimit);
WriteElement("UpperTranslation", pjd.UpperLimit);
WriteElement("MaxMotorForce", pjd.MaxMotorForce);
WriteElement("MotorSpeed", pjd.MotorSpeed);
}
break;
case JointType.Pulley:
{
PulleyJoint pjd = (PulleyJoint)joint;
WriteElement("GroundAnchorA", pjd.GroundAnchorA);
WriteElement("GroundAnchorB", pjd.GroundAnchorB);
WriteElement("LengthA", pjd.LengthA);
WriteElement("LengthB", pjd.LengthB);
WriteElement("LocalAnchorA", pjd.LocalAnchorA);
WriteElement("LocalAnchorB", pjd.LocalAnchorB);
WriteElement("MaxLengthA", pjd.MaxLengthA);
WriteElement("MaxLengthB", pjd.MaxLengthB);
WriteElement("Ratio", pjd.Ratio);
}
break;
case JointType.Revolute:
{
RevoluteJoint rjd = (RevoluteJoint)joint;
WriteElement("EnableLimit", rjd.LimitEnabled);
WriteElement("EnableMotor", rjd.MotorEnabled);
WriteElement("LocalAnchorA", rjd.LocalAnchorA);
WriteElement("LocalAnchorB", rjd.LocalAnchorB);
WriteElement("LowerAngle", rjd.LowerLimit);
WriteElement("MaxMotorTorque", rjd.MaxMotorTorque);
WriteElement("MotorSpeed", rjd.MotorSpeed);
WriteElement("ReferenceAngle", rjd.ReferenceAngle);
WriteElement("UpperAngle", rjd.UpperLimit);
}
break;
case JointType.Weld:
{
WeldJoint wjd = (WeldJoint)joint;
WriteElement("LocalAnchorA", wjd.LocalAnchorA);
WriteElement("LocalAnchorB", wjd.LocalAnchorB);
}
break;
//
// Not part of Box2DScene
//
case JointType.Rope:
{
RopeJoint rjd = (RopeJoint)joint;
WriteElement("LocalAnchorA", rjd.LocalAnchorA);
WriteElement("LocalAnchorB", rjd.LocalAnchorB);
WriteElement("MaxLength", rjd.MaxLength);
}
break;
case JointType.Angle:
{
AngleJoint aj = (AngleJoint)joint;
WriteElement("BiasFactor", aj.BiasFactor);
WriteElement("MaxImpulse", aj.MaxImpulse);
WriteElement("Softness", aj.Softness);
WriteElement("TargetAngle", aj.TargetAngle);
}
break;
case JointType.Slider:
{
SliderJoint sliderJoint = (SliderJoint)joint;
WriteElement("DampingRatio", sliderJoint.DampingRatio);
WriteElement("FrequencyHz", sliderJoint.Frequency);
WriteElement("MaxLength", sliderJoint.MaxLength);
WriteElement("MinLength", sliderJoint.MinLength);
WriteElement("LocalAnchorA", sliderJoint.LocalAnchorA);
WriteElement("LocalAnchorB", sliderJoint.LocalAnchorB);
}
break;
default:
throw new Exception("Joint not supported");
}
_writer.WriteEndElement();
}
private void DrawJoint(Joint joint)
{
if (!joint.Enabled)
return;
Body b1 = joint.BodyA;
Body b2 = joint.BodyB;
Transform xf1;
b1.GetTransform(out xf1);
Vector2 x2 = Vector2.Zero;
// WIP David
if (!joint.IsFixedType())
{
Transform xf2;
b2.GetTransform(out xf2);
x2 = xf2.p;
}
Vector2 p1 = joint.WorldAnchorA;
Vector2 p2 = joint.WorldAnchorB;
Vector2 x1 = xf1.p;
Color color = Color.FromArgb(255, 128, 205, 205);
switch (joint.JointType)
{
case JointType.Distance:
DrawSegment(p1, p2, color);
break;
case JointType.Pulley:
PulleyJoint pulley = (PulleyJoint)joint;
Vector2 s1 = b1.GetWorldPoint(pulley.LocalAnchorA);
Vector2 s2 = b2.GetWorldPoint(pulley.LocalAnchorB);
DrawSegment(p1, p2, color);
DrawSegment(p1, s1, color);
DrawSegment(p2, s2, color);
break;
case JointType.FixedMouse:
DrawPoint(p1, 0.5f, Color.FromArgb(255, 0, 255, 0));
DrawSegment(p1, p2, Color.FromArgb(255, 205, 205, 205));
break;
case JointType.Revolute:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
DrawSegment(x2, p2, color);
DrawSolidCircle(p2, 0.1f, Vector2.Zero, Colors.Red);
DrawSolidCircle(p1, 0.1f, Vector2.Zero, Colors.Blue);
break;
case JointType.FixedAngle:
//Should not draw anything.
break;
case JointType.FixedRevolute:
DrawSegment(x1, p1, color);
DrawSolidCircle(p1, 0.1f, Vector2.Zero, Colors.Purple);
break;
case JointType.FixedLine:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.FixedDistance:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.FixedPrismatic:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.Gear:
DrawSegment(x1, x2, color);
break;
default:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
DrawSegment(x2, p2, color);
break;
}
}
/// <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="joint1">The first joint.</param>
/// <param name="joint2">The second joint.</param>
/// <param name="ratio">The ratio.</param>
public GearJoint(Joint joint1, Joint joint2, float ratio = 1f)
{
JointType = JointType.Gear;
Joint1 = joint1;
Joint2 = joint2;
Ratio = ratio;
_typeA = joint1.JointType;
_typeB = joint2.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 = Joint1.BodyA;
_bodyA = Joint1.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)joint1;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC = Vector2.Zero;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)joint1;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxisA;
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 = Joint2.BodyA;
_bodyB = Joint2.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)joint2;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD = Vector2.Zero;
coordinateB = aB - aD - _referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)joint2;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxisA;
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;
}
protected override void JointRemoved(Joint joint)
{
for (int i = 0; i < 8; ++i)
{
if (_joints[i] == joint)
{
_joints[i] = null;
break;
}
}
base.JointRemoved(joint);
}
protected virtual void JointRemoved(Joint joint)
{
if (_fixedMouseJoint == joint)
_fixedMouseJoint = null;
}
/// <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;
}
protected int lookupJointIndex(Joint joint)
{
int? val = m_jointToIndexMap[joint];
if (null != val)
return val.Value;
else
return -1;
}
/// <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 = Complex.Divide(Complex.Multiply(ref _localAnchorA, ref xfA.q) + (xfA.p - xfC.p), ref xfC.q);
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 = Complex.Divide(Complex.Multiply(ref _localAnchorB, ref xfB.q) + (xfB.p - xfD.p), ref xfD.q);
coordinateB = Vector2.Dot(pB - pD, _localAxisD);
}
_ratio = ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
public static GearJoint CreateGearJoint(World world, Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio)
{
GearJoint gearJoint = new GearJoint(bodyA, bodyB, jointA, jointB, ratio);
world.Add(gearJoint);
return(gearJoint);
}
public String GetJointName(Joint joint)
{
if (m_jointToNameMap.ContainsKey(joint))
return m_jointToNameMap[joint];
return null;
}
public void Add(Joint joint)
{
//Debug.Assert(JointCount < _jointCapacity);
_joints[JointCount++] = joint;
}
private void DrawJoint(Joint joint)
{
Body b1 = joint.BodyA;
Body b2 = joint.BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
Vector2 x2 = new Vector2();
// WIP David
if (!joint.IsFixedType())
{
b2.GetTransform(out xf2);
x2 = xf2.Position;
}
Vector2 p2 = joint.WorldAnchorB;
Vector2 x1 = xf1.Position;
Vector2 p1 = joint.WorldAnchorA;
Color color = new Color(0.5f, 0.8f, 0.8f);
switch (joint.JointType)
{
case JointType.Distance:
DrawSegment(p1, p2, color);
break;
case JointType.Pulley:
{
PulleyJoint pulley = (PulleyJoint) joint;
Vector2 s1 = pulley.GroundAnchorA;
Vector2 s2 = pulley.GroundAnchorB;
DrawSegment(s1, p1, color);
DrawSegment(s2, p2, color);
DrawSegment(s1, s2, color);
}
break;
case JointType.FixedMouse:
FixedMouseJoint fixedMouseJoint = (FixedMouseJoint) joint;
p1 = fixedMouseJoint.Target;
DrawPoint(p2, 0.5f, new Color(0.0f, 1.0f, 0.0f));
DrawSegment(p1, p2, new Color(0.8f, 0.8f, 0.8f));
break;
case JointType.Revolute:
//DrawSegment(x2, p1, color);
DrawSegment(p2, p1, color);
DrawSolidCircle(p2, 0.1f, new Vector2(), Color.Red);
DrawSolidCircle(p1, 0.1f, new Vector2(), Color.Blue);
break;
case JointType.FixedRevolute:
DrawSolidCircle(p1, 0.1f, new Vector2(), Color.Pink);
break;
case JointType.FixedLine:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.FixedDistance:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.FixedPrismatic:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.Gear:
DrawSegment(x1, x2, color);
//DrawSegment(x1, p1, color);
//DrawSegment(p1, p2, color);
break;
default:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
DrawSegment(x2, p2, color);
break;
}
}
public void Solve(ref TimeStep step, ref Vector2 gravity)
{
// Integrate velocities and apply damping.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType != BodyType.Dynamic)
{
continue;
}
// Integrate velocities.
// FPE 3 only - Only apply gravity if the body wants it.
if (b.IgnoreGravity)
{
b.LinearVelocityInternal.x += step.dt * (b.InvMass * b.Force.x);
b.LinearVelocityInternal.y += step.dt * (b.InvMass * b.Force.y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
else
{
b.LinearVelocityInternal.x += step.dt * (gravity.x + b.InvMass * b.Force.x);
b.LinearVelocityInternal.y += step.dt * (gravity.y + b.InvMass * b.Force.y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f);
b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f);
}
// Partition contacts so that contacts with static bodies are solved last.
int i1 = -1;
for (int i2 = 0; i2 < ContactCount; ++i2)
{
Fixture fixtureA = _contacts[i2].FixtureA;
Fixture fixtureB = _contacts[i2].FixtureB;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static;
if (nonStatic)
{
++i1;
//TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162
Contact tmp = _contacts[i1];
_contacts[i1] = _contacts[i2];
_contacts[i2] = tmp;
}
}
// Initialize velocity constraints.
_contactSolver.Reset(_contacts, ContactCount, step.dtRatio, Settings.EnableWarmstarting);
_contactSolver.InitializeVelocityConstraints();
if (Settings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
_tmpTime = 0;
}
#endif
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref step);
}
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_tmpTime += _watch.ElapsedTicks;
}
#endif
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref step);
joint.Validate(step.inv_dt);
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
_contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
_contactSolver.StoreImpulses();
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
// Check for large velocities.
float translationX = step.dt * b.LinearVelocityInternal.x;
float translationY = step.dt * b.LinearVelocityInternal.y;
float result = translationX * translationX + translationY * translationY;
if (result > Settings.MaxTranslationSquared)
{
float sq = Mathf.Sqrt(result);
float ratio = Settings.MaxTranslation / sq;
b.LinearVelocityInternal.x *= ratio;
b.LinearVelocityInternal.y *= ratio;
}
float rotation = step.dt * b.AngularVelocityInternal;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MaxRotation / Mathf.Abs(rotation);
b.AngularVelocityInternal *= ratio;
}
// Store positions for continuous collision.
b.Sweep.C0.x = b.Sweep.C.x;
b.Sweep.C0.y = b.Sweep.C.y;
b.Sweep.A0 = b.Sweep.A;
// Integrate
b.Sweep.C.x += step.dt * b.LinearVelocityInternal.x;
b.Sweep.C.y += step.dt * b.LinearVelocityInternal.y;
b.Sweep.A += step.dt * b.AngularVelocityInternal;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints();
jointsOkay = jointsOkay && jointOkay;
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
JointUpdateTime = _tmpTime;
}
#endif
Report(_contactSolver.Constraints);
if (Settings.AllowSleep)
{
float minSleepTime = Settings.MaxFloat;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0 ||
b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr ||
Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += step.dt;
minSleepTime = Mathf.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
private void JointRemovedFired(Joint joint)
{
if (joint is DistanceJoint)
_removedJoints++;
}
protected void readCustomPropertiesFromJson(Joint item, JObject value)
{
if (null == item)
return;
if (value["customProperties"] != null)
return;
int i = 0;
JArray propValues = (JArray)value["customProperties"];
if (null != propValues)
{
int numPropValues = propValues.Count;
for (i = 0; i < numPropValues; i++)
{
JObject propValue = (JObject)propValues[i];
String propertyName = propValue["name"].ToString();
if (propValue["int"] != null)
SetCustomInt(item, propertyName, (int)propValue["int"]);
if (propValue["float"] != null)
SetCustomFloat(item, propertyName, (float)propValue["float"]);
if (propValue["string"] != null)
SetCustomString(item, propertyName, propValue["string"].ToString());
if (propValue["vec2"] != null)
SetCustomVector(item, propertyName, this.jsonToVec("vec2", propValue));
if (propValue["bool"] != null)
SetCustomBool(item, propertyName, (bool)propValue["bool"]);
}
}
}
/// <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;
m_typeA = jointA.JointType;
m_typeB = jointB.JointType;
// Make sure its the right kind of joint
Debug.Assert(m_typeA == JointType.Revolute || m_typeA == JointType.Prismatic || m_typeA == JointType.FixedRevolute || m_typeA == JointType.FixedPrismatic);
Debug.Assert(m_typeB == JointType.Revolute || m_typeB == JointType.Prismatic || m_typeB == JointType.FixedRevolute || m_typeB == JointType.FixedPrismatic);
float coordinateA = 0.0f, coordinateB = 0.0f;
m_bodyC = m_joint1.BodyA;
BodyA = m_joint1.BodyB;
// Get geometry of joint1
Transform xfA = BodyA.Xf;
float aA = BodyA.Sweep.A;
Transform xfC = m_bodyC.Xf;
float aC = m_bodyC.Sweep.A;
if (m_typeA == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointA;
m_localAnchorC = revolute.LocalAnchorA;
m_localAnchorA = revolute.LocalAnchorB;
m_referenceAngleA = revolute.ReferenceAngle;
m_localAxisC = Vector2.Zero;
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointA;
m_localAnchorC = prismatic.LocalAnchorA;
m_localAnchorA = prismatic.LocalAnchorB;
m_referenceAngleA = prismatic.ReferenceAngle;
m_localAxisC = prismatic.LocalXAxisA;
Vector2 pC = m_localAnchorC;
Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = Vector2.Dot(pA - pC, m_localAxisC);
}
m_bodyD = m_joint2.BodyA;
BodyB = m_joint2.BodyB;
// Get geometry of joint2
Transform xfB = BodyB.Xf;
float aB = BodyB.Sweep.A;
Transform xfD = m_bodyD.Xf;
float aD = m_bodyD.Sweep.A;
if (m_typeB == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointB;
m_localAnchorD = revolute.LocalAnchorA;
m_localAnchorB = revolute.LocalAnchorB;
m_referenceAngleB = revolute.ReferenceAngle;
m_localAxisD = Vector2.Zero;
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointB;
m_localAnchorD = prismatic.LocalAnchorA;
m_localAnchorB = prismatic.LocalAnchorB;
m_referenceAngleB = prismatic.ReferenceAngle;
m_localAxisD = prismatic.LocalXAxisA;
Vector2 pD = m_localAnchorD;
Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = Vector2.Dot(pB - pD, m_localAxisD);
}
_ratio = ratio;
m_constant = coordinateA + _ratio * coordinateB;
}
public void SetCustomFloat(Joint item, String propertyName, float val)
{
m_jointsWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_float.Add(propertyName, (float)val);
}
/// <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="joint1">The first joint.</param>
/// <param name="joint2">The second joint.</param>
/// <param name="ratio">The ratio.</param>
public GearJoint(Joint joint1, Joint joint2, float ratio = 1f)
{
JointType = JointType.Gear;
Joint1 = joint1;
Joint2 = joint2;
Ratio = ratio;
_typeA = joint1.JointType;
_typeB = joint2.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 = Joint1.BodyA;
_bodyA = Joint1.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)joint1;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC = Vector2.Zero;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)joint1;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxisA;
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 = Joint2.BodyA;
_bodyB = Joint2.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)joint2;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD = Vector2.Zero;
coordinateB = aB - aD - _referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)joint2;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxisA;
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 void SetCustomString(Joint item, String propertyName, String val)
{
m_jointsWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_string.Add(propertyName, val);
}
public void Add(Joint joint)
{
Debug.Assert(JointCount < _jointCapacity);
_joints[JointCount++] = joint;
}
public void SetCustomVector(Joint item, String propertyName, Vector2 val)
{
m_jointsWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_vec2.Add(propertyName, val);
}
/// <summary>
/// Create a joint to constrain bodies together. This may cause the connected bodies to cease colliding.
/// </summary>
/// <param name="joint">The joint.</param>
public void AddJoint(Joint joint)
{
Debug.Assert(!_jointAddList.Contains(joint), "You are adding the same joint more than once.");
if (!_jointAddList.Contains(joint))
_jointAddList.Add(joint);
}
public void SetCustomBool(Joint item, String propertyName, bool val)
{
m_jointsWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_bool.Add(propertyName, val);
}
/// <summary>
/// Destroy a joint. This may cause the connected bodies to begin colliding.
/// </summary>
/// <param name="joint">The joint.</param>
public void RemoveJoint(Joint joint)
{
RemoveJoint(joint, true);
}
public JObject B2n(Joint joint)
{
JObject jointValue = new JObject();
String jointName = GetJointName(joint);
if (null != jointName)
jointValue["name"] = jointName;
int bodyIndexA = lookupBodyIndex(joint.BodyA);
int bodyIndexB = lookupBodyIndex(joint.BodyB);
jointValue["bodyA"] = bodyIndexA;
jointValue["bodyB"] = bodyIndexB;
if (joint.CollideConnected)
jointValue["collideConnected"] = true;
Body bodyA = joint.BodyA;
Body bodyB = joint.BodyB;
// why do Joint.getAnchor methods need to take an argOut style
// parameter!?
Vector2 tmpAnchor = new Vector2();
switch (joint.JointType)
{
case JointType.Revolute:
{
jointValue["type"] = "revolute";
RevoluteJoint revoluteJoint = (RevoluteJoint)joint;
tmpAnchor = revoluteJoint.WorldAnchorA;
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = revoluteJoint.WorldAnchorA;
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("refAngle", bodyB.Rotation - bodyA.Rotation - revoluteJoint.JointAngle, jointValue);
FloatToJson("jointSpeed", revoluteJoint.JointSpeed, jointValue);
jointValue["enableLimit"] = revoluteJoint.LimitEnabled;
FloatToJson("lowerLimit", revoluteJoint.LowerLimit, jointValue);
FloatToJson("upperLimit", revoluteJoint.UpperLimit, jointValue);
jointValue["enableMotor"] = revoluteJoint.Enabled;
FloatToJson("motorSpeed", revoluteJoint.MotorSpeed, jointValue);
FloatToJson("maxMotorTorque", revoluteJoint.MaxMotorTorque, jointValue);
}
break;
case JointType.Prismatic:
{
jointValue["type"] = "prismatic";
PrismaticJoint prismaticJoint = (PrismaticJoint)joint;
tmpAnchor = prismaticJoint.WorldAnchorA;
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = prismaticJoint.WorldAnchorB;
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
VecToJson("localAxisA", prismaticJoint.LocalXAxisA, jointValue);
FloatToJson("refAngle", prismaticJoint.ReferenceAngle, jointValue);
jointValue["enableLimit"] = prismaticJoint.LimitEnabled;
FloatToJson("lowerLimit", prismaticJoint.LowerLimit, jointValue);
FloatToJson("upperLimit", prismaticJoint.UpperLimit, jointValue);
jointValue["enableMotor"] = prismaticJoint.MotorEnabled;
FloatToJson("maxMotorForce", prismaticJoint.MaxMotorForce, jointValue);
FloatToJson("motorSpeed", prismaticJoint.MotorSpeed, jointValue);
}
break;
case JointType.Distance:
{
jointValue["type"] = "distance";
DistanceJoint distanceJoint = (DistanceJoint)joint;
tmpAnchor = distanceJoint.WorldAnchorA;
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = distanceJoint.WorldAnchorB;
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("length", distanceJoint.Length, jointValue);
FloatToJson("frequency", distanceJoint.Frequency, jointValue);
FloatToJson("dampingRatio", distanceJoint.DampingRatio, jointValue);
}
break;
case JointType.Pulley:
{
jointValue["type"] = "pulley";
PulleyJoint pulleyJoint = (PulleyJoint)joint;
VecToJson("groundAnchorA", pulleyJoint.GroundAnchorA, jointValue);
VecToJson("groundAnchorB", pulleyJoint.GroundAnchorB, jointValue);
tmpAnchor = pulleyJoint.WorldAnchorA;
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("lengthA", (pulleyJoint.GroundAnchorA - tmpAnchor).Length(), jointValue);
tmpAnchor = pulleyJoint.WorldAnchorB;
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("lengthB", (pulleyJoint.GroundAnchorB - tmpAnchor).Length(), jointValue);
FloatToJson("ratio", pulleyJoint.Ratio, jointValue);
}
break;
case JointType.FixedMouse:
{
jointValue["type"] = "mouse";
FixedMouseJoint mouseJoint = (FixedMouseJoint)joint;
VecToJson("target", mouseJoint.WorldAnchorB, jointValue);
tmpAnchor = mouseJoint.WorldAnchorB;
VecToJson("anchorB", tmpAnchor, jointValue);
FloatToJson("maxForce", mouseJoint.MaxForce, jointValue);
FloatToJson("frequency", mouseJoint.Frequency, jointValue);
FloatToJson("dampingRatio", mouseJoint.DampingRatio, jointValue);
}
break;
case JointType.Gear:
{
jointValue["type"] = "gear";
GearJoint gearJoint = (GearJoint)joint;
int jointIndex1 = lookupJointIndex(gearJoint.Joint1);
int jointIndex2 = lookupJointIndex(gearJoint.Joint2);
jointValue["joint1"] = jointIndex1;
jointValue["joint2"] = jointIndex2;
jointValue["ratio"] = gearJoint.Ratio;
}
break;
case JointType.Wheel:
{
jointValue["type"] = "wheel";
WheelJoint wheelJoint = (WheelJoint)joint;
tmpAnchor = wheelJoint.LocalAnchorA;
VecToJson("anchorA", tmpAnchor, jointValue);
tmpAnchor = wheelJoint.LocalAnchorB;
VecToJson("anchorB", tmpAnchor, jointValue);
VecToJson("localAxisA", wheelJoint.LocalXAxisA , jointValue);
jointValue["enableMotor"] = wheelJoint.MotorEnabled;
FloatToJson("motorSpeed", wheelJoint.MotorSpeed, jointValue);
FloatToJson("maxMotorTorque", wheelJoint.MaxMotorTorque, jointValue);
FloatToJson("springFrequency", wheelJoint.SpringFrequencyHz, jointValue);
FloatToJson("springDampingRatio", wheelJoint.SpringDampingRatio, jointValue);
}
break;
case JointType.Weld:
{
jointValue["type"] = "weld";
WeldJoint weldJoint = (WeldJoint)joint;
tmpAnchor = weldJoint.WorldAnchorA;
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = weldJoint.WorldAnchorB;
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("refAngle", weldJoint.ReferenceAngle, jointValue);
FloatToJson("frequency", weldJoint.FrequencyHz, jointValue);
FloatToJson("dampingRatio", weldJoint.DampingRatio, jointValue);
}
break;
case JointType.Friction:
{
jointValue["type"] = "friction";
FrictionJoint frictionJoint = (FrictionJoint)joint;
tmpAnchor = frictionJoint.WorldAnchorA;
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = frictionJoint.WorldAnchorB;
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("maxForce", frictionJoint.MaxForce, jointValue);
FloatToJson("maxTorque", frictionJoint.MaxTorque, jointValue);
}
break;
case JointType.Rope:
{
// Rope joints are apparently not implemented in JBox2D yet, but
// when they are, commenting out the following section should work.
jointValue["type"] = "rope";
RopeJoint ropeJoint = (RopeJoint)joint;
tmpAnchor = ropeJoint.WorldAnchorA;
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = ropeJoint.WorldAnchorB;
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("maxLength", ropeJoint.MaxLength, jointValue);
}
break;
case JointType.Motor:
{
jointValue["type"] = "motor";
MotorJoint motor = (MotorJoint)joint;
tmpAnchor = motor.WorldAnchorA;
VecToJson("anchorA", motor.LinearOffset, jointValue);
//tmpAnchor = motor.WorldAnchorB;
//VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("maxForce", motor.MaxForce, jointValue);
FloatToJson("maxTorque", motor.MaxTorque, jointValue);
FloatToJson("refAngle", motor.AngularOffset, jointValue);
}
break;
default:
Console.WriteLine("Unknown joint type : " + joint.JointType);
break;
}
JArray customPropertyValue = WriteCustomPropertiesToJson(joint);
if (customPropertyValue.Count > 0)
jointValue["customProperties"] = customPropertyValue;
return jointValue;
}
/// <summary>
/// Destroy a joint. This may cause the connected bodies to begin colliding.
/// </summary>
/// <param name="joint">The joint.</param>
public void RemoveJoint(Joint joint)
{
//You are removing a joint twice?
Debug.Assert(!_jointRemoveList.Contains(joint));
if (!_jointRemoveList.Contains(joint))
_jointRemoveList.Add(joint);
}
/// <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.BodyA)
{
this.JointType = JointType.Gear;
this.JointA = jointA;
this.JointB = jointB;
this.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);
Debug.Assert(type2 == JointType.Revolute ||
type2 == JointType.Prismatic);
// 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.BodyB.BodyType == BodyType.Static);
}
if (type2 == JointType.Revolute || type2 == JointType.Prismatic)
{
Debug.Assert(jointB.BodyB.BodyType == BodyType.Static);
}
float coordinate1 = 0.0f, coordinate2 = 0.0f;
switch (type1)
{
case JointType.Revolute:
this.BodyA = jointA.BodyA;
this._revolute1 = (RevoluteJoint)jointA;
this.LocalAnchor1 = this._revolute1.LocalAnchorA;
coordinate1 = this._revolute1.JointAngle;
break;
case JointType.Prismatic:
this.BodyA = jointA.BodyA;
this._prismatic1 = (PrismaticJoint)jointA;
this.LocalAnchor1 = this._prismatic1.LocalAnchorA;
coordinate1 = this._prismatic1.JointTranslation;
break;
}
switch (type2)
{
case JointType.Revolute:
this.BodyB = jointB.BodyA;
this._revolute2 = (RevoluteJoint)jointB;
this.LocalAnchor2 = this._revolute2.LocalAnchorA;
coordinate2 = this._revolute2.JointAngle;
break;
case JointType.Prismatic:
this.BodyB = jointB.BodyA;
this._prismatic2 = (PrismaticJoint)jointB;
this.LocalAnchor2 = this._prismatic2.LocalAnchorA;
coordinate2 = this._prismatic2.JointTranslation;
break;
}
this._ant = coordinate1 + this.Ratio * coordinate2;
}