Brought up to date with the private repository (feea4c7)

This commit is contained in:
S committed 2026-10-14 20:57:33 +03:00
1 parent 1aa738d692
commit 1b1df1ae48
7925 files changed
+2617237 -20

No files matched your search

@@ -0,0 +1,218 @@
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 b2DistanceJoint extends b2Joint
{
public var m_localAnchor1:b2Vec2 = new b2Vec2();
public var m_localAnchor2:b2Vec2 = new b2Vec2();
public var m_u:b2Vec2 = new b2Vec2();
public var m_frequencyHz:Number;
public var m_dampingRatio:Number;
public var m_gamma:Number;
public var m_bias:Number;
public var m_impulse:Number;
public var m_mass:Number;
public var m_length:Number;
public function b2DistanceJoint(param1:b2DistanceJointDef)
{
var _loc4_:* = null;
var _loc2_:Number = NaN;
var _loc3_:Number = NaN;
super(param1);
m_localAnchor1.SetV(param1.localAnchor1);
m_localAnchor2.SetV(param1.localAnchor2);
m_length = param1.length;
m_frequencyHz = param1.frequencyHz;
m_dampingRatio = param1.dampingRatio;
m_impulse = 0;
m_gamma = 0;
m_bias = 0;
m_inv_dt = 0;
}
override public function InitVelocityConstraints(param1:b2TimeStep) : void
{
var _loc15_:b2Mat22 = null;
var _loc4_:Number = NaN;
var _loc3_:Number = NaN;
var _loc10_:Number = NaN;
var _loc5_:Number = NaN;
var _loc9_:Number = NaN;
var _loc7_:Number = NaN;
var _loc8_:Number = NaN;
m_inv_dt = param1.inv_dt;
var _loc11_:b2Body = m_body1;
var _loc12_:b2Body = m_body2;
_loc15_ = _loc11_.m_xf.R;
var _loc13_:Number = m_localAnchor1.x - _loc11_.m_sweep.localCenter.x;
var _loc17_:Number = m_localAnchor1.y - _loc11_.m_sweep.localCenter.y;
_loc4_ = _loc15_.col1.x * _loc13_ + _loc15_.col2.x * _loc17_;
_loc17_ = _loc15_.col1.y * _loc13_ + _loc15_.col2.y * _loc17_;
_loc13_ = _loc4_;
_loc15_ = _loc12_.m_xf.R;
var _loc16_:Number = m_localAnchor2.x - _loc12_.m_sweep.localCenter.x;
var _loc14_:Number = m_localAnchor2.y - _loc12_.m_sweep.localCenter.y;
_loc4_ = _loc15_.col1.x * _loc16_ + _loc15_.col2.x * _loc14_;
_loc14_ = _loc15_.col1.y * _loc16_ + _loc15_.col2.y * _loc14_;
_loc16_ = _loc4_;
m_u.x = _loc12_.m_sweep.c.x + _loc16_ - _loc11_.m_sweep.c.x - _loc13_;
m_u.y = _loc12_.m_sweep.c.y + _loc14_ - _loc11_.m_sweep.c.y - _loc17_;
var _loc6_:Number = Math.sqrt(m_u.x * m_u.x + m_u.y * m_u.y);
if(_loc6_ > 0.005)
{
m_u.Multiply(1 / _loc6_);
}
else
{
m_u.SetZero();
}
var _loc19_:Number = _loc13_ * m_u.y - _loc17_ * m_u.x;
var _loc18_:Number = _loc16_ * m_u.y - _loc14_ * m_u.x;
var _loc2_:Number = _loc11_.m_invMass + _loc11_.m_invI * _loc19_ * _loc19_ + _loc12_.m_invMass + _loc12_.m_invI * _loc18_ * _loc18_;
m_mass = 1 / _loc2_;
if(m_frequencyHz > 0)
{
_loc3_ = _loc6_ - m_length;
_loc10_ = 2 * 3.141592653589793 * m_frequencyHz;
_loc5_ = 2 * m_mass * m_dampingRatio * _loc10_;
_loc9_ = m_mass * _loc10_ * _loc10_;
m_gamma = 1 / (param1.dt * (_loc5_ + param1.dt * _loc9_));
m_bias = _loc3_ * param1.dt * _loc9_ * m_gamma;
m_mass = 1 / (_loc2_ + m_gamma);
}
if(param1.warmStarting)
{
m_impulse *= param1.dtRatio;
_loc7_ = m_impulse * m_u.x;
_loc8_ = m_impulse * m_u.y;
_loc11_.m_linearVelocity.x -= _loc11_.m_invMass * _loc7_;
_loc11_.m_linearVelocity.y -= _loc11_.m_invMass * _loc8_;
_loc11_.m_angularVelocity -= _loc11_.m_invI * (_loc13_ * _loc8_ - _loc17_ * _loc7_);
_loc12_.m_linearVelocity.x += _loc12_.m_invMass * _loc7_;
_loc12_.m_linearVelocity.y += _loc12_.m_invMass * _loc8_;
_loc12_.m_angularVelocity += _loc12_.m_invI * (_loc16_ * _loc8_ - _loc14_ * _loc7_);
}
else
{
m_impulse = 0;
}
}
override public function SolveVelocityConstraints(param1:b2TimeStep) : void
{
var _loc9_:b2Mat22 = null;
var _loc5_:b2Body = m_body1;
var _loc6_:b2Body = m_body2;
_loc9_ = _loc5_.m_xf.R;
var _loc7_:Number = m_localAnchor1.x - _loc5_.m_sweep.localCenter.x;
var _loc13_:Number = m_localAnchor1.y - _loc5_.m_sweep.localCenter.y;
var _loc2_:Number = _loc9_.col1.x * _loc7_ + _loc9_.col2.x * _loc13_;
_loc13_ = _loc9_.col1.y * _loc7_ + _loc9_.col2.y * _loc13_;
_loc7_ = _loc2_;
_loc9_ = _loc6_.m_xf.R;
var _loc10_:Number = m_localAnchor2.x - _loc6_.m_sweep.localCenter.x;
var _loc8_:Number = m_localAnchor2.y - _loc6_.m_sweep.localCenter.y;
_loc2_ = _loc9_.col1.x * _loc10_ + _loc9_.col2.x * _loc8_;
_loc8_ = _loc9_.col1.y * _loc10_ + _loc9_.col2.y * _loc8_;
_loc10_ = _loc2_;
var _loc14_:Number = _loc5_.m_linearVelocity.x + -_loc5_.m_angularVelocity * _loc13_;
var _loc17_:Number = _loc5_.m_linearVelocity.y + _loc5_.m_angularVelocity * _loc7_;
var _loc16_:Number = _loc6_.m_linearVelocity.x + -_loc6_.m_angularVelocity * _loc8_;
var _loc15_:Number = _loc6_.m_linearVelocity.y + _loc6_.m_angularVelocity * _loc10_;
var _loc11_:Number = m_u.x * (_loc16_ - _loc14_) + m_u.y * (_loc15_ - _loc17_);
var _loc12_:Number = -m_mass * (_loc11_ + m_bias + m_gamma * m_impulse);
m_impulse += _loc12_;
var _loc3_:Number = _loc12_ * m_u.x;
var _loc4_:Number = _loc12_ * m_u.y;
_loc5_.m_linearVelocity.x -= _loc5_.m_invMass * _loc3_;
_loc5_.m_linearVelocity.y -= _loc5_.m_invMass * _loc4_;
_loc5_.m_angularVelocity -= _loc5_.m_invI * (_loc7_ * _loc4_ - _loc13_ * _loc3_);
_loc6_.m_linearVelocity.x += _loc6_.m_invMass * _loc3_;
_loc6_.m_linearVelocity.y += _loc6_.m_invMass * _loc4_;
_loc6_.m_angularVelocity += _loc6_.m_invI * (_loc10_ * _loc4_ - _loc8_ * _loc3_);
}
override public function SolvePositionConstraints() : Boolean
{
var _loc12_:b2Mat22 = null;
if(m_frequencyHz > 0)
{
return true;
}
var _loc6_:b2Body = m_body1;
var _loc7_:b2Body = m_body2;
_loc12_ = _loc6_.m_xf.R;
var _loc10_:Number = m_localAnchor1.x - _loc6_.m_sweep.localCenter.x;
var _loc15_:Number = m_localAnchor1.y - _loc6_.m_sweep.localCenter.y;
var _loc2_:Number = _loc12_.col1.x * _loc10_ + _loc12_.col2.x * _loc15_;
_loc15_ = _loc12_.col1.y * _loc10_ + _loc12_.col2.y * _loc15_;
_loc10_ = _loc2_;
_loc12_ = _loc7_.m_xf.R;
var _loc13_:Number = m_localAnchor2.x - _loc7_.m_sweep.localCenter.x;
var _loc11_:Number = m_localAnchor2.y - _loc7_.m_sweep.localCenter.y;
_loc2_ = _loc12_.col1.x * _loc13_ + _loc12_.col2.x * _loc11_;
_loc11_ = _loc12_.col1.y * _loc13_ + _loc12_.col2.y * _loc11_;
_loc13_ = _loc2_;
var _loc8_:Number = _loc7_.m_sweep.c.x + _loc13_ - _loc6_.m_sweep.c.x - _loc10_;
var _loc9_:Number = _loc7_.m_sweep.c.y + _loc11_ - _loc6_.m_sweep.c.y - _loc15_;
var _loc3_:Number = Math.sqrt(_loc8_ * _loc8_ + _loc9_ * _loc9_);
_loc8_ /= _loc3_;
_loc9_ /= _loc3_;
var _loc1_:Number = _loc3_ - m_length;
_loc1_ = b2Math.b2Clamp(_loc1_,-0.2,0.2);
var _loc14_:Number = -m_mass * _loc1_;
m_u.Set(_loc8_,_loc9_);
var _loc4_:Number = _loc14_ * m_u.x;
var _loc5_:Number = _loc14_ * m_u.y;
_loc6_.m_sweep.c.x -= _loc6_.m_invMass * _loc4_;
_loc6_.m_sweep.c.y -= _loc6_.m_invMass * _loc5_;
_loc6_.m_sweep.a -= _loc6_.m_invI * (_loc10_ * _loc5_ - _loc15_ * _loc4_);
_loc7_.m_sweep.c.x += _loc7_.m_invMass * _loc4_;
_loc7_.m_sweep.c.y += _loc7_.m_invMass * _loc5_;
_loc7_.m_sweep.a += _loc7_.m_invI * (_loc13_ * _loc5_ - _loc11_ * _loc4_);
_loc6_.SynchronizeTransform();
_loc7_.SynchronizeTransform();
return b2Math.b2Abs(_loc1_) < 0.005;
}
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 = new b2Vec2();
_loc1_.SetV(m_u);
_loc1_.Multiply(m_inv_dt * m_impulse);
return _loc1_;
}
override public function GetReactionTorque() : Number
{
return 0;
}
}
}
@@ -0,0 +1,42 @@
package Box2D.Dynamics.Joints
{
import Box2D.Common.Math.b2Vec2;
import Box2D.Dynamics.b2Body;
public class b2DistanceJointDef extends b2JointDef
{
public var localAnchor1:b2Vec2 = new b2Vec2();
public var localAnchor2:b2Vec2 = new b2Vec2();
public var length:Number;
public var frequencyHz:Number;
public var dampingRatio:Number;
public function b2DistanceJointDef()
{
super();
type = 3;
length = 1;
frequencyHz = 0;
dampingRatio = 0;
}
public function Initialize(param1:b2Body, param2:b2Body, param3:b2Vec2, param4:b2Vec2) : void
{
body1 = param1;
body2 = param2;
localAnchor1.SetV(body1.GetLocalPoint(param3));
localAnchor2.SetV(body2.GetLocalPoint(param4));
var _loc5_:Number = param4.x - param3.x;
var _loc6_:Number = param4.y - param3.y;
length = Math.sqrt(_loc5_ * _loc5_ + _loc6_ * _loc6_);
frequencyHz = 0;
dampingRatio = 0;
}
}
}
@@ -0,0 +1,251 @@
package Box2D.Dynamics.Joints
{
import Box2D.Common.Math.b2Mat22;
import Box2D.Common.Math.b2Vec2;
import Box2D.Dynamics.b2Body;
import Box2D.Dynamics.b2TimeStep;
public class b2GearJoint extends b2Joint
{
public var m_ground1:b2Body;
public var m_ground2:b2Body;
public var m_revolute1:b2RevoluteJoint;
public var m_prismatic1:b2PrismaticJoint;
public var m_revolute2:b2RevoluteJoint;
public var m_prismatic2:b2PrismaticJoint;
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_J:b2Jacobian = new b2Jacobian();
public var m_constant:Number;
public var m_ratio:Number;
public var m_mass:Number;
public var m_force:Number;
public function b2GearJoint(param1:b2GearJointDef)
{
var _loc4_:Number = NaN;
var _loc3_:Number = NaN;
super(param1);
var _loc5_:int = param1.joint1.m_type;
var _loc2_:int = param1.joint2.m_type;
m_revolute1 = null;
m_prismatic1 = null;
m_revolute2 = null;
m_prismatic2 = null;
m_ground1 = param1.joint1.m_body1;
m_body1 = param1.joint1.m_body2;
if(_loc5_ == 1)
{
m_revolute1 = param1.joint1 as b2RevoluteJoint;
m_groundAnchor1.SetV(m_revolute1.m_localAnchor1);
m_localAnchor1.SetV(m_revolute1.m_localAnchor2);
_loc4_ = m_revolute1.GetJointAngle();
}
else
{
m_prismatic1 = param1.joint1 as b2PrismaticJoint;
m_groundAnchor1.SetV(m_prismatic1.m_localAnchor1);
m_localAnchor1.SetV(m_prismatic1.m_localAnchor2);
_loc4_ = m_prismatic1.GetJointTranslation();
}
m_ground2 = param1.joint2.m_body1;
m_body2 = param1.joint2.m_body2;
if(_loc2_ == 1)
{
m_revolute2 = param1.joint2 as b2RevoluteJoint;
m_groundAnchor2.SetV(m_revolute2.m_localAnchor1);
m_localAnchor2.SetV(m_revolute2.m_localAnchor2);
_loc3_ = m_revolute2.GetJointAngle();
}
else
{
m_prismatic2 = param1.joint2 as b2PrismaticJoint;
m_groundAnchor2.SetV(m_prismatic2.m_localAnchor1);
m_localAnchor2.SetV(m_prismatic2.m_localAnchor2);
_loc3_ = m_prismatic2.GetJointTranslation();
}
m_ratio = param1.ratio;
m_constant = _loc4_ + m_ratio * _loc3_;
m_force = 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
{
return new b2Vec2(m_force * m_J.linear2.x,m_force * m_J.linear2.y);
}
override public function GetReactionTorque() : Number
{
var _loc4_:b2Mat22 = m_body2.m_xf.R;
var _loc2_:Number = m_localAnchor1.x - m_body2.m_sweep.localCenter.x;
var _loc3_:Number = m_localAnchor1.y - m_body2.m_sweep.localCenter.y;
var _loc1_:Number = _loc4_.col1.x * _loc2_ + _loc4_.col2.x * _loc3_;
_loc3_ = _loc4_.col1.y * _loc2_ + _loc4_.col2.y * _loc3_;
_loc2_ = _loc1_;
return m_force * m_J.angular2 - (_loc2_ * (m_force * m_J.linear2.y) - _loc3_ * (m_force * m_J.linear2.x));
}
public function GetRatio() : Number
{
return m_ratio;
}
override public function InitVelocityConstraints(param1:b2TimeStep) : void
{
var _loc5_:Number = NaN;
var _loc3_:Number = NaN;
var _loc4_:Number = NaN;
var _loc6_:Number = NaN;
var _loc15_:b2Mat22 = null;
var _loc14_:b2Vec2 = null;
var _loc11_:Number = NaN;
var _loc2_:Number = NaN;
var _loc13_:Number = NaN;
var _loc7_:b2Body = m_ground1;
var _loc8_:b2Body = m_ground2;
var _loc10_:b2Body = m_body1;
var _loc12_:b2Body = m_body2;
var _loc9_:Number = 0;
m_J.SetZero();
if(m_revolute1)
{
m_J.angular1 = -1;
_loc9_ += _loc10_.m_invI;
}
else
{
_loc15_ = _loc7_.m_xf.R;
_loc14_ = m_prismatic1.m_localXAxis1;
_loc5_ = _loc15_.col1.x * _loc14_.x + _loc15_.col2.x * _loc14_.y;
_loc3_ = _loc15_.col1.y * _loc14_.x + _loc15_.col2.y * _loc14_.y;
_loc15_ = _loc10_.m_xf.R;
_loc4_ = m_localAnchor1.x - _loc10_.m_sweep.localCenter.x;
_loc6_ = m_localAnchor1.y - _loc10_.m_sweep.localCenter.y;
_loc2_ = _loc15_.col1.x * _loc4_ + _loc15_.col2.x * _loc6_;
_loc6_ = _loc15_.col1.y * _loc4_ + _loc15_.col2.y * _loc6_;
_loc4_ = _loc2_;
_loc11_ = _loc4_ * _loc3_ - _loc6_ * _loc5_;
m_J.linear1.Set(-_loc5_,-_loc3_);
m_J.angular1 = -_loc11_;
_loc9_ += _loc10_.m_invMass + _loc10_.m_invI * _loc11_ * _loc11_;
}
if(m_revolute2)
{
m_J.angular2 = -m_ratio;
_loc9_ += m_ratio * m_ratio * _loc12_.m_invI;
}
else
{
_loc15_ = _loc8_.m_xf.R;
_loc14_ = m_prismatic2.m_localXAxis1;
_loc5_ = _loc15_.col1.x * _loc14_.x + _loc15_.col2.x * _loc14_.y;
_loc3_ = _loc15_.col1.y * _loc14_.x + _loc15_.col2.y * _loc14_.y;
_loc15_ = _loc12_.m_xf.R;
_loc4_ = m_localAnchor2.x - _loc12_.m_sweep.localCenter.x;
_loc6_ = m_localAnchor2.y - _loc12_.m_sweep.localCenter.y;
_loc2_ = _loc15_.col1.x * _loc4_ + _loc15_.col2.x * _loc6_;
_loc6_ = _loc15_.col1.y * _loc4_ + _loc15_.col2.y * _loc6_;
_loc4_ = _loc2_;
_loc11_ = _loc4_ * _loc3_ - _loc6_ * _loc5_;
m_J.linear2.Set(-m_ratio * _loc5_,-m_ratio * _loc3_);
m_J.angular2 = -m_ratio * _loc11_;
_loc9_ += m_ratio * m_ratio * (_loc12_.m_invMass + _loc12_.m_invI * _loc11_ * _loc11_);
}
m_mass = 1 / _loc9_;
if(param1.warmStarting)
{
_loc13_ = param1.dt * m_force;
_loc10_.m_linearVelocity.x += _loc10_.m_invMass * _loc13_ * m_J.linear1.x;
_loc10_.m_linearVelocity.y += _loc10_.m_invMass * _loc13_ * m_J.linear1.y;
_loc10_.m_angularVelocity += _loc10_.m_invI * _loc13_ * m_J.angular1;
_loc12_.m_linearVelocity.x += _loc12_.m_invMass * _loc13_ * m_J.linear2.x;
_loc12_.m_linearVelocity.y += _loc12_.m_invMass * _loc13_ * m_J.linear2.y;
_loc12_.m_angularVelocity += _loc12_.m_invI * _loc13_ * m_J.angular2;
}
else
{
m_force = 0;
}
}
override public function SolveVelocityConstraints(param1:b2TimeStep) : void
{
var _loc6_:b2Body = m_body1;
var _loc2_:b2Body = m_body2;
var _loc4_:Number = m_J.Compute(_loc6_.m_linearVelocity,_loc6_.m_angularVelocity,_loc2_.m_linearVelocity,_loc2_.m_angularVelocity);
var _loc5_:Number = -param1.inv_dt * m_mass * _loc4_;
m_force += _loc5_;
var _loc3_:Number = param1.dt * _loc5_;
_loc6_.m_linearVelocity.x += _loc6_.m_invMass * _loc3_ * m_J.linear1.x;
_loc6_.m_linearVelocity.y += _loc6_.m_invMass * _loc3_ * m_J.linear1.y;
_loc6_.m_angularVelocity += _loc6_.m_invI * _loc3_ * m_J.angular1;
_loc2_.m_linearVelocity.x += _loc2_.m_invMass * _loc3_ * m_J.linear2.x;
_loc2_.m_linearVelocity.y += _loc2_.m_invMass * _loc3_ * m_J.linear2.y;
_loc2_.m_angularVelocity += _loc2_.m_invI * _loc3_ * m_J.angular2;
}
override public function SolvePositionConstraints() : Boolean
{
var _loc5_:Number = NaN;
var _loc4_:Number = NaN;
var _loc2_:Number = 0;
var _loc7_:b2Body = m_body1;
var _loc1_:b2Body = m_body2;
if(m_revolute1)
{
_loc5_ = m_revolute1.GetJointAngle();
}
else
{
_loc5_ = m_prismatic1.GetJointTranslation();
}
if(m_revolute2)
{
_loc4_ = m_revolute2.GetJointAngle();
}
else
{
_loc4_ = m_prismatic2.GetJointTranslation();
}
var _loc3_:Number = m_constant - (_loc5_ + m_ratio * _loc4_);
var _loc6_:Number = -m_mass * _loc3_;
_loc7_.m_sweep.c.x += _loc7_.m_invMass * _loc6_ * m_J.linear1.x;
_loc7_.m_sweep.c.y += _loc7_.m_invMass * _loc6_ * m_J.linear1.y;
_loc7_.m_sweep.a += _loc7_.m_invI * _loc6_ * m_J.angular1;
_loc1_.m_sweep.c.x += _loc1_.m_invMass * _loc6_ * m_J.linear2.x;
_loc1_.m_sweep.c.y += _loc1_.m_invMass * _loc6_ * m_J.linear2.y;
_loc1_.m_sweep.a += _loc1_.m_invI * _loc6_ * m_J.angular2;
_loc7_.SynchronizeTransform();
_loc1_.SynchronizeTransform();
return _loc2_ < 0.005;
}
}
}
@@ -0,0 +1,22 @@
package Box2D.Dynamics.Joints
{
public class b2GearJointDef extends b2JointDef
{
public var joint1:b2Joint;
public var joint2:b2Joint;
public var ratio:Number;
public function b2GearJointDef()
{
super();
type = 6;
joint1 = null;
joint2 = null;
ratio = 1;
}
}
}
@@ -0,0 +1,43 @@
package Box2D.Dynamics.Joints
{
import Box2D.Common.Math.b2Vec2;
public class b2Jacobian
{
public var linear1:b2Vec2 = new b2Vec2();
public var angular1:Number;
public var linear2:b2Vec2 = new b2Vec2();
public var angular2:Number;
public function b2Jacobian()
{
super();
}
public function SetZero() : void
{
linear1.SetZero();
angular1 = 0;
linear2.SetZero();
angular2 = 0;
}
public function Set(param1:b2Vec2, param2:Number, param3:b2Vec2, param4:Number) : void
{
linear1.SetV(param1);
angular1 = param2;
linear2.SetV(param3);
angular2 = param4;
}
public function Compute(param1:b2Vec2, param2:Number, param3:b2Vec2, param4:Number) : Number
{
return linear1.x * param1.x + linear1.y * param1.y + angular1 * param2 + (linear2.x * param3.x + linear2.y * param3.y) + angular2 * param4;
}
}
}
@@ -0,0 +1,165 @@
package Box2D.Dynamics.Joints
{
import Box2D.Common.Math.b2Vec2;
import Box2D.Dynamics.b2Body;
import Box2D.Dynamics.b2TimeStep;
public class b2Joint
{
public static const e_unknownJoint:int = 0;
public static const e_revoluteJoint:int = 1;
public static const e_prismaticJoint:int = 2;
public static const e_distanceJoint:int = 3;
public static const e_pulleyJoint:int = 4;
public static const e_mouseJoint:int = 5;
public static const e_gearJoint:int = 6;
public static const e_inactiveLimit:int = 0;
public static const e_atLowerLimit:int = 1;
public static const e_atUpperLimit:int = 2;
public static const e_equalLimits:int = 3;
public var m_type:int;
public var m_prev:b2Joint;
public var m_next:b2Joint;
public var m_node1:b2JointEdge = new b2JointEdge();
public var m_node2:b2JointEdge = new b2JointEdge();
public var m_body1:b2Body;
public var m_body2:b2Body;
public var m_inv_dt:Number;
public var m_islandFlag:Boolean;
public var m_collideConnected:Boolean;
public var m_userData:*;
public function b2Joint(param1:b2JointDef)
{
super();
m_type = param1.type;
m_prev = null;
m_next = null;
m_body1 = param1.body1;
m_body2 = param1.body2;
m_collideConnected = param1.collideConnected;
m_islandFlag = false;
m_userData = param1.userData;
}
public static function Create(param1:b2JointDef, param2:*) : b2Joint
{
var _loc3_:b2Joint = null;
switch(param1.type - 1)
{
case 0:
_loc3_ = new b2RevoluteJoint(param1 as b2RevoluteJointDef);
break;
case 1:
_loc3_ = new b2PrismaticJoint(param1 as b2PrismaticJointDef);
break;
case 2:
_loc3_ = new b2DistanceJoint(param1 as b2DistanceJointDef);
break;
case 3:
_loc3_ = new b2PulleyJoint(param1 as b2PulleyJointDef);
break;
case 4:
_loc3_ = new b2MouseJoint(param1 as b2MouseJointDef);
break;
case 5:
_loc3_ = new b2GearJoint(param1 as b2GearJointDef);
}
return _loc3_;
}
public static function Destroy(param1:b2Joint, param2:*) : void
{
}
public function GetType() : int
{
return m_type;
}
public function GetAnchor1() : b2Vec2
{
return null;
}
public function GetAnchor2() : b2Vec2
{
return null;
}
public function GetReactionForce() : b2Vec2
{
return null;
}
public function GetReactionTorque() : Number
{
return 0;
}
public function GetBody1() : b2Body
{
return m_body1;
}
public function GetBody2() : b2Body
{
return m_body2;
}
public function GetNext() : b2Joint
{
return m_next;
}
public function GetUserData() : *
{
return m_userData;
}
public function SetUserData(param1:*) : void
{
m_userData = param1;
}
public function InitVelocityConstraints(param1:b2TimeStep) : void
{
}
public function SolveVelocityConstraints(param1:b2TimeStep) : void
{
}
public function InitPositionConstraints() : void
{
}
public function SolvePositionConstraints() : Boolean
{
return false;
}
}
}
@@ -0,0 +1,29 @@
package Box2D.Dynamics.Joints
{
import Box2D.Dynamics.b2Body;
public class b2JointDef
{
public var type:int;
public var userData:*;
public var body1:b2Body;
public var body2:b2Body;
public var collideConnected:Boolean;
public function b2JointDef()
{
super();
type = 0;
userData = null;
body1 = null;
body2 = null;
collideConnected = false;
}
}
}
@@ -0,0 +1,22 @@
package Box2D.Dynamics.Joints
{
import Box2D.Dynamics.b2Body;
public class b2JointEdge
{
public var other:b2Body;
public var joint:b2Joint;
public var prev:b2JointEdge;
public var next:b2JointEdge;
public function b2JointEdge()
{
super();
}
}
}
@@ -0,0 +1,159 @@
package Box2D.Dynamics.Joints
{
import Box2D.Common.Math.b2Mat22;
import Box2D.Common.Math.b2Vec2;
import Box2D.Dynamics.b2Body;
import Box2D.Dynamics.b2TimeStep;
public class b2MouseJoint extends b2Joint
{
private var K:b2Mat22 = new b2Mat22();
private var K1:b2Mat22 = new b2Mat22();
private var K2:b2Mat22 = new b2Mat22();
public var m_localAnchor:b2Vec2 = new b2Vec2();
public var m_target:b2Vec2 = new b2Vec2();
public var m_impulse:b2Vec2 = new b2Vec2();
public var m_mass:b2Mat22 = new b2Mat22();
public var m_C:b2Vec2 = new b2Vec2();
public var m_maxForce:Number;
public var m_beta:Number;
public var m_gamma:Number;
public function b2MouseJoint(param1:b2MouseJointDef)
{
super(param1);
m_target.SetV(param1.target);
var _loc2_:Number = m_target.x - m_body2.m_xf.position.x;
var _loc4_:Number = m_target.y - m_body2.m_xf.position.y;
var _loc6_:b2Mat22 = m_body2.m_xf.R;
m_localAnchor.x = _loc2_ * _loc6_.col1.x + _loc4_ * _loc6_.col1.y;
m_localAnchor.y = _loc2_ * _loc6_.col2.x + _loc4_ * _loc6_.col2.y;
m_maxForce = param1.maxForce;
m_impulse.SetZero();
var _loc5_:Number = m_body2.m_mass;
var _loc8_:Number = 2 * 3.141592653589793 * param1.frequencyHz;
var _loc3_:Number = 2 * _loc5_ * param1.dampingRatio * _loc8_;
var _loc7_:Number = param1.timeStep * _loc5_ * (_loc8_ * _loc8_);
m_gamma = 1 / (_loc3_ + _loc7_);
m_beta = _loc7_ / (_loc3_ + _loc7_);
}
override public function GetAnchor1() : b2Vec2
{
return m_target;
}
override public function GetAnchor2() : b2Vec2
{
return m_body2.GetWorldPoint(m_localAnchor);
}
override public function GetReactionForce() : b2Vec2
{
return m_impulse;
}
override public function GetReactionTorque() : Number
{
return 0;
}
public function SetTarget(param1:b2Vec2) : void
{
if(m_body2.IsSleeping())
{
m_body2.WakeUp();
}
m_target = param1;
}
override public function InitVelocityConstraints(param1:b2TimeStep) : void
{
var _loc8_:b2Mat22 = null;
var _loc2_:b2Body = m_body2;
_loc8_ = _loc2_.m_xf.R;
var _loc5_:Number = m_localAnchor.x - _loc2_.m_sweep.localCenter.x;
var _loc6_:Number = m_localAnchor.y - _loc2_.m_sweep.localCenter.y;
var _loc4_:Number = _loc8_.col1.x * _loc5_ + _loc8_.col2.x * _loc6_;
_loc6_ = _loc8_.col1.y * _loc5_ + _loc8_.col2.y * _loc6_;
_loc5_ = _loc4_;
var _loc3_:Number = _loc2_.m_invMass;
var _loc9_:Number = _loc2_.m_invI;
K1.col1.x = _loc3_;
K1.col2.x = 0;
K1.col1.y = 0;
K1.col2.y = _loc3_;
K2.col1.x = _loc9_ * _loc6_ * _loc6_;
K2.col2.x = -_loc9_ * _loc5_ * _loc6_;
K2.col1.y = -_loc9_ * _loc5_ * _loc6_;
K2.col2.y = _loc9_ * _loc5_ * _loc5_;
K.SetM(K1);
K.AddM(K2);
K.col1.x += m_gamma;
K.col2.y += m_gamma;
K.Invert(m_mass);
m_C.x = _loc2_.m_sweep.c.x + _loc5_ - m_target.x;
m_C.y = _loc2_.m_sweep.c.y + _loc6_ - m_target.y;
_loc2_.m_angularVelocity *= 0.98;
var _loc7_:Number = param1.dt * m_impulse.x;
var _loc10_:Number = param1.dt * m_impulse.y;
_loc2_.m_linearVelocity.x += _loc3_ * _loc7_;
_loc2_.m_linearVelocity.y += _loc3_ * _loc10_;
_loc2_.m_angularVelocity += _loc9_ * (_loc5_ * _loc10_ - _loc6_ * _loc7_);
}
override public function SolveVelocityConstraints(param1:b2TimeStep) : void
{
var _loc16_:b2Mat22 = null;
var _loc3_:Number = NaN;
var _loc4_:Number = NaN;
var _loc2_:b2Body = m_body2;
_loc16_ = _loc2_.m_xf.R;
var _loc5_:Number = m_localAnchor.x - _loc2_.m_sweep.localCenter.x;
var _loc6_:Number = m_localAnchor.y - _loc2_.m_sweep.localCenter.y;
_loc3_ = _loc16_.col1.x * _loc5_ + _loc16_.col2.x * _loc6_;
_loc6_ = _loc16_.col1.y * _loc5_ + _loc16_.col2.y * _loc6_;
_loc5_ = _loc3_;
var _loc12_:Number = _loc2_.m_linearVelocity.x + -_loc2_.m_angularVelocity * _loc6_;
var _loc11_:Number = _loc2_.m_linearVelocity.y + _loc2_.m_angularVelocity * _loc5_;
_loc16_ = m_mass;
_loc3_ = _loc12_ + m_beta * param1.inv_dt * m_C.x + m_gamma * param1.dt * m_impulse.x;
_loc4_ = _loc11_ + m_beta * param1.inv_dt * m_C.y + m_gamma * param1.dt * m_impulse.y;
var _loc14_:Number = -param1.inv_dt * (_loc16_.col1.x * _loc3_ + _loc16_.col2.x * _loc4_);
var _loc13_:Number = -param1.inv_dt * (_loc16_.col1.y * _loc3_ + _loc16_.col2.y * _loc4_);
var _loc9_:Number = m_impulse.x;
var _loc10_:Number = m_impulse.y;
m_impulse.x += _loc14_;
m_impulse.y += _loc13_;
var _loc15_:Number = m_impulse.Length();
if(_loc15_ > m_maxForce)
{
m_impulse.Multiply(m_maxForce / _loc15_);
}
_loc14_ = m_impulse.x - _loc9_;
_loc13_ = m_impulse.y - _loc10_;
var _loc7_:Number = param1.dt * _loc14_;
var _loc8_:Number = param1.dt * _loc13_;
_loc2_.m_linearVelocity.x += _loc2_.m_invMass * _loc7_;
_loc2_.m_linearVelocity.y += _loc2_.m_invMass * _loc8_;
_loc2_.m_angularVelocity += _loc2_.m_invI * (_loc5_ * _loc8_ - _loc6_ * _loc7_);
}
override public function SolvePositionConstraints() : Boolean
{
return true;
}
}
}
@@ -0,0 +1,29 @@
package Box2D.Dynamics.Joints
{
import Box2D.Common.Math.b2Vec2;
public class b2MouseJointDef extends b2JointDef
{
public var target:b2Vec2 = new b2Vec2();
public var maxForce:Number;
public var frequencyHz:Number;
public var dampingRatio:Number;
public var timeStep:Number;
public function b2MouseJointDef()
{
super();
type = 5;
maxForce = 0;
frequencyHz = 5;
dampingRatio = 0.7;
timeStep = 0.016666666666666666;
}
}
}
@@ -0,0 +1,534 @@
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 b2PrismaticJoint extends b2Joint
{
public var m_localAnchor1:b2Vec2 = new b2Vec2();
public var m_localAnchor2:b2Vec2 = new b2Vec2();
public var m_localXAxis1:b2Vec2 = new b2Vec2();
public var m_localYAxis1:b2Vec2 = new b2Vec2();
public var m_refAngle:Number;
public var m_linearJacobian:b2Jacobian = new b2Jacobian();
public var m_linearMass:Number;
public var m_force:Number;
public var m_angularMass:Number;
public var m_torque:Number;
public var m_motorJacobian:b2Jacobian = new b2Jacobian();
public var m_motorMass:Number;
public var m_motorForce:Number;
public var m_limitForce:Number;
public var m_limitPositionImpulse:Number;
public var m_lowerTranslation:Number;
public var m_upperTranslation:Number;
public var m_maxMotorForce:Number;
public var m_motorSpeed:Number;
public var m_enableLimit:Boolean;
public var m_enableMotor:Boolean;
public var m_limitState:int;
public function b2PrismaticJoint(param1:b2PrismaticJointDef)
{
var _loc4_:* = null;
var _loc2_:Number = NaN;
var _loc3_:Number = NaN;
super(param1);
m_localAnchor1.SetV(param1.localAnchor1);
m_localAnchor2.SetV(param1.localAnchor2);
m_localXAxis1.SetV(param1.localAxis1);
m_localYAxis1.x = -m_localXAxis1.y;
m_localYAxis1.y = m_localXAxis1.x;
m_refAngle = param1.referenceAngle;
m_linearJacobian.SetZero();
m_linearMass = 0;
m_force = 0;
m_angularMass = 0;
m_torque = 0;
m_motorJacobian.SetZero();
m_motorMass = 0;
m_motorForce = 0;
m_limitForce = 0;
m_limitPositionImpulse = 0;
m_lowerTranslation = param1.lowerTranslation;
m_upperTranslation = param1.upperTranslation;
m_maxMotorForce = param1.maxMotorForce;
m_motorSpeed = param1.motorSpeed;
m_enableLimit = param1.enableLimit;
m_enableMotor = param1.enableMotor;
}
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 _loc4_:b2Mat22 = m_body1.m_xf.R;
var _loc3_:Number = m_limitForce * (_loc4_.col1.x * m_localXAxis1.x + _loc4_.col2.x * m_localXAxis1.y);
var _loc1_:Number = m_limitForce * (_loc4_.col1.y * m_localXAxis1.x + _loc4_.col2.y * m_localXAxis1.y);
var _loc2_:Number = m_force * (_loc4_.col1.x * m_localYAxis1.x + _loc4_.col2.x * m_localYAxis1.y);
var _loc5_:Number = m_force * (_loc4_.col1.y * m_localYAxis1.x + _loc4_.col2.y * m_localYAxis1.y);
return new b2Vec2(m_limitForce * _loc3_ + m_force * _loc2_,m_limitForce * _loc1_ + m_force * _loc5_);
}
override public function GetReactionTorque() : Number
{
return m_torque;
}
public function GetJointTranslation() : Number
{
var _loc7_:* = null;
var _loc9_:b2Body = m_body1;
var _loc1_:b2Body = m_body2;
var _loc2_:b2Vec2 = _loc9_.GetWorldPoint(m_localAnchor1);
var _loc3_:b2Vec2 = _loc1_.GetWorldPoint(m_localAnchor2);
var _loc4_:Number = _loc3_.x - _loc2_.x;
var _loc5_:Number = _loc3_.y - _loc2_.y;
var _loc8_:b2Vec2 = _loc9_.GetWorldVector(m_localXAxis1);
return _loc8_.x * _loc4_ + _loc8_.y * _loc5_;
}
public function GetJointSpeed() : Number
{
var _loc13_:b2Mat22 = null;
var _loc4_:b2Body = m_body1;
var _loc5_:b2Body = m_body2;
_loc13_ = _loc4_.m_xf.R;
var _loc11_:Number = m_localAnchor1.x - _loc4_.m_sweep.localCenter.x;
var _loc17_:Number = m_localAnchor1.y - _loc4_.m_sweep.localCenter.y;
var _loc1_:Number = _loc13_.col1.x * _loc11_ + _loc13_.col2.x * _loc17_;
_loc17_ = _loc13_.col1.y * _loc11_ + _loc13_.col2.y * _loc17_;
_loc11_ = _loc1_;
_loc13_ = _loc5_.m_xf.R;
var _loc14_:Number = m_localAnchor2.x - _loc5_.m_sweep.localCenter.x;
var _loc12_:Number = m_localAnchor2.y - _loc5_.m_sweep.localCenter.y;
_loc1_ = _loc13_.col1.x * _loc14_ + _loc13_.col2.x * _loc12_;
_loc12_ = _loc13_.col1.y * _loc14_ + _loc13_.col2.y * _loc12_;
_loc14_ = _loc1_;
var _loc8_:Number = _loc4_.m_sweep.c.x + _loc11_;
var _loc15_:Number = _loc4_.m_sweep.c.y + _loc17_;
var _loc10_:Number = _loc5_.m_sweep.c.x + _loc14_;
var _loc9_:Number = _loc5_.m_sweep.c.y + _loc12_;
var _loc6_:Number = _loc10_ - _loc8_;
var _loc7_:Number = _loc9_ - _loc15_;
var _loc2_:b2Vec2 = _loc4_.GetWorldVector(m_localXAxis1);
var _loc18_:b2Vec2 = _loc4_.m_linearVelocity;
var _loc20_:b2Vec2 = _loc5_.m_linearVelocity;
var _loc16_:Number = _loc4_.m_angularVelocity;
var _loc19_:Number = _loc5_.m_angularVelocity;
return _loc6_ * (-_loc16_ * _loc2_.y) + _loc7_ * (_loc16_ * _loc2_.x) + (_loc2_.x * (_loc20_.x + -_loc19_ * _loc12_ - _loc18_.x - -_loc16_ * _loc17_) + _loc2_.y * (_loc20_.y + _loc19_ * _loc14_ - _loc18_.y - _loc16_ * _loc11_));
}
public function IsLimitEnabled() : Boolean
{
return m_enableLimit;
}
public function EnableLimit(param1:Boolean) : void
{
m_enableLimit = param1;
}
public function GetLowerLimit() : Number
{
return m_lowerTranslation;
}
public function GetUpperLimit() : Number
{
return m_upperTranslation;
}
public function SetLimits(param1:Number, param2:Number) : void
{
m_lowerTranslation = param1;
m_upperTranslation = param2;
}
public function IsMotorEnabled() : Boolean
{
return m_enableMotor;
}
public function EnableMotor(param1:Boolean) : void
{
m_enableMotor = param1;
}
public function SetMotorSpeed(param1:Number) : void
{
m_motorSpeed = param1;
}
public function GetMotorSpeed() : Number
{
return m_motorSpeed;
}
public function SetMaxMotorForce(param1:Number) : void
{
m_maxMotorForce = param1;
}
public function GetMotorForce() : Number
{
return m_motorForce;
}
override public function InitVelocityConstraints(param1:b2TimeStep) : void
{
var _loc27_:b2Mat22 = null;
var _loc17_:Number = NaN;
var _loc11_:Number = NaN;
var _loc9_:Number = NaN;
var _loc21_:Number = NaN;
var _loc22_:Number = NaN;
var _loc26_:Number = NaN;
var _loc13_:Number = NaN;
var _loc16_:Number = NaN;
var _loc24_:Number = NaN;
var _loc23_:Number = NaN;
var _loc18_:Number = NaN;
var _loc19_:Number = NaN;
var _loc6_:b2Body = m_body1;
var _loc7_:b2Body = m_body2;
_loc27_ = _loc6_.m_xf.R;
var _loc25_:Number = m_localAnchor1.x - _loc6_.m_sweep.localCenter.x;
var _loc28_:Number = m_localAnchor1.y - _loc6_.m_sweep.localCenter.y;
_loc17_ = _loc27_.col1.x * _loc25_ + _loc27_.col2.x * _loc28_;
_loc28_ = _loc27_.col1.y * _loc25_ + _loc27_.col2.y * _loc28_;
_loc25_ = _loc17_;
_loc27_ = _loc7_.m_xf.R;
var _loc15_:Number = m_localAnchor2.x - _loc7_.m_sweep.localCenter.x;
var _loc14_:Number = m_localAnchor2.y - _loc7_.m_sweep.localCenter.y;
_loc17_ = _loc27_.col1.x * _loc15_ + _loc27_.col2.x * _loc14_;
_loc14_ = _loc27_.col1.y * _loc15_ + _loc27_.col2.y * _loc14_;
_loc15_ = _loc17_;
var _loc5_:Number = _loc6_.m_invMass;
var _loc3_:Number = _loc7_.m_invMass;
var _loc4_:Number = _loc6_.m_invI;
var _loc2_:Number = _loc7_.m_invI;
_loc27_ = _loc6_.m_xf.R;
var _loc8_:Number = _loc27_.col1.x * m_localYAxis1.x + _loc27_.col2.x * m_localYAxis1.y;
var _loc20_:Number = _loc27_.col1.y * m_localYAxis1.x + _loc27_.col2.y * m_localYAxis1.y;
var _loc10_:Number = _loc7_.m_sweep.c.x + _loc15_ - _loc6_.m_sweep.c.x;
var _loc12_:Number = _loc7_.m_sweep.c.y + _loc14_ - _loc6_.m_sweep.c.y;
m_linearJacobian.linear1.x = -_loc8_;
m_linearJacobian.linear1.y = -_loc20_;
m_linearJacobian.linear2.x = _loc8_;
m_linearJacobian.linear2.y = _loc20_;
m_linearJacobian.angular1 = -(_loc10_ * _loc20_ - _loc12_ * _loc8_);
m_linearJacobian.angular2 = _loc15_ * _loc20_ - _loc14_ * _loc8_;
m_linearMass = _loc5_ + _loc4_ * m_linearJacobian.angular1 * m_linearJacobian.angular1 + _loc3_ + _loc2_ * m_linearJacobian.angular2 * m_linearJacobian.angular2;
m_linearMass = 1 / m_linearMass;
m_angularMass = _loc4_ + _loc2_;
if(m_angularMass > Number.MIN_VALUE)
{
m_angularMass = 1 / m_angularMass;
}
if(m_enableLimit || m_enableMotor)
{
_loc27_ = _loc6_.m_xf.R;
_loc11_ = _loc27_.col1.x * m_localXAxis1.x + _loc27_.col2.x * m_localXAxis1.y;
_loc9_ = _loc27_.col1.y * m_localXAxis1.x + _loc27_.col2.y * m_localXAxis1.y;
m_motorJacobian.linear1.x = -_loc11_;
m_motorJacobian.linear1.y = -_loc9_;
m_motorJacobian.linear2.x = _loc11_;
m_motorJacobian.linear2.y = _loc9_;
m_motorJacobian.angular1 = -(_loc10_ * _loc9_ - _loc12_ * _loc11_);
m_motorJacobian.angular2 = _loc15_ * _loc9_ - _loc14_ * _loc11_;
m_motorMass = _loc5_ + _loc4_ * m_motorJacobian.angular1 * m_motorJacobian.angular1 + _loc3_ + _loc2_ * m_motorJacobian.angular2 * m_motorJacobian.angular2;
m_motorMass = 1 / m_motorMass;
if(m_enableLimit)
{
_loc21_ = _loc10_ - _loc25_;
_loc22_ = _loc12_ - _loc28_;
_loc26_ = _loc11_ * _loc21_ + _loc9_ * _loc22_;
if(b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2 * 0.005)
{
m_limitState = 3;
}
else if(_loc26_ <= m_lowerTranslation)
{
if(m_limitState != 1)
{
m_limitForce = 0;
}
m_limitState = 1;
}
else if(_loc26_ >= m_upperTranslation)
{
if(m_limitState != 2)
{
m_limitForce = 0;
}
m_limitState = 2;
}
else
{
m_limitState = 0;
m_limitForce = 0;
}
}
}
if(m_enableMotor == false)
{
m_motorForce = 0;
}
if(m_enableLimit == false)
{
m_limitForce = 0;
}
if(param1.warmStarting)
{
_loc13_ = param1.dt * (m_force * m_linearJacobian.linear1.x + (m_motorForce + m_limitForce) * m_motorJacobian.linear1.x);
_loc16_ = param1.dt * (m_force * m_linearJacobian.linear1.y + (m_motorForce + m_limitForce) * m_motorJacobian.linear1.y);
_loc24_ = param1.dt * (m_force * m_linearJacobian.linear2.x + (m_motorForce + m_limitForce) * m_motorJacobian.linear2.x);
_loc23_ = param1.dt * (m_force * m_linearJacobian.linear2.y + (m_motorForce + m_limitForce) * m_motorJacobian.linear2.y);
_loc18_ = param1.dt * (m_force * m_linearJacobian.angular1 - m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular1);
_loc19_ = param1.dt * (m_force * m_linearJacobian.angular2 + m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular2);
_loc6_.m_linearVelocity.x += _loc5_ * _loc13_;
_loc6_.m_linearVelocity.y += _loc5_ * _loc16_;
_loc6_.m_angularVelocity += _loc4_ * _loc18_;
_loc7_.m_linearVelocity.x += _loc3_ * _loc24_;
_loc7_.m_linearVelocity.y += _loc3_ * _loc23_;
_loc7_.m_angularVelocity += _loc2_ * _loc19_;
}
else
{
m_force = 0;
m_torque = 0;
m_limitForce = 0;
m_motorForce = 0;
}
m_limitPositionImpulse = 0;
}
override public function SolveVelocityConstraints(param1:b2TimeStep) : void
{
var _loc14_:Number = NaN;
var _loc16_:Number = NaN;
var _loc15_:Number = NaN;
var _loc12_:Number = NaN;
var _loc13_:Number = NaN;
var _loc8_:Number = NaN;
var _loc9_:b2Body = m_body1;
var _loc10_:b2Body = m_body2;
var _loc5_:Number = _loc9_.m_invMass;
var _loc3_:Number = _loc10_.m_invMass;
var _loc4_:Number = _loc9_.m_invI;
var _loc2_:Number = _loc10_.m_invI;
var _loc18_:Number = m_linearJacobian.Compute(_loc9_.m_linearVelocity,_loc9_.m_angularVelocity,_loc10_.m_linearVelocity,_loc10_.m_angularVelocity);
var _loc17_:Number = -param1.inv_dt * m_linearMass * _loc18_;
m_force += _loc17_;
var _loc11_:Number = param1.dt * _loc17_;
_loc9_.m_linearVelocity.x += _loc5_ * _loc11_ * m_linearJacobian.linear1.x;
_loc9_.m_linearVelocity.y += _loc5_ * _loc11_ * m_linearJacobian.linear1.y;
_loc9_.m_angularVelocity += _loc4_ * _loc11_ * m_linearJacobian.angular1;
_loc10_.m_linearVelocity.x += _loc3_ * _loc11_ * m_linearJacobian.linear2.x;
_loc10_.m_linearVelocity.y += _loc3_ * _loc11_ * m_linearJacobian.linear2.y;
_loc10_.m_angularVelocity += _loc2_ * _loc11_ * m_linearJacobian.angular2;
var _loc19_:Number = _loc10_.m_angularVelocity - _loc9_.m_angularVelocity;
var _loc6_:Number = -param1.inv_dt * m_angularMass * _loc19_;
m_torque += _loc6_;
var _loc7_:Number = param1.dt * _loc6_;
_loc9_.m_angularVelocity -= _loc4_ * _loc7_;
_loc10_.m_angularVelocity += _loc2_ * _loc7_;
if(m_enableMotor && m_limitState != 3)
{
_loc16_ = m_motorJacobian.Compute(_loc9_.m_linearVelocity,_loc9_.m_angularVelocity,_loc10_.m_linearVelocity,_loc10_.m_angularVelocity) - m_motorSpeed;
_loc15_ = -param1.inv_dt * m_motorMass * _loc16_;
_loc12_ = m_motorForce;
m_motorForce = b2Math.b2Clamp(m_motorForce + _loc15_,-m_maxMotorForce,m_maxMotorForce);
_loc15_ = m_motorForce - _loc12_;
_loc11_ = param1.dt * _loc15_;
_loc9_.m_linearVelocity.x += _loc5_ * _loc11_ * m_motorJacobian.linear1.x;
_loc9_.m_linearVelocity.y += _loc5_ * _loc11_ * m_motorJacobian.linear1.y;
_loc9_.m_angularVelocity += _loc4_ * _loc11_ * m_motorJacobian.angular1;
_loc10_.m_linearVelocity.x += _loc3_ * _loc11_ * m_motorJacobian.linear2.x;
_loc10_.m_linearVelocity.y += _loc3_ * _loc11_ * m_motorJacobian.linear2.y;
_loc10_.m_angularVelocity += _loc2_ * _loc11_ * m_motorJacobian.angular2;
}
if(m_enableLimit && m_limitState != 0)
{
_loc13_ = m_motorJacobian.Compute(_loc9_.m_linearVelocity,_loc9_.m_angularVelocity,_loc10_.m_linearVelocity,_loc10_.m_angularVelocity);
_loc8_ = -param1.inv_dt * m_motorMass * _loc13_;
if(m_limitState == 3)
{
m_limitForce += _loc8_;
}
else if(m_limitState == 1)
{
_loc14_ = m_limitForce;
m_limitForce = b2Math.b2Max(m_limitForce + _loc8_,0);
_loc8_ = m_limitForce - _loc14_;
}
else if(m_limitState == 2)
{
_loc14_ = m_limitForce;
m_limitForce = b2Math.b2Min(m_limitForce + _loc8_,0);
_loc8_ = m_limitForce - _loc14_;
}
_loc11_ = param1.dt * _loc8_;
_loc9_.m_linearVelocity.x += _loc5_ * _loc11_ * m_motorJacobian.linear1.x;
_loc9_.m_linearVelocity.y += _loc5_ * _loc11_ * m_motorJacobian.linear1.y;
_loc9_.m_angularVelocity += _loc4_ * _loc11_ * m_motorJacobian.angular1;
_loc10_.m_linearVelocity.x += _loc3_ * _loc11_ * m_motorJacobian.linear2.x;
_loc10_.m_linearVelocity.y += _loc3_ * _loc11_ * m_motorJacobian.linear2.y;
_loc10_.m_angularVelocity += _loc2_ * _loc11_ * m_motorJacobian.angular2;
}
}
override public function SolvePositionConstraints() : Boolean
{
var _loc22_:Number = NaN;
var _loc13_:Number = NaN;
var _loc29_:b2Mat22 = null;
var _loc19_:Number = NaN;
var _loc12_:Number = NaN;
var _loc11_:Number = NaN;
var _loc28_:Number = NaN;
var _loc18_:Number = NaN;
var _loc8_:b2Body = m_body1;
var _loc9_:b2Body = m_body2;
var _loc5_:Number = _loc8_.m_invMass;
var _loc2_:Number = _loc9_.m_invMass;
var _loc3_:Number = _loc8_.m_invI;
var _loc1_:Number = _loc9_.m_invI;
_loc29_ = _loc8_.m_xf.R;
var _loc27_:Number = m_localAnchor1.x - _loc8_.m_sweep.localCenter.x;
var _loc31_:Number = m_localAnchor1.y - _loc8_.m_sweep.localCenter.y;
_loc19_ = _loc29_.col1.x * _loc27_ + _loc29_.col2.x * _loc31_;
_loc31_ = _loc29_.col1.y * _loc27_ + _loc29_.col2.y * _loc31_;
_loc27_ = _loc19_;
_loc29_ = _loc9_.m_xf.R;
var _loc17_:Number = m_localAnchor2.x - _loc9_.m_sweep.localCenter.x;
var _loc16_:Number = m_localAnchor2.y - _loc9_.m_sweep.localCenter.y;
_loc19_ = _loc29_.col1.x * _loc17_ + _loc29_.col2.x * _loc16_;
_loc16_ = _loc29_.col1.y * _loc17_ + _loc29_.col2.y * _loc16_;
_loc17_ = _loc19_;
var _loc26_:Number = _loc8_.m_sweep.c.x + _loc27_;
var _loc30_:Number = _loc8_.m_sweep.c.y + _loc31_;
var _loc15_:Number = _loc9_.m_sweep.c.x + _loc17_;
var _loc14_:Number = _loc9_.m_sweep.c.y + _loc16_;
var _loc23_:Number = _loc15_ - _loc26_;
var _loc24_:Number = _loc14_ - _loc30_;
_loc29_ = _loc8_.m_xf.R;
var _loc10_:Number = _loc29_.col1.x * m_localYAxis1.x + _loc29_.col2.x * m_localYAxis1.y;
var _loc21_:Number = _loc29_.col1.y * m_localYAxis1.x + _loc29_.col2.y * m_localYAxis1.y;
var _loc25_:Number = _loc10_ * _loc23_ + _loc21_ * _loc24_;
_loc25_ = b2Math.b2Clamp(_loc25_,-0.2,0.2);
var _loc7_:Number = -m_linearMass * _loc25_;
_loc8_.m_sweep.c.x += _loc5_ * _loc7_ * m_linearJacobian.linear1.x;
_loc8_.m_sweep.c.y += _loc5_ * _loc7_ * m_linearJacobian.linear1.y;
_loc8_.m_sweep.a += _loc3_ * _loc7_ * m_linearJacobian.angular1;
_loc9_.m_sweep.c.x += _loc2_ * _loc7_ * m_linearJacobian.linear2.x;
_loc9_.m_sweep.c.y += _loc2_ * _loc7_ * m_linearJacobian.linear2.y;
_loc9_.m_sweep.a += _loc1_ * _loc7_ * m_linearJacobian.angular2;
var _loc4_:Number = b2Math.b2Abs(_loc25_);
var _loc6_:Number = _loc9_.m_sweep.a - _loc8_.m_sweep.a - m_refAngle;
_loc6_ = b2Math.b2Clamp(_loc6_,-0.13962634015954636,0.13962634015954636);
var _loc32_:Number = -m_angularMass * _loc6_;
_loc8_.m_sweep.a -= _loc8_.m_invI * _loc32_;
_loc9_.m_sweep.a += _loc9_.m_invI * _loc32_;
_loc8_.SynchronizeTransform();
_loc9_.SynchronizeTransform();
var _loc20_:Number = b2Math.b2Abs(_loc6_);
if(m_enableLimit && m_limitState != 0)
{
_loc29_ = _loc8_.m_xf.R;
_loc27_ = m_localAnchor1.x - _loc8_.m_sweep.localCenter.x;
_loc31_ = m_localAnchor1.y - _loc8_.m_sweep.localCenter.y;
_loc19_ = _loc29_.col1.x * _loc27_ + _loc29_.col2.x * _loc31_;
_loc31_ = _loc29_.col1.y * _loc27_ + _loc29_.col2.y * _loc31_;
_loc27_ = _loc19_;
_loc29_ = _loc9_.m_xf.R;
_loc17_ = m_localAnchor2.x - _loc9_.m_sweep.localCenter.x;
_loc16_ = m_localAnchor2.y - _loc9_.m_sweep.localCenter.y;
_loc19_ = _loc29_.col1.x * _loc17_ + _loc29_.col2.x * _loc16_;
_loc16_ = _loc29_.col1.y * _loc17_ + _loc29_.col2.y * _loc16_;
_loc17_ = _loc19_;
_loc26_ = _loc8_.m_sweep.c.x + _loc27_;
_loc30_ = _loc8_.m_sweep.c.y + _loc31_;
_loc15_ = _loc9_.m_sweep.c.x + _loc17_;
_loc14_ = _loc9_.m_sweep.c.y + _loc16_;
_loc23_ = _loc15_ - _loc26_;
_loc24_ = _loc14_ - _loc30_;
_loc29_ = _loc8_.m_xf.R;
_loc12_ = _loc29_.col1.x * m_localXAxis1.x + _loc29_.col2.x * m_localXAxis1.y;
_loc11_ = _loc29_.col1.y * m_localXAxis1.x + _loc29_.col2.y * m_localXAxis1.y;
_loc28_ = _loc12_ * _loc23_ + _loc11_ * _loc24_;
_loc18_ = 0;
if(m_limitState == 3)
{
_loc22_ = b2Math.b2Clamp(_loc28_,-0.2,0.2);
_loc18_ = -m_motorMass * _loc22_;
_loc4_ = b2Math.b2Max(_loc4_,b2Math.b2Abs(_loc6_));
}
else if(m_limitState == 1)
{
_loc22_ = _loc28_ - m_lowerTranslation;
_loc4_ = b2Math.b2Max(_loc4_,-_loc22_);
_loc22_ = b2Math.b2Clamp(_loc22_ + 0.005,-0.2,0);
_loc18_ = -m_motorMass * _loc22_;
_loc13_ = m_limitPositionImpulse;
m_limitPositionImpulse = b2Math.b2Max(m_limitPositionImpulse + _loc18_,0);
_loc18_ = m_limitPositionImpulse - _loc13_;
}
else if(m_limitState == 2)
{
_loc22_ = _loc28_ - m_upperTranslation;
_loc4_ = b2Math.b2Max(_loc4_,_loc22_);
_loc22_ = b2Math.b2Clamp(_loc22_ - 0.005,0,0.2);
_loc18_ = -m_motorMass * _loc22_;
_loc13_ = m_limitPositionImpulse;
m_limitPositionImpulse = b2Math.b2Min(m_limitPositionImpulse + _loc18_,0);
_loc18_ = m_limitPositionImpulse - _loc13_;
}
_loc8_.m_sweep.c.x += _loc5_ * _loc18_ * m_motorJacobian.linear1.x;
_loc8_.m_sweep.c.y += _loc5_ * _loc18_ * m_motorJacobian.linear1.y;
_loc8_.m_sweep.a += _loc3_ * _loc18_ * m_motorJacobian.angular1;
_loc9_.m_sweep.c.x += _loc2_ * _loc18_ * m_motorJacobian.linear2.x;
_loc9_.m_sweep.c.y += _loc2_ * _loc18_ * m_motorJacobian.linear2.y;
_loc9_.m_sweep.a += _loc1_ * _loc18_ * m_motorJacobian.angular2;
_loc8_.SynchronizeTransform();
_loc9_.SynchronizeTransform();
}
return _loc4_ <= 0.005 && _loc20_ <= 0.03490658503988659;
}
}
}
@@ -0,0 +1,54 @@
package Box2D.Dynamics.Joints
{
import Box2D.Common.Math.b2Vec2;
import Box2D.Dynamics.b2Body;
public class b2PrismaticJointDef extends b2JointDef
{
public var localAnchor1:b2Vec2 = new b2Vec2();
public var localAnchor2:b2Vec2 = new b2Vec2();
public var localAxis1:b2Vec2 = new b2Vec2();
public var referenceAngle:Number;
public var enableLimit:Boolean;
public var lowerTranslation:Number;
public var upperTranslation:Number;
public var enableMotor:Boolean;
public var maxMotorForce:Number;
public var motorSpeed:Number;
public function b2PrismaticJointDef()
{
super();
type = 2;
localAxis1.Set(1,0);
referenceAngle = 0;
enableLimit = false;
lowerTranslation = 0;
upperTranslation = 0;
enableMotor = false;
maxMotorForce = 0;
motorSpeed = 0;
}
public function Initialize(param1:b2Body, param2:b2Body, param3:b2Vec2, param4:b2Vec2) : void
{
body1 = param1;
body2 = param2;
localAnchor1 = body1.GetLocalPoint(param3);
localAnchor2 = body2.GetLocalPoint(param3);
localAxis1 = body1.GetLocalVector(param4);
referenceAngle = body2.GetAngle() - body1.GetAngle();
}
}
}
@@ -0,0 +1,491 @@
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;
}
}
}
@@ -0,0 +1,64 @@
package Box2D.Dynamics.Joints
{
import Box2D.Common.Math.b2Vec2;
import Box2D.Dynamics.b2Body;
public class b2PulleyJointDef extends b2JointDef
{
public var groundAnchor1:b2Vec2 = new b2Vec2();
public var groundAnchor2:b2Vec2 = new b2Vec2();
public var localAnchor1:b2Vec2 = new b2Vec2();
public var localAnchor2:b2Vec2 = new b2Vec2();
public var length1:Number;
public var maxLength1:Number;
public var length2:Number;
public var maxLength2:Number;
public var ratio:Number;
public function b2PulleyJointDef()
{
super();
type = 4;
groundAnchor1.Set(-1,1);
groundAnchor2.Set(1,1);
localAnchor1.Set(-1,0);
localAnchor2.Set(1,0);
length1 = 0;
maxLength1 = 0;
length2 = 0;
maxLength2 = 0;
ratio = 1;
collideConnected = true;
}
public function Initialize(param1:b2Body, param2:b2Body, param3:b2Vec2, param4:b2Vec2, param5:b2Vec2, param6:b2Vec2, param7:Number) : void
{
body1 = param1;
body2 = param2;
groundAnchor1.SetV(param3);
groundAnchor2.SetV(param4);
localAnchor1 = body1.GetLocalPoint(param5);
localAnchor2 = body2.GetLocalPoint(param6);
var _loc9_:Number = param5.x - param3.x;
var _loc12_:Number = param5.y - param3.y;
length1 = Math.sqrt(_loc9_ * _loc9_ + _loc12_ * _loc12_);
var _loc11_:Number = param6.x - param4.x;
var _loc10_:Number = param6.y - param4.y;
length2 = Math.sqrt(_loc11_ * _loc11_ + _loc10_ * _loc10_);
ratio = param7;
var _loc8_:Number = length1 + ratio * length2;
maxLength1 = _loc8_ - ratio * 2;
maxLength2 = (_loc8_ - 2) / ratio;
}
}
}
@@ -0,0 +1,425 @@
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 b2RevoluteJoint extends b2Joint
{
public static var tImpulse:b2Vec2 = new b2Vec2();
private var K:b2Mat22 = new b2Mat22();
private var K1:b2Mat22 = new b2Mat22();
private var K2:b2Mat22 = new b2Mat22();
private var K3:b2Mat22 = new b2Mat22();
public var m_localAnchor1:b2Vec2 = new b2Vec2();
public var m_localAnchor2:b2Vec2 = new b2Vec2();
public var m_pivotForce:b2Vec2 = new b2Vec2();
public var m_motorForce:Number;
public var m_limitForce:Number;
public var m_limitPositionImpulse:Number;
public var m_pivotMass:b2Mat22 = new b2Mat22();
public var m_motorMass:Number;
public var m_enableMotor:Boolean;
public var m_maxMotorTorque:Number;
public var m_motorSpeed:Number;
public var m_enableLimit:Boolean;
public var m_referenceAngle:Number;
public var m_lowerAngle:Number;
public var m_upperAngle:Number;
public var m_limitState:int;
public function b2RevoluteJoint(param1:b2RevoluteJointDef)
{
super(param1);
m_localAnchor1.SetV(param1.localAnchor1);
m_localAnchor2.SetV(param1.localAnchor2);
m_referenceAngle = param1.referenceAngle;
m_pivotForce.Set(0,0);
m_motorForce = 0;
m_limitForce = 0;
m_limitPositionImpulse = 0;
m_lowerAngle = param1.lowerAngle;
m_upperAngle = param1.upperAngle;
m_maxMotorTorque = param1.maxMotorTorque;
m_motorSpeed = param1.motorSpeed;
m_enableLimit = param1.enableLimit;
m_enableMotor = param1.enableMotor;
}
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
{
return m_pivotForce;
}
override public function GetReactionTorque() : Number
{
return m_limitForce;
}
public function GetJointAngle() : Number
{
return m_body2.m_sweep.a - m_body1.m_sweep.a - m_referenceAngle;
}
public function GetJointSpeed() : Number
{
return m_body2.m_angularVelocity - m_body1.m_angularVelocity;
}
public function IsLimitEnabled() : Boolean
{
return m_enableLimit;
}
public function EnableLimit(param1:Boolean) : void
{
m_enableLimit = param1;
}
public function GetLowerLimit() : Number
{
return m_lowerAngle;
}
public function GetUpperLimit() : Number
{
return m_upperAngle;
}
public function SetLimits(param1:Number, param2:Number) : void
{
m_lowerAngle = param1;
m_upperAngle = param2;
}
public function IsMotorEnabled() : Boolean
{
return m_enableMotor;
}
public function EnableMotor(param1:Boolean) : void
{
m_enableMotor = param1;
}
public function SetMotorSpeed(param1:Number) : void
{
m_motorSpeed = param1;
}
public function GetMotorSpeed() : Number
{
return m_motorSpeed;
}
public function SetMaxMotorTorque(param1:Number) : void
{
m_maxMotorTorque = param1;
}
public function GetMotorTorque() : Number
{
return m_motorForce;
}
override public function InitVelocityConstraints(param1:b2TimeStep) : void
{
var _loc11_:b2Mat22 = null;
var _loc6_:Number = NaN;
var _loc14_:Number = NaN;
var _loc7_:b2Body = m_body1;
var _loc8_:b2Body = m_body2;
_loc11_ = _loc7_.m_xf.R;
var _loc9_:Number = m_localAnchor1.x - _loc7_.m_sweep.localCenter.x;
var _loc13_:Number = m_localAnchor1.y - _loc7_.m_sweep.localCenter.y;
_loc6_ = _loc11_.col1.x * _loc9_ + _loc11_.col2.x * _loc13_;
_loc13_ = _loc11_.col1.y * _loc9_ + _loc11_.col2.y * _loc13_;
_loc9_ = _loc6_;
_loc11_ = _loc8_.m_xf.R;
var _loc12_:Number = m_localAnchor2.x - _loc8_.m_sweep.localCenter.x;
var _loc10_:Number = m_localAnchor2.y - _loc8_.m_sweep.localCenter.y;
_loc6_ = _loc11_.col1.x * _loc12_ + _loc11_.col2.x * _loc10_;
_loc10_ = _loc11_.col1.y * _loc12_ + _loc11_.col2.y * _loc10_;
_loc12_ = _loc6_;
var _loc5_:Number = _loc7_.m_invMass;
var _loc3_:Number = _loc8_.m_invMass;
var _loc4_:Number = _loc7_.m_invI;
var _loc2_:Number = _loc8_.m_invI;
K1.col1.x = _loc5_ + _loc3_;
K1.col2.x = 0;
K1.col1.y = 0;
K1.col2.y = _loc5_ + _loc3_;
K2.col1.x = _loc4_ * _loc13_ * _loc13_;
K2.col2.x = -_loc4_ * _loc9_ * _loc13_;
K2.col1.y = -_loc4_ * _loc9_ * _loc13_;
K2.col2.y = _loc4_ * _loc9_ * _loc9_;
K3.col1.x = _loc2_ * _loc10_ * _loc10_;
K3.col2.x = -_loc2_ * _loc12_ * _loc10_;
K3.col1.y = -_loc2_ * _loc12_ * _loc10_;
K3.col2.y = _loc2_ * _loc12_ * _loc12_;
K.SetM(K1);
K.AddM(K2);
K.AddM(K3);
K.Invert(m_pivotMass);
m_motorMass = 1 / (_loc4_ + _loc2_);
if(m_enableMotor == false)
{
m_motorForce = 0;
}
if(m_enableLimit)
{
_loc14_ = _loc8_.m_sweep.a - _loc7_.m_sweep.a - m_referenceAngle;
if(b2Math.b2Abs(m_upperAngle - m_lowerAngle) < 2 * 0.03490658503988659)
{
m_limitState = 3;
}
else if(_loc14_ <= m_lowerAngle)
{
if(m_limitState != 1)
{
m_limitForce = 0;
}
m_limitState = 1;
}
else if(_loc14_ >= m_upperAngle)
{
if(m_limitState != 2)
{
m_limitForce = 0;
}
m_limitState = 2;
}
else
{
m_limitState = 0;
m_limitForce = 0;
}
}
else
{
m_limitForce = 0;
}
if(param1.warmStarting)
{
_loc7_.m_linearVelocity.x -= param1.dt * _loc5_ * m_pivotForce.x;
_loc7_.m_linearVelocity.y -= param1.dt * _loc5_ * m_pivotForce.y;
_loc7_.m_angularVelocity -= param1.dt * _loc4_ * (_loc9_ * m_pivotForce.y - _loc13_ * m_pivotForce.x + m_motorForce + m_limitForce);
_loc8_.m_linearVelocity.x += param1.dt * _loc3_ * m_pivotForce.x;
_loc8_.m_linearVelocity.y += param1.dt * _loc3_ * m_pivotForce.y;
_loc8_.m_angularVelocity += param1.dt * _loc2_ * (_loc12_ * m_pivotForce.y - _loc10_ * m_pivotForce.x + m_motorForce + m_limitForce);
}
else
{
m_pivotForce.SetZero();
m_motorForce = 0;
m_limitForce = 0;
}
m_limitPositionImpulse = 0;
}
override public function SolveVelocityConstraints(param1:b2TimeStep) : void
{
var _loc18_:b2Mat22 = null;
var _loc3_:Number = NaN;
var _loc11_:Number = NaN;
var _loc15_:Number = NaN;
var _loc13_:Number = NaN;
var _loc9_:Number = NaN;
var _loc10_:Number = NaN;
var _loc6_:Number = NaN;
var _loc7_:b2Body = m_body1;
var _loc8_:b2Body = m_body2;
_loc18_ = _loc7_.m_xf.R;
var _loc16_:Number = m_localAnchor1.x - _loc7_.m_sweep.localCenter.x;
var _loc20_:Number = m_localAnchor1.y - _loc7_.m_sweep.localCenter.y;
_loc3_ = _loc18_.col1.x * _loc16_ + _loc18_.col2.x * _loc20_;
_loc20_ = _loc18_.col1.y * _loc16_ + _loc18_.col2.y * _loc20_;
_loc16_ = _loc3_;
_loc18_ = _loc8_.m_xf.R;
var _loc19_:Number = m_localAnchor2.x - _loc8_.m_sweep.localCenter.x;
var _loc17_:Number = m_localAnchor2.y - _loc8_.m_sweep.localCenter.y;
_loc3_ = _loc18_.col1.x * _loc19_ + _loc18_.col2.x * _loc17_;
_loc17_ = _loc18_.col1.y * _loc19_ + _loc18_.col2.y * _loc17_;
_loc19_ = _loc3_;
var _loc12_:Number = _loc8_.m_linearVelocity.x + -_loc8_.m_angularVelocity * _loc17_ - _loc7_.m_linearVelocity.x - -_loc7_.m_angularVelocity * _loc20_;
var _loc14_:Number = _loc8_.m_linearVelocity.y + _loc8_.m_angularVelocity * _loc19_ - _loc7_.m_linearVelocity.y - _loc7_.m_angularVelocity * _loc16_;
var _loc21_:Number = -param1.inv_dt * (m_pivotMass.col1.x * _loc12_ + m_pivotMass.col2.x * _loc14_);
var _loc2_:Number = -param1.inv_dt * (m_pivotMass.col1.y * _loc12_ + m_pivotMass.col2.y * _loc14_);
m_pivotForce.x += _loc21_;
m_pivotForce.y += _loc2_;
var _loc4_:Number = param1.dt * _loc21_;
var _loc5_:Number = param1.dt * _loc2_;
_loc7_.m_linearVelocity.x -= _loc7_.m_invMass * _loc4_;
_loc7_.m_linearVelocity.y -= _loc7_.m_invMass * _loc5_;
_loc7_.m_angularVelocity -= _loc7_.m_invI * (_loc16_ * _loc5_ - _loc20_ * _loc4_);
_loc8_.m_linearVelocity.x += _loc8_.m_invMass * _loc4_;
_loc8_.m_linearVelocity.y += _loc8_.m_invMass * _loc5_;
_loc8_.m_angularVelocity += _loc8_.m_invI * (_loc19_ * _loc5_ - _loc17_ * _loc4_);
if(m_enableMotor && m_limitState != 3)
{
_loc15_ = _loc8_.m_angularVelocity - _loc7_.m_angularVelocity - m_motorSpeed;
_loc13_ = -param1.inv_dt * m_motorMass * _loc15_;
_loc9_ = m_motorForce;
m_motorForce = b2Math.b2Clamp(m_motorForce + _loc13_,-m_maxMotorTorque,m_maxMotorTorque);
_loc13_ = m_motorForce - _loc9_;
_loc7_.m_angularVelocity -= _loc7_.m_invI * param1.dt * _loc13_;
_loc8_.m_angularVelocity += _loc8_.m_invI * param1.dt * _loc13_;
}
if(m_enableLimit && m_limitState != 0)
{
_loc10_ = _loc8_.m_angularVelocity - _loc7_.m_angularVelocity;
_loc6_ = -param1.inv_dt * m_motorMass * _loc10_;
if(m_limitState == 3)
{
m_limitForce += _loc6_;
}
else if(m_limitState == 1)
{
_loc11_ = m_limitForce;
m_limitForce = b2Math.b2Max(m_limitForce + _loc6_,0);
_loc6_ = m_limitForce - _loc11_;
}
else if(m_limitState == 2)
{
_loc11_ = m_limitForce;
m_limitForce = b2Math.b2Min(m_limitForce + _loc6_,0);
_loc6_ = m_limitForce - _loc11_;
}
_loc7_.m_angularVelocity -= _loc7_.m_invI * param1.dt * _loc6_;
_loc8_.m_angularVelocity += _loc8_.m_invI * param1.dt * _loc6_;
}
}
override public function SolvePositionConstraints() : Boolean
{
var _loc10_:Number = NaN;
var _loc21_:Number = NaN;
var _loc24_:b2Mat22 = null;
var _loc15_:Number = NaN;
var _loc16_:Number = NaN;
var _loc8_:b2Body = m_body1;
var _loc9_:b2Body = m_body2;
var _loc3_:Number = 0;
_loc24_ = _loc8_.m_xf.R;
var _loc23_:Number = m_localAnchor1.x - _loc8_.m_sweep.localCenter.x;
var _loc26_:Number = m_localAnchor1.y - _loc8_.m_sweep.localCenter.y;
var _loc18_:Number = _loc24_.col1.x * _loc23_ + _loc24_.col2.x * _loc26_;
_loc26_ = _loc24_.col1.y * _loc23_ + _loc24_.col2.y * _loc26_;
_loc23_ = _loc18_;
_loc24_ = _loc9_.m_xf.R;
var _loc14_:Number = m_localAnchor2.x - _loc9_.m_sweep.localCenter.x;
var _loc13_:Number = m_localAnchor2.y - _loc9_.m_sweep.localCenter.y;
_loc18_ = _loc24_.col1.x * _loc14_ + _loc24_.col2.x * _loc13_;
_loc13_ = _loc24_.col1.y * _loc14_ + _loc24_.col2.y * _loc13_;
_loc14_ = _loc18_;
var _loc22_:Number = _loc8_.m_sweep.c.x + _loc23_;
var _loc25_:Number = _loc8_.m_sweep.c.y + _loc26_;
var _loc12_:Number = _loc9_.m_sweep.c.x + _loc14_;
var _loc11_:Number = _loc9_.m_sweep.c.y + _loc13_;
var _loc1_:Number = _loc12_ - _loc22_;
var _loc4_:Number = _loc11_ - _loc25_;
_loc3_ = Math.sqrt(_loc1_ * _loc1_ + _loc4_ * _loc4_);
var _loc7_:Number = _loc8_.m_invMass;
var _loc5_:Number = _loc9_.m_invMass;
var _loc6_:Number = _loc8_.m_invI;
var _loc2_:Number = _loc9_.m_invI;
K1.col1.x = _loc7_ + _loc5_;
K1.col2.x = 0;
K1.col1.y = 0;
K1.col2.y = _loc7_ + _loc5_;
K2.col1.x = _loc6_ * _loc26_ * _loc26_;
K2.col2.x = -_loc6_ * _loc23_ * _loc26_;
K2.col1.y = -_loc6_ * _loc23_ * _loc26_;
K2.col2.y = _loc6_ * _loc23_ * _loc23_;
K3.col1.x = _loc2_ * _loc13_ * _loc13_;
K3.col2.x = -_loc2_ * _loc14_ * _loc13_;
K3.col1.y = -_loc2_ * _loc14_ * _loc13_;
K3.col2.y = _loc2_ * _loc14_ * _loc14_;
K.SetM(K1);
K.AddM(K2);
K.AddM(K3);
K.Solve(tImpulse,-_loc1_,-_loc4_);
var _loc19_:Number = tImpulse.x;
var _loc17_:Number = tImpulse.y;
_loc8_.m_sweep.c.x -= _loc8_.m_invMass * _loc19_;
_loc8_.m_sweep.c.y -= _loc8_.m_invMass * _loc17_;
_loc8_.m_sweep.a -= _loc8_.m_invI * (_loc23_ * _loc17_ - _loc26_ * _loc19_);
_loc9_.m_sweep.c.x += _loc9_.m_invMass * _loc19_;
_loc9_.m_sweep.c.y += _loc9_.m_invMass * _loc17_;
_loc9_.m_sweep.a += _loc9_.m_invI * (_loc14_ * _loc17_ - _loc13_ * _loc19_);
_loc8_.SynchronizeTransform();
_loc9_.SynchronizeTransform();
var _loc20_:Number = 0;
if(m_enableLimit && m_limitState != 0)
{
_loc15_ = _loc9_.m_sweep.a - _loc8_.m_sweep.a - m_referenceAngle;
_loc16_ = 0;
if(m_limitState == 3)
{
_loc21_ = b2Math.b2Clamp(_loc15_,-0.13962634015954636,0.13962634015954636);
_loc16_ = -m_motorMass * _loc21_;
_loc20_ = b2Math.b2Abs(_loc21_);
}
else if(m_limitState == 1)
{
_loc21_ = _loc15_ - m_lowerAngle;
_loc20_ = b2Math.b2Max(0,-_loc21_);
_loc21_ = b2Math.b2Clamp(_loc21_ + 0.03490658503988659,-0.13962634015954636,0);
_loc16_ = -m_motorMass * _loc21_;
_loc10_ = m_limitPositionImpulse;
m_limitPositionImpulse = b2Math.b2Max(m_limitPositionImpulse + _loc16_,0);
_loc16_ = m_limitPositionImpulse - _loc10_;
}
else if(m_limitState == 2)
{
_loc21_ = _loc15_ - m_upperAngle;
_loc20_ = b2Math.b2Max(0,_loc21_);
_loc21_ = b2Math.b2Clamp(_loc21_ - 0.03490658503988659,0,0.13962634015954636);
_loc16_ = -m_motorMass * _loc21_;
_loc10_ = m_limitPositionImpulse;
m_limitPositionImpulse = b2Math.b2Min(m_limitPositionImpulse + _loc16_,0);
_loc16_ = m_limitPositionImpulse - _loc10_;
}
_loc8_.m_sweep.a -= _loc8_.m_invI * _loc16_;
_loc9_.m_sweep.a += _loc9_.m_invI * _loc16_;
_loc8_.SynchronizeTransform();
_loc9_.SynchronizeTransform();
}
return _loc3_ <= 0.005 && _loc20_ <= 0.03490658503988659;
}
}
}
@@ -0,0 +1,52 @@
package Box2D.Dynamics.Joints
{
import Box2D.Common.Math.b2Vec2;
import Box2D.Dynamics.b2Body;
public class b2RevoluteJointDef extends b2JointDef
{
public var localAnchor1:b2Vec2 = new b2Vec2();
public var localAnchor2:b2Vec2 = new b2Vec2();
public var referenceAngle:Number;
public var enableLimit:Boolean;
public var lowerAngle:Number;
public var upperAngle:Number;
public var enableMotor:Boolean;
public var motorSpeed:Number;
public var maxMotorTorque:Number;
public function b2RevoluteJointDef()
{
super();
type = 1;
localAnchor1.Set(0,0);
localAnchor2.Set(0,0);
referenceAngle = 0;
lowerAngle = 0;
upperAngle = 0;
maxMotorTorque = 0;
motorSpeed = 0;
enableLimit = false;
enableMotor = false;
}
public function Initialize(param1:b2Body, param2:b2Body, param3:b2Vec2) : void
{
body1 = param1;
body2 = param2;
localAnchor1 = body1.GetLocalPoint(param3);
localAnchor2 = body2.GetLocalPoint(param3);
referenceAngle = body2.GetAngle() - body1.GetAngle();
}
}
}