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; }