Files
burrow-server/tools/decompiled/old-builds/b1568/Box2D/Dynamics/Joints/b2PulleyJoint.as
T

492 lines
20 KiB
ActionScript

package Box2D.Dynamics.Joints
{
import Box2D.Common.Math.b2Mat22;
import Box2D.Common.Math.b2Math;
import Box2D.Common.Math.b2Vec2;
import Box2D.Dynamics.b2Body;
import Box2D.Dynamics.b2TimeStep;
public class b2PulleyJoint extends b2Joint
{
public static const b2_minPulleyLength:Number = 2;
public var m_ground:b2Body;
public var m_groundAnchor1:b2Vec2 = new b2Vec2();
public var m_groundAnchor2:b2Vec2 = new b2Vec2();
public var m_localAnchor1:b2Vec2 = new b2Vec2();
public var m_localAnchor2:b2Vec2 = new b2Vec2();
public var m_u1:b2Vec2 = new b2Vec2();
public var m_u2:b2Vec2 = new b2Vec2();
public var m_constant:Number;
public var m_ratio:Number;
public var m_maxLength1:Number;
public var m_maxLength2:Number;
public var m_pulleyMass:Number;
public var m_limitMass1:Number;
public var m_limitMass2:Number;
public var m_force:Number;
public var m_limitForce1:Number;
public var m_limitForce2:Number;
public var m_positionImpulse:Number;
public var m_limitPositionImpulse1:Number;
public var m_limitPositionImpulse2:Number;
public var m_state:int;
public var m_limitState1:int;
public var m_limitState2:int;
public function b2PulleyJoint(param1:b2PulleyJointDef)
{
var _loc4_:* = null;
var _loc2_:Number = NaN;
var _loc3_:Number = NaN;
super(param1);
m_ground = m_body1.m_world.m_groundBody;
m_groundAnchor1.x = param1.groundAnchor1.x - m_ground.m_xf.position.x;
m_groundAnchor1.y = param1.groundAnchor1.y - m_ground.m_xf.position.y;
m_groundAnchor2.x = param1.groundAnchor2.x - m_ground.m_xf.position.x;
m_groundAnchor2.y = param1.groundAnchor2.y - m_ground.m_xf.position.y;
m_localAnchor1.SetV(param1.localAnchor1);
m_localAnchor2.SetV(param1.localAnchor2);
m_ratio = param1.ratio;
m_constant = param1.length1 + m_ratio * param1.length2;
m_maxLength1 = b2Math.b2Min(param1.maxLength1,m_constant - m_ratio * 2);
m_maxLength2 = b2Math.b2Min(param1.maxLength2,(m_constant - 2) / m_ratio);
m_force = 0;
m_limitForce1 = 0;
m_limitForce2 = 0;
}
override public function GetAnchor1() : b2Vec2
{
return m_body1.GetWorldPoint(m_localAnchor1);
}
override public function GetAnchor2() : b2Vec2
{
return m_body2.GetWorldPoint(m_localAnchor2);
}
override public function GetReactionForce() : b2Vec2
{
var _loc1_:b2Vec2 = m_u2.Copy();
_loc1_.Multiply(m_force);
return _loc1_;
}
override public function GetReactionTorque() : Number
{
return 0;
}
public function GetGroundAnchor1() : b2Vec2
{
var _loc1_:b2Vec2 = m_ground.m_xf.position.Copy();
_loc1_.Add(m_groundAnchor1);
return _loc1_;
}
public function GetGroundAnchor2() : b2Vec2
{
var _loc1_:b2Vec2 = m_ground.m_xf.position.Copy();
_loc1_.Add(m_groundAnchor2);
return _loc1_;
}
public function GetLength1() : Number
{
var _loc1_:b2Vec2 = m_body1.GetWorldPoint(m_localAnchor1);
var _loc3_:Number = m_ground.m_xf.position.x + m_groundAnchor1.x;
var _loc5_:Number = m_ground.m_xf.position.y + m_groundAnchor1.y;
var _loc2_:Number = _loc1_.x - _loc3_;
var _loc4_:Number = _loc1_.y - _loc5_;
return Math.sqrt(_loc2_ * _loc2_ + _loc4_ * _loc4_);
}
public function GetLength2() : Number
{
var _loc1_:b2Vec2 = m_body2.GetWorldPoint(m_localAnchor2);
var _loc3_:Number = m_ground.m_xf.position.x + m_groundAnchor2.x;
var _loc5_:Number = m_ground.m_xf.position.y + m_groundAnchor2.y;
var _loc2_:Number = _loc1_.x - _loc3_;
var _loc4_:Number = _loc1_.y - _loc5_;
return Math.sqrt(_loc2_ * _loc2_ + _loc4_ * _loc4_);
}
public function GetRatio() : Number
{
return m_ratio;
}
override public function InitVelocityConstraints(param1:b2TimeStep) : void
{
var _loc22_:b2Mat22 = null;
var _loc6_:Number = NaN;
var _loc12_:Number = NaN;
var _loc20_:Number = NaN;
var _loc19_:Number = NaN;
var _loc3_:b2Body = m_body1;
var _loc4_:b2Body = m_body2;
_loc22_ = _loc3_.m_xf.R;
var _loc21_:Number = m_localAnchor1.x - _loc3_.m_sweep.localCenter.x;
var _loc25_:Number = m_localAnchor1.y - _loc3_.m_sweep.localCenter.y;
var _loc14_:Number = _loc22_.col1.x * _loc21_ + _loc22_.col2.x * _loc25_;
_loc25_ = _loc22_.col1.y * _loc21_ + _loc22_.col2.y * _loc25_;
_loc21_ = _loc14_;
_loc22_ = _loc4_.m_xf.R;
var _loc10_:Number = m_localAnchor2.x - _loc4_.m_sweep.localCenter.x;
var _loc8_:Number = m_localAnchor2.y - _loc4_.m_sweep.localCenter.y;
_loc14_ = _loc22_.col1.x * _loc10_ + _loc22_.col2.x * _loc8_;
_loc8_ = _loc22_.col1.y * _loc10_ + _loc22_.col2.y * _loc8_;
_loc10_ = _loc14_;
var _loc18_:Number = _loc3_.m_sweep.c.x + _loc21_;
var _loc23_:Number = _loc3_.m_sweep.c.y + _loc25_;
var _loc7_:Number = _loc4_.m_sweep.c.x + _loc10_;
var _loc5_:Number = _loc4_.m_sweep.c.y + _loc8_;
var _loc26_:Number = m_ground.m_xf.position.x + m_groundAnchor1.x;
var _loc24_:Number = m_ground.m_xf.position.y + m_groundAnchor1.y;
var _loc9_:Number = m_ground.m_xf.position.x + m_groundAnchor2.x;
var _loc13_:Number = m_ground.m_xf.position.y + m_groundAnchor2.y;
m_u1.Set(_loc18_ - _loc26_,_loc23_ - _loc24_);
m_u2.Set(_loc7_ - _loc9_,_loc5_ - _loc13_);
var _loc16_:Number = m_u1.Length();
var _loc17_:Number = m_u2.Length();
if(_loc16_ > 0.005)
{
m_u1.Multiply(1 / _loc16_);
}
else
{
m_u1.SetZero();
}
if(_loc17_ > 0.005)
{
m_u2.Multiply(1 / _loc17_);
}
else
{
m_u2.SetZero();
}
var _loc2_:Number = m_constant - _loc16_ - m_ratio * _loc17_;
if(_loc2_ > 0)
{
m_state = 0;
m_force = 0;
}
else
{
m_state = 2;
m_positionImpulse = 0;
}
if(_loc16_ < m_maxLength1)
{
m_limitState1 = 0;
m_limitForce1 = 0;
}
else
{
m_limitState1 = 2;
m_limitPositionImpulse1 = 0;
}
if(_loc17_ < m_maxLength2)
{
m_limitState2 = 0;
m_limitForce2 = 0;
}
else
{
m_limitState2 = 2;
m_limitPositionImpulse2 = 0;
}
var _loc11_:Number = _loc21_ * m_u1.y - _loc25_ * m_u1.x;
var _loc15_:Number = _loc10_ * m_u2.y - _loc8_ * m_u2.x;
m_limitMass1 = _loc3_.m_invMass + _loc3_.m_invI * _loc11_ * _loc11_;
m_limitMass2 = _loc4_.m_invMass + _loc4_.m_invI * _loc15_ * _loc15_;
m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
m_limitMass1 = 1 / m_limitMass1;
m_limitMass2 = 1 / m_limitMass2;
m_pulleyMass = 1 / m_pulleyMass;
if(param1.warmStarting)
{
_loc6_ = param1.dt * (-m_force - m_limitForce1) * m_u1.x;
_loc12_ = param1.dt * (-m_force - m_limitForce1) * m_u1.y;
_loc20_ = param1.dt * (-m_ratio * m_force - m_limitForce2) * m_u2.x;
_loc19_ = param1.dt * (-m_ratio * m_force - m_limitForce2) * m_u2.y;
_loc3_.m_linearVelocity.x += _loc3_.m_invMass * _loc6_;
_loc3_.m_linearVelocity.y += _loc3_.m_invMass * _loc12_;
_loc3_.m_angularVelocity += _loc3_.m_invI * (_loc21_ * _loc12_ - _loc25_ * _loc6_);
_loc4_.m_linearVelocity.x += _loc4_.m_invMass * _loc20_;
_loc4_.m_linearVelocity.y += _loc4_.m_invMass * _loc19_;
_loc4_.m_angularVelocity += _loc4_.m_invI * (_loc10_ * _loc19_ - _loc8_ * _loc20_);
}
else
{
m_force = 0;
m_limitForce1 = 0;
m_limitForce2 = 0;
}
}
override public function SolveVelocityConstraints(param1:b2TimeStep) : void
{
var _loc11_:b2Mat22 = null;
var _loc17_:Number = NaN;
var _loc20_:Number = NaN;
var _loc19_:Number = NaN;
var _loc18_:Number = NaN;
var _loc6_:Number = NaN;
var _loc13_:Number = NaN;
var _loc8_:Number = NaN;
var _loc7_:Number = NaN;
var _loc14_:Number = NaN;
var _loc15_:Number = NaN;
var _loc5_:Number = NaN;
var _loc3_:b2Body = m_body1;
var _loc4_:b2Body = m_body2;
_loc11_ = _loc3_.m_xf.R;
var _loc9_:Number = m_localAnchor1.x - _loc3_.m_sweep.localCenter.x;
var _loc16_:Number = m_localAnchor1.y - _loc3_.m_sweep.localCenter.y;
var _loc2_:Number = _loc11_.col1.x * _loc9_ + _loc11_.col2.x * _loc16_;
_loc16_ = _loc11_.col1.y * _loc9_ + _loc11_.col2.y * _loc16_;
_loc9_ = _loc2_;
_loc11_ = _loc4_.m_xf.R;
var _loc12_:Number = m_localAnchor2.x - _loc4_.m_sweep.localCenter.x;
var _loc10_:Number = m_localAnchor2.y - _loc4_.m_sweep.localCenter.y;
_loc2_ = _loc11_.col1.x * _loc12_ + _loc11_.col2.x * _loc10_;
_loc10_ = _loc11_.col1.y * _loc12_ + _loc11_.col2.y * _loc10_;
_loc12_ = _loc2_;
if(m_state == 2)
{
_loc17_ = _loc3_.m_linearVelocity.x + -_loc3_.m_angularVelocity * _loc16_;
_loc20_ = _loc3_.m_linearVelocity.y + _loc3_.m_angularVelocity * _loc9_;
_loc19_ = _loc4_.m_linearVelocity.x + -_loc4_.m_angularVelocity * _loc10_;
_loc18_ = _loc4_.m_linearVelocity.y + _loc4_.m_angularVelocity * _loc12_;
_loc14_ = -(m_u1.x * _loc17_ + m_u1.y * _loc20_) - m_ratio * (m_u2.x * _loc19_ + m_u2.y * _loc18_);
_loc15_ = -param1.inv_dt * m_pulleyMass * _loc14_;
_loc5_ = m_force;
m_force = b2Math.b2Max(0,m_force + _loc15_);
_loc15_ = m_force - _loc5_;
_loc6_ = -param1.dt * _loc15_ * m_u1.x;
_loc13_ = -param1.dt * _loc15_ * m_u1.y;
_loc8_ = -param1.dt * m_ratio * _loc15_ * m_u2.x;
_loc7_ = -param1.dt * m_ratio * _loc15_ * m_u2.y;
_loc3_.m_linearVelocity.x += _loc3_.m_invMass * _loc6_;
_loc3_.m_linearVelocity.y += _loc3_.m_invMass * _loc13_;
_loc3_.m_angularVelocity += _loc3_.m_invI * (_loc9_ * _loc13_ - _loc16_ * _loc6_);
_loc4_.m_linearVelocity.x += _loc4_.m_invMass * _loc8_;
_loc4_.m_linearVelocity.y += _loc4_.m_invMass * _loc7_;
_loc4_.m_angularVelocity += _loc4_.m_invI * (_loc12_ * _loc7_ - _loc10_ * _loc8_);
}
if(m_limitState1 == 2)
{
_loc17_ = _loc3_.m_linearVelocity.x + -_loc3_.m_angularVelocity * _loc16_;
_loc20_ = _loc3_.m_linearVelocity.y + _loc3_.m_angularVelocity * _loc9_;
_loc14_ = -(m_u1.x * _loc17_ + m_u1.y * _loc20_);
_loc15_ = -param1.inv_dt * m_limitMass1 * _loc14_;
_loc5_ = m_limitForce1;
m_limitForce1 = b2Math.b2Max(0,m_limitForce1 + _loc15_);
_loc15_ = m_limitForce1 - _loc5_;
_loc6_ = -param1.dt * _loc15_ * m_u1.x;
_loc13_ = -param1.dt * _loc15_ * m_u1.y;
_loc3_.m_linearVelocity.x += _loc3_.m_invMass * _loc6_;
_loc3_.m_linearVelocity.y += _loc3_.m_invMass * _loc13_;
_loc3_.m_angularVelocity += _loc3_.m_invI * (_loc9_ * _loc13_ - _loc16_ * _loc6_);
}
if(m_limitState2 == 2)
{
_loc19_ = _loc4_.m_linearVelocity.x + -_loc4_.m_angularVelocity * _loc10_;
_loc18_ = _loc4_.m_linearVelocity.y + _loc4_.m_angularVelocity * _loc12_;
_loc14_ = -(m_u2.x * _loc19_ + m_u2.y * _loc18_);
_loc15_ = -param1.inv_dt * m_limitMass2 * _loc14_;
_loc5_ = m_limitForce2;
m_limitForce2 = b2Math.b2Max(0,m_limitForce2 + _loc15_);
_loc15_ = m_limitForce2 - _loc5_;
_loc8_ = -param1.dt * _loc15_ * m_u2.x;
_loc7_ = -param1.dt * _loc15_ * m_u2.y;
_loc4_.m_linearVelocity.x += _loc4_.m_invMass * _loc8_;
_loc4_.m_linearVelocity.y += _loc4_.m_invMass * _loc7_;
_loc4_.m_angularVelocity += _loc4_.m_invI * (_loc12_ * _loc7_ - _loc10_ * _loc8_);
}
}
override public function SolvePositionConstraints() : Boolean
{
var _loc14_:b2Mat22 = null;
var _loc12_:Number = NaN;
var _loc22_:Number = NaN;
var _loc15_:Number = NaN;
var _loc13_:Number = NaN;
var _loc8_:Number = NaN;
var _loc16_:Number = NaN;
var _loc10_:Number = NaN;
var _loc9_:Number = NaN;
var _loc6_:Number = NaN;
var _loc7_:Number = NaN;
var _loc1_:Number = NaN;
var _loc18_:Number = NaN;
var _loc19_:Number = NaN;
var _loc23_:Number = NaN;
var _loc3_:Number = NaN;
var _loc4_:b2Body = m_body1;
var _loc5_:b2Body = m_body2;
var _loc20_:Number = m_ground.m_xf.position.x + m_groundAnchor1.x;
var _loc17_:Number = m_ground.m_xf.position.y + m_groundAnchor1.y;
var _loc11_:Number = m_ground.m_xf.position.x + m_groundAnchor2.x;
var _loc21_:Number = m_ground.m_xf.position.y + m_groundAnchor2.y;
var _loc2_:Number = 0;
if(m_state == 2)
{
_loc14_ = _loc4_.m_xf.R;
_loc12_ = m_localAnchor1.x - _loc4_.m_sweep.localCenter.x;
_loc22_ = m_localAnchor1.y - _loc4_.m_sweep.localCenter.y;
_loc3_ = _loc14_.col1.x * _loc12_ + _loc14_.col2.x * _loc22_;
_loc22_ = _loc14_.col1.y * _loc12_ + _loc14_.col2.y * _loc22_;
_loc12_ = _loc3_;
_loc14_ = _loc5_.m_xf.R;
_loc15_ = m_localAnchor2.x - _loc5_.m_sweep.localCenter.x;
_loc13_ = m_localAnchor2.y - _loc5_.m_sweep.localCenter.y;
_loc3_ = _loc14_.col1.x * _loc15_ + _loc14_.col2.x * _loc13_;
_loc13_ = _loc14_.col1.y * _loc15_ + _loc14_.col2.y * _loc13_;
_loc15_ = _loc3_;
_loc8_ = _loc4_.m_sweep.c.x + _loc12_;
_loc16_ = _loc4_.m_sweep.c.y + _loc22_;
_loc10_ = _loc5_.m_sweep.c.x + _loc15_;
_loc9_ = _loc5_.m_sweep.c.y + _loc13_;
m_u1.Set(_loc8_ - _loc20_,_loc16_ - _loc17_);
m_u2.Set(_loc10_ - _loc11_,_loc9_ - _loc21_);
_loc6_ = m_u1.Length();
_loc7_ = m_u2.Length();
if(_loc6_ > 0.005)
{
m_u1.Multiply(1 / _loc6_);
}
else
{
m_u1.SetZero();
}
if(_loc7_ > 0.005)
{
m_u2.Multiply(1 / _loc7_);
}
else
{
m_u2.SetZero();
}
_loc1_ = m_constant - _loc6_ - m_ratio * _loc7_;
_loc2_ = b2Math.b2Max(_loc2_,-_loc1_);
_loc1_ = b2Math.b2Clamp(_loc1_ + 0.005,-0.2,0);
_loc18_ = -m_pulleyMass * _loc1_;
_loc19_ = m_positionImpulse;
m_positionImpulse = b2Math.b2Max(0,m_positionImpulse + _loc18_);
_loc18_ = m_positionImpulse - _loc19_;
_loc8_ = -_loc18_ * m_u1.x;
_loc16_ = -_loc18_ * m_u1.y;
_loc10_ = -m_ratio * _loc18_ * m_u2.x;
_loc9_ = -m_ratio * _loc18_ * m_u2.y;
_loc4_.m_sweep.c.x += _loc4_.m_invMass * _loc8_;
_loc4_.m_sweep.c.y += _loc4_.m_invMass * _loc16_;
_loc4_.m_sweep.a += _loc4_.m_invI * (_loc12_ * _loc16_ - _loc22_ * _loc8_);
_loc5_.m_sweep.c.x += _loc5_.m_invMass * _loc10_;
_loc5_.m_sweep.c.y += _loc5_.m_invMass * _loc9_;
_loc5_.m_sweep.a += _loc5_.m_invI * (_loc15_ * _loc9_ - _loc13_ * _loc10_);
_loc4_.SynchronizeTransform();
_loc5_.SynchronizeTransform();
}
if(m_limitState1 == 2)
{
_loc14_ = _loc4_.m_xf.R;
_loc12_ = m_localAnchor1.x - _loc4_.m_sweep.localCenter.x;
_loc22_ = m_localAnchor1.y - _loc4_.m_sweep.localCenter.y;
_loc3_ = _loc14_.col1.x * _loc12_ + _loc14_.col2.x * _loc22_;
_loc22_ = _loc14_.col1.y * _loc12_ + _loc14_.col2.y * _loc22_;
_loc12_ = _loc3_;
_loc8_ = _loc4_.m_sweep.c.x + _loc12_;
_loc16_ = _loc4_.m_sweep.c.y + _loc22_;
m_u1.Set(_loc8_ - _loc20_,_loc16_ - _loc17_);
_loc6_ = m_u1.Length();
if(_loc6_ > 0.005)
{
m_u1.x *= 1 / _loc6_;
m_u1.y *= 1 / _loc6_;
}
else
{
m_u1.SetZero();
}
_loc1_ = m_maxLength1 - _loc6_;
_loc2_ = b2Math.b2Max(_loc2_,-_loc1_);
_loc1_ = b2Math.b2Clamp(_loc1_ + 0.005,-0.2,0);
_loc18_ = -m_limitMass1 * _loc1_;
_loc23_ = m_limitPositionImpulse1;
m_limitPositionImpulse1 = b2Math.b2Max(0,m_limitPositionImpulse1 + _loc18_);
_loc18_ = m_limitPositionImpulse1 - _loc23_;
_loc8_ = -_loc18_ * m_u1.x;
_loc16_ = -_loc18_ * m_u1.y;
_loc4_.m_sweep.c.x += _loc4_.m_invMass * _loc8_;
_loc4_.m_sweep.c.y += _loc4_.m_invMass * _loc16_;
_loc4_.m_sweep.a += _loc4_.m_invI * (_loc12_ * _loc16_ - _loc22_ * _loc8_);
_loc4_.SynchronizeTransform();
}
if(m_limitState2 == 2)
{
_loc14_ = _loc5_.m_xf.R;
_loc15_ = m_localAnchor2.x - _loc5_.m_sweep.localCenter.x;
_loc13_ = m_localAnchor2.y - _loc5_.m_sweep.localCenter.y;
_loc3_ = _loc14_.col1.x * _loc15_ + _loc14_.col2.x * _loc13_;
_loc13_ = _loc14_.col1.y * _loc15_ + _loc14_.col2.y * _loc13_;
_loc15_ = _loc3_;
_loc10_ = _loc5_.m_sweep.c.x + _loc15_;
_loc9_ = _loc5_.m_sweep.c.y + _loc13_;
m_u2.Set(_loc10_ - _loc11_,_loc9_ - _loc21_);
_loc7_ = m_u2.Length();
if(_loc7_ > 0.005)
{
m_u2.x *= 1 / _loc7_;
m_u2.y *= 1 / _loc7_;
}
else
{
m_u2.SetZero();
}
_loc1_ = m_maxLength2 - _loc7_;
_loc2_ = b2Math.b2Max(_loc2_,-_loc1_);
_loc1_ = b2Math.b2Clamp(_loc1_ + 0.005,-0.2,0);
_loc18_ = -m_limitMass2 * _loc1_;
_loc23_ = m_limitPositionImpulse2;
m_limitPositionImpulse2 = b2Math.b2Max(0,m_limitPositionImpulse2 + _loc18_);
_loc18_ = m_limitPositionImpulse2 - _loc23_;
_loc10_ = -_loc18_ * m_u2.x;
_loc9_ = -_loc18_ * m_u2.y;
_loc5_.m_sweep.c.x += _loc5_.m_invMass * _loc10_;
_loc5_.m_sweep.c.y += _loc5_.m_invMass * _loc9_;
_loc5_.m_sweep.a += _loc5_.m_invI * (_loc15_ * _loc9_ - _loc13_ * _loc10_);
_loc5_.SynchronizeTransform();
}
return _loc2_ < 0.005;
}
}
}