internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2PrismaticJoint obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
internal static b2Joint Create(b2JointDef def)
{
b2Joint joint = null;
switch (def.type)
{
case b2JointType.e_distanceJoint:
{
joint = new b2DistanceJoint((b2DistanceJointDef)def);
}
break;
case b2JointType.e_mouseJoint:
{
joint = new b2MouseJoint((b2MouseJointDef)def);
}
break;
case b2JointType.e_prismaticJoint:
{
joint = new b2PrismaticJoint((b2PrismaticJointDef)def);
}
break;
case b2JointType.e_revoluteJoint:
{
joint = new b2RevoluteJoint((b2RevoluteJointDef)def);
}
break;
case b2JointType.e_pulleyJoint:
{
joint = new b2PulleyJoint((b2PulleyJointDef)def);
}
break;
case b2JointType.e_gearJoint:
{
joint = new b2GearJoint((b2GearJointDef)def);
}
break;
case b2JointType.e_wheelJoint:
{
joint = new b2WheelJoint((b2WheelJointDef)def);
}
break;
case b2JointType.e_weldJoint:
{
joint = new b2WeldJoint((b2WeldJointDef)def);
}
break;
case b2JointType.e_frictionJoint:
{
joint = new b2FrictionJoint((b2FrictionJointDef)def);
}
break;
case b2JointType.e_ropeJoint:
{
joint = new b2RopeJoint((b2RopeJointDef)def);
}
break;
case b2JointType.e_motorJoint:
{
joint = new b2MotorJoint((b2MotorJointDef)def);
}
break;
default:
Debug.Assert(false);
break;
}
return(joint);
}
// friend class b2Joint;
// 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
internal b2GearJoint(b2GearJointDef def) : base(def)
{
m_joint1 = def.joint1;
m_joint2 = def.joint2;
m_typeA = m_joint1.GetType();
m_typeB = m_joint2.GetType();
Debug.Assert(m_typeA == b2JointType.e_revoluteJoint || m_typeA == b2JointType.e_prismaticJoint);
Debug.Assert(m_typeB == b2JointType.e_revoluteJoint || m_typeB == b2JointType.e_prismaticJoint);
float coordinateA;
float coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
m_bodyC = m_joint1.GetBodyA();
m_bodyA = m_joint1.GetBodyB();
// Get geometry of joint1
b2Transform xfA = new b2Transform(m_bodyA.m_xf);
float aA = m_bodyA.m_sweep.a;
b2Transform xfC = new b2Transform(m_bodyC.m_xf);
float aC = m_bodyC.m_sweep.a;
if (m_typeA == b2JointType.e_revoluteJoint)
{
b2RevoluteJoint revolute = (b2RevoluteJoint)def.joint1;
m_localAnchorC = revolute.m_localAnchorA;
m_localAnchorA = revolute.m_localAnchorB;
m_referenceAngleA = revolute.m_referenceAngle;
m_localAxisC.SetZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint1;
m_localAnchorC = prismatic.m_localAnchorA;
m_localAnchorA = prismatic.m_localAnchorB;
m_referenceAngleA = prismatic.m_referenceAngle;
m_localAxisC = prismatic.m_localXAxisA;
b2Vec2 pC = new b2Vec2(m_localAnchorC);
b2Vec2 pA = Utils.b2MulT(xfC.q, Utils.b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = Utils.b2Dot(pA - pC, m_localAxisC);
}
m_bodyD = m_joint2.GetBodyA();
m_bodyB = m_joint2.GetBodyB();
// Get geometry of joint2
b2Transform xfB = new b2Transform(m_bodyB.m_xf);
float aB = m_bodyB.m_sweep.a;
b2Transform xfD = new b2Transform(m_bodyD.m_xf);
float aD = m_bodyD.m_sweep.a;
if (m_typeB == b2JointType.e_revoluteJoint)
{
b2RevoluteJoint revolute = (b2RevoluteJoint)def.joint2;
m_localAnchorD = revolute.m_localAnchorA;
m_localAnchorB = revolute.m_localAnchorB;
m_referenceAngleB = revolute.m_referenceAngle;
m_localAxisD.SetZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint2;
m_localAnchorD = prismatic.m_localAnchorA;
m_localAnchorB = prismatic.m_localAnchorB;
m_referenceAngleB = prismatic.m_referenceAngle;
m_localAxisD = prismatic.m_localXAxisA;
b2Vec2 pD = new b2Vec2(m_localAnchorD);
b2Vec2 pB = Utils.b2MulT(xfD.q, Utils.b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = Utils.b2Dot(pB - pD, m_localAxisD);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}
