internal WheelJoint(WheelJointDef def)
: base(def)
{
_localAnchorA = def.LocalAnchorA;
_localAnchorB = def.LocalAnchorB;
_localXAxisA = def.LocalAxisA;
_localYAxisA = MathUtils.Cross(1.0f, _localXAxisA);
_mass = 0.0f;
_impulse = 0.0f;
_motorMass = 0.0f;
_motorImpulse = 0.0f;
_springMass = 0.0f;
_springImpulse = 0.0f;
_axialMass = 0.0f;
_lowerImpulse = 0.0f;
_upperImpulse = 0.0f;
_lowerTranslation = def.LowerTranslation;
_upperTranslation = def.UpperTranslation;
_enableLimit = def.EnableLimit;
_maxMotorTorque = def.MaxMotorTorque;
_motorSpeed = def.MotorSpeed;
_enableMotor = def.EnableMotor;
_bias = 0.0f;
_gamma = 0.0f;
_ax.SetZero();
_ay.SetZero();
_stiffness = def.Stiffness;
_damping = def.Damping;
}
public FrictionJointDef()
{
JointType = JointType.FrictionJoint;
LocalAnchorA.SetZero();
LocalAnchorB.SetZero();
MaxForce = 0.0f;
MaxTorque = 0.0f;
}
public MotorJointDef()
{
JointType = JointType.MotorJoint;
LinearOffset.SetZero();
AngularOffset = 0.0f;
MaxForce = 1.0f;
MaxTorque = 1.0f;
CorrectionFactor = 0.3f;
}
internal FrictionJoint(FrictionJointDef def) : base(def)
{
_localAnchorA = def.LocalAnchorA;
_localAnchorB = def.LocalAnchorB;
_linearImpulse.SetZero();
_angularImpulse = 0.0f;
_maxForce = def.MaxForce;
_maxTorque = def.MaxTorque;
}
internal MotorJoint(MotorJointDef def) : base(def)
{
_linearOffset = def.LinearOffset;
_angularOffset = def.AngularOffset;
_linearImpulse.SetZero();
_angularImpulse = 0.0f;
_maxForce = def.MaxForce;
_maxTorque = def.MaxTorque;
_correctionFactor = def.CorrectionFactor;
}
/// Build vertices to represent an axis-aligned box centered on the local origin.
/// @param hx the half-width.
/// @param hy the half-height.
public void SetAsBox(float hx, float hy)
{
Count = 4;
Vertices[0].Set(-hx, -hy);
Vertices[1].Set(hx, -hy);
Vertices[2].Set(hx, hy);
Vertices[3].Set(-hx, hy);
Normals[0].Set(0.0f, -1.0f);
Normals[1].Set(1.0f, 0.0f);
Normals[2].Set(0.0f, 1.0f);
Normals[3].Set(-1.0f, 0.0f);
Centroid.SetZero();
}
internal MouseJoint(MouseJointDef def)
: base(def)
{
Target = def.Target;
_localAnchorB = MathUtils.MulT(BodyB.GetTransform(), Target);
MaxForce = def.MaxForce;
Stiffness = def.Stiffness;
Damping = def.Damping;
_impulse.SetZero();
_beta = 0.0f;
_gamma = 0.0f;
}
public PrismaticJointDef()
{
JointType = JointType.PrismaticJoint;
LocalAnchorA.SetZero();
LocalAnchorB.SetZero();
LocalAxisA.Set(1.0f, 0.0f);
ReferenceAngle = 0.0f;
EnableLimit = false;
LowerTranslation = 0.0f;
UpperTranslation = 0.0f;
EnableMotor = false;
MaxMotorForce = 0.0f;
MotorSpeed = 0.0f;
}
public WheelJointDef()
{
JointType = JointType.WheelJoint;
LocalAnchorA.SetZero();
LocalAnchorB.SetZero();
LocalAxisA.Set(1.0f, 0.0f);
EnableLimit = false;
LowerTranslation = 0.0f;
UpperTranslation = 0.0f;
EnableMotor = false;
MaxMotorTorque = 0.0f;
MotorSpeed = 0.0f;
Stiffness = 0.0f;
Damping = 0.0f;
}
internal RevoluteJoint(RevoluteJointDef def)
: base(def)
{
LocalAnchorA = def.LocalAnchorA;
LocalAnchorB = def.LocalAnchorB;
ReferenceAngle = def.ReferenceAngle;
_impulse.SetZero();
_motorImpulse = 0.0f;
_axialMass = 0.0f;
_lowerImpulse = 0.0f;
_upperImpulse = 0.0f;
_lowerAngle = def.LowerAngle;
_upperAngle = def.UpperAngle;
_maxMotorTorque = def.MaxMotorTorque;
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_angle = 0.0f;
}
public GearJoint(GearJointDef def)
: base(def)
{
_joint1 = def.Joint1;
_joint2 = def.Joint2;
_typeA = _joint1.JointType;
_typeB = _joint2.JointType;
Debug.Assert(_typeA == JointType.RevoluteJoint || _typeA == JointType.PrismaticJoint);
Debug.Assert(_typeB == JointType.RevoluteJoint || _typeB == JointType.PrismaticJoint);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
_bodyC = _joint1.BodyA;
BodyA = _joint1.BodyB;
// Body B on joint1 must be dynamic
Debug.Assert(BodyA.BodyType == BodyType.DynamicBody);
// Get geometry of joint1
var xfA = BodyA.Transform;
var aA = BodyA.Sweep.A;
var xfC = _bodyC.Transform;
var aC = _bodyC.Sweep.A;
if (_typeA == JointType.RevoluteJoint)
{
var revolute = (RevoluteJoint)def.Joint1;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC.SetZero();
coordinateA = aA - aC - _referenceAngleA;
}
else
{
var prismatic = (PrismaticJoint)def.Joint1;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxisA;
var pC = _localAnchorC;
var pA = MathUtils.MulT(
xfC.Rotation,
MathUtils.Mul(xfA.Rotation, _localAnchorA) + (xfA.Position - xfC.Position));
coordinateA = Vector2.Dot(pA - pC, _localAxisC);
}
_bodyD = _joint2.BodyA;
BodyB = _joint2.BodyB;
// Body B on joint2 must be dynamic
Debug.Assert(BodyB.BodyType == BodyType.DynamicBody);
// Get geometry of joint2
var xfB = BodyB.Transform;
var aB = BodyB.Sweep.A;
var xfD = _bodyD.Transform;
var aD = _bodyD.Sweep.A;
if (_typeB == JointType.RevoluteJoint)
{
var revolute = (RevoluteJoint)def.Joint2;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD.SetZero();
coordinateB = aB - aD - _referenceAngleB;
}
else
{
var prismatic = (PrismaticJoint)def.Joint2;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxisA;
var pD = _localAnchorD;
var pB = MathUtils.MulT(
xfD.Rotation,
MathUtils.Mul(xfB.Rotation, _localAnchorB) + (xfB.Position - xfD.Position));
coordinateB = Vector2.Dot(pB - pD, _localAxisD);
}
_ratio = def.Ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}