| public Compute ( System.Vector2 x1, float a1, System.Vector2 x2, float a2 ) : float | ||
| x1 | System.Vector2 | |
| a1 | float | |
| x2 | System.Vector2 | |
| a2 | float | |
| Результат | float | |
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
float Cdot = _J.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);
float impulse = _mass * (-Cdot);
_impulse += impulse;
b1._linearVelocity += b1._invMass * impulse * _J.Linear1;
b1._angularVelocity += b1._invI * impulse * _J.Angular1;
b2._linearVelocity += b2._invMass * impulse * _J.Linear2;
b2._angularVelocity += b2._invI * impulse * _J.Angular2;
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
float Cdot = _J.Compute(b1._linearVelocity, b1._angularVelocity,
b2._linearVelocity, b2._angularVelocity);
float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _mass * Cdot;
_force += force;
float P = Settings.FORCE_SCALE(step.Dt) * force;
b1._linearVelocity += b1._invMass * P * _J.Linear1;
b1._angularVelocity += b1._invI * P * _J.Angular1;
b2._linearVelocity += b2._invMass * P * _J.Linear2;
b2._angularVelocity += b2._invI * P * _J.Angular2;
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Solve linear constraint.
float linearCdot = _linearJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);
float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _linearMass * linearCdot;
_force += force;
float P = Settings.FORCE_SCALE(step.Dt) * force;
b1._linearVelocity += (invMass1 * P) * _linearJacobian.Linear1;
b1._angularVelocity += invI1 * P * _linearJacobian.Angular1;
b2._linearVelocity += (invMass2 * P) * _linearJacobian.Linear2;
b2._angularVelocity += invI2 * P * _linearJacobian.Angular2;
// Solve angular constraint.
float angularCdot = b2._angularVelocity - b1._angularVelocity;
float torque = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _angularMass * angularCdot;
_torque += torque;
float L = Settings.FORCE_SCALE(step.Dt) * torque;
b1._angularVelocity -= invI1 * L;
b2._angularVelocity += invI2 * L;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float motorCdot = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity) - _motorSpeed;
float motorForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * motorCdot;
float oldMotorForce = _motorForce;
_motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorForce, _maxMotorForce);
motorForce = _motorForce - oldMotorForce;
float P_ = Settings.FORCE_SCALE(step.Dt) * motorForce;
b1._linearVelocity += (invMass1 * P_) * _motorJacobian.Linear1;
b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1;
b2._linearVelocity += (invMass2 * P_) * _motorJacobian.Linear2;
b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2;
}
// Solve linear limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float limitCdot = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);
float limitForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * limitCdot;
if (_limitState == LimitState.EqualLimits)
{
_limitForce += limitForce;
}
else if (_limitState == LimitState.AtLowerLimit)
{
float oldLimitForce = _limitForce;
_limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f);
limitForce = _limitForce - oldLimitForce;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float oldLimitForce = _limitForce;
_limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f);
limitForce = _limitForce - oldLimitForce;
}
float P_ = Settings.FORCE_SCALE(step.Dt) * limitForce;
b1._linearVelocity += (invMass1 * P_) * _motorJacobian.Linear1;
b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1;
b2._linearVelocity += (invMass2 * P_) * _motorJacobian.Linear2;
b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2;
}
}
