static int IntToEnum(IntPtr L)
{
int arg0 = (int)LuaDLL.lua_tonumber(L, 1);
JointLimitState2D o = (JointLimitState2D)arg0;
LuaScriptMgr.PushEnum(L, o);
return(1);
}
// Update is called once per frame
void Update()
{
JointLimitState2D limitState = sliderJ.limitState;
if (limitState == JointLimitState2D.LowerLimit)
{
ApplyMotorSpeed(movSpeed);
}
else if (limitState == JointLimitState2D.UpperLimit)
{
ApplyMotorSpeed(-movSpeed);
}
}
internal P2DSliderJoint(Rigidbody2DExt bodyA, Rigidbody2DExt bodyB, Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis)
: base(bodyA, bodyB)
{
_jointType = JointType.Slider;
_localAnchorA = localAnchorA;
_localAnchorB = localAnchorB;
SetAxis(axis);
m_LocalYAxisA = MathUtils.Cross(1.0f, localXAxisA);
referenceAngle = -Mathf.DeltaAngle(bodyB.rotation, bodyA.rotation) * Mathf.Deg2Rad;
m_LimitState = JointLimitState2D.Inactive;
}
internal override void InitVelocityConstraints()
{
m_LocalCenterA = _bodyA.centerOfMass;
m_LocalCenterB = _bodyB.centerOfMass;
m_InvMassA = _bodyA._invMass;
m_InvMassB = _bodyB._invMass;
m_InvIA = _bodyA._invI;
m_InvIB = _bodyB._invI;
Vector2 cA = _bodyA.GetRelativePoint(m_LocalCenterA);
float aA = _bodyA.rotation * Mathf.Deg2Rad;
Vector2 vA = _bodyA.velocity;
float wA = _bodyA.angularVelocity * Mathf.Deg2Rad;
Vector2 cB = _bodyB.GetRelativePoint(m_LocalCenterB);
float aB = _bodyB.rotation * Mathf.Deg2Rad;
Vector2 vB = _bodyB.velocity;
float wB = _bodyB.angularVelocity * Mathf.Deg2Rad;
Rot qA = new Rot(aA), qB = new Rot(aB);
Vector2 rA = MathUtils.Mul(qA, _localAnchorA - m_LocalCenterA);
Vector2 rB = MathUtils.Mul(qB, _localAnchorB - m_LocalCenterB);
float mA = m_InvMassA, mB = m_InvMassB;
float iA = m_InvIA, iB = m_InvIB;
Vector2 d = (cB - cA) + rB - rA;
// Compute motor Jacobian and effective mass.
{
m_Axis = MathUtils.Mul(qA, localXAxisA);
m_A1 = MathUtils.Cross(d + rA, m_Axis);
m_A2 = MathUtils.Cross(rB, m_Axis);
m_MotorMass = mA + mB + iA * m_A1 * m_A1 + iB * m_A2 * m_A2;
if (m_MotorMass > 0.0f)
{
m_MotorMass = 1.0f / m_MotorMass;
}
}
// Prismatic constraint.
{
m_Perp = MathUtils.Mul(qA, m_LocalYAxisA);
m_S1 = MathUtils.Cross(d + rA, m_Perp);
m_S2 = MathUtils.Cross(rB, m_Perp);
float k11 = mA + mB + iA * m_S1 * m_S1 + iB * m_S2 * m_S2;
float k12 = iA * m_S1 + iB * m_S2;
float k13 = iA * m_S1 * m_A1 + iB * m_S2 * m_A2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * m_A1 + iB * m_A2;
float k33 = mA + mB + iA * m_A1 * m_A1 + iB * m_A2 * m_A2;
m_K.ex = new Vector3(k11, k12, k13);
m_K.ey = new Vector3(k12, k22, k23);
m_K.ez = new Vector3(k13, k23, k33);
}
// Get joint translation
m_Translation = Vector2.Dot(m_Axis, d);
// Compute motor and limit terms.
if (m_EnableLimit)
{
float jointTranslation = m_Translation;
if (Mathf.Abs(m_UpperTranslation - m_LowerTranslation) < 2.0f * Constants.linearSlop)
{
m_LimitState = JointLimitState2D.EqualLimits;
}
else if (jointTranslation <= m_LowerTranslation)
{
if (m_LimitState != JointLimitState2D.LowerLimit)
{
m_LimitState = JointLimitState2D.LowerLimit;
m_Impulse.z = 0.0f;
}
}
else if (jointTranslation >= m_UpperTranslation)
{
if (m_LimitState != JointLimitState2D.UpperLimit)
{
m_LimitState = JointLimitState2D.UpperLimit;
m_Impulse.z = 0.0f;
}
}
else
{
m_LimitState = JointLimitState2D.Inactive;
m_Impulse.z = 0.0f;
}
}
else
{
m_LimitState = JointLimitState2D.Inactive;
}
if (m_EnableMotor == false)
{
m_MotorImpulse = 0.0f;
}
// TODO: Account for variable time step.
// m_Impulse *= data.step.dtRatio;
// m_MotorImpulse *= data.step.dtRatio;
Vector2 P = m_Impulse.x * m_Perp + (m_MotorImpulse + m_Impulse.z) * m_Axis;
float LA = m_Impulse.x * m_S1 + m_Impulse.y + (m_MotorImpulse + m_Impulse.z) * m_A1;
float LB = m_Impulse.x * m_S2 + m_Impulse.y + (m_MotorImpulse + m_Impulse.z) * m_A2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
if (!_bodyA.isKinematic)
{
_bodyA.velocity = vA;
if (!_bodyA.fixedAngle)
{
_bodyA.angularVelocity = wA * Mathf.Rad2Deg;
}
}
if (!_bodyB.isKinematic)
{
_bodyB.velocity = vB;
if (!_bodyB.fixedAngle)
{
_bodyB.angularVelocity = wB * Mathf.Rad2Deg;
}
}
}
