API tools faq
paste
Advertisement
Guest User

b2PrismaticJoint

a guest
Apr 13th, 2018
131
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
ActionScript 3 23.04 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
  3. *
  4. * This software is provided 'as-is', without any express or implied
  5. * warranty.  In no event will the authors be held liable for any damages
  6. * arising from the use of this software.
  7. * Permission is granted to anyone to use this software for any purpose,
  8. * including commercial applications, and to alter it and redistribute it
  9. * freely, subject to the following restrictions:
  10. * 1. The origin of this software must not be misrepresented; you must not
  11. * claim that you wrote the original software. If you use this software
  12. * in a product, an acknowledgment in the product documentation would be
  13. * appreciated but is not required.
  14. * 2. Altered source versions must be plainly marked as such, and must not be
  15. * misrepresented as being the original software.
  16. * 3. This notice may not be removed or altered from any source distribution.
  17. */
  18.  
  19. package Box2D.Dynamics.Joints
  20. {
  21.  
  22.    
  23.     import Box2D.Common.b2Pool;
  24.     import Box2D.Common.b2Settings;
  25.     import Box2D.Common.b2internal;
  26.     import Box2D.Common.Math.b2Mat22;
  27.     import Box2D.Common.Math.b2Mat33;
  28.     import Box2D.Common.Math.b2Math;
  29.     import Box2D.Common.Math.b2Transform;
  30.     import Box2D.Common.Math.b2Vec2;
  31.     import Box2D.Common.Math.b2Vec3;
  32.     import Box2D.Dynamics.b2Body;
  33.     import Box2D.Dynamics.b2TimeStep;
  34.    
  35.     use namespace b2internal;
  36.  
  37.    
  38.     /**
  39.     * A prismatic joint. This joint provides one degree of freedom: translation
  40.     * along an axis fixed in body1. Relative rotation is prevented. You can
  41.     * use a joint limit to restrict the range of motion and a joint motor to
  42.     * drive the motion or to model joint friction.
  43.     * @see b2PrismaticJointDef
  44.     */
  45.     public class b2PrismaticJoint extends b2Joint
  46.     {
  47.         /** @inheritDoc */
  48.         public override function GetAnchorA(result:b2Vec2):void
  49.         {
  50.             return m_bodyA.GetWorldPoint(m_localAnchor1, result);
  51.         }
  52.        
  53.         /** @inheritDoc */
  54.         public override function GetAnchorB(result:b2Vec2):void
  55.         {
  56.             return m_bodyB.GetWorldPoint(m_localAnchor2, result);
  57.         }
  58.         /** @inheritDoc */
  59.         public override function GetReactionForce(inv_dt:Number) : b2Vec2
  60.         {
  61.             //return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis);
  62.             return new b2Vec2(  inv_dt * (m_impulse.x * m_perp.x + (m_motorImpulse + m_impulse.z) * m_axis.x),
  63.                                 inv_dt * (m_impulse.x * m_perp.y + (m_motorImpulse + m_impulse.z) * m_axis.y));
  64.         }
  65.    
  66.         /** @inheritDoc */
  67.         public override function GetReactionTorque(inv_dt:Number) : Number
  68.         {
  69.             return inv_dt * m_impulse.y;
  70.         }
  71.        
  72.        
  73.        
  74.         private var p1:b2Vec2;         
  75.         private var p2:b2Vec2;
  76.         private var dX:Number;
  77.         private var dY:Number;
  78.         private var axis:b2Vec2;
  79.         private var translation:Number;
  80.        
  81.        
  82.         private var tMat:b2Mat22;
  83.        
  84.         private var r1X:Number;
  85.         private var r1Y:Number;
  86.         private var tX:Number;
  87.         private var r2X:Number;
  88.         private var r2Y:Number;
  89.        
  90.         private var p1X:Number;
  91.         private var p1Y:Number;
  92.        
  93.         private var p2X:Number;
  94.         private var p2Y:Number;    
  95.        
  96.         private var v1:b2Vec2;
  97.         private var v2:b2Vec2;
  98.         private var w1:Number;
  99.         private var w2:Number;
  100.        
  101.         private var speed:Number;
  102.        
  103.         private var xf1:b2Transform;
  104.         private var xf2:b2Transform;
  105.        
  106.         private var m1:Number;
  107.         private var m2:Number;
  108.         private var i1:Number;
  109.         private var i2:Number;
  110.        
  111.         private var jointTransition:Number;
  112.        
  113.         private var PX:Number;
  114.         private var PY:Number;
  115.         private var L1:Number;
  116.         private var L2:Number;
  117.        
  118.         private var Cdot:Number;
  119.         private var impulse:Number;
  120.         private var oldImpulse:Number;
  121.         private var maxImpulse:Number;
  122.        
  123.         private var Cdot1X:Number;
  124.         private var Cdot1Y:Number;
  125.        
  126.        
  127.         private var Cdot2:Number;
  128.         private var _f1:b2Vec3 = new b2Vec3(0, 0, 0);
  129.        
  130.         private var _df:b2Vec3 = new b2Vec3(0, 0, 0);
  131.        
  132.         private var bX:Number;
  133.         private var bY:Number;
  134.        
  135.         private var _f2r:b2Vec2 = new b2Vec2(0, 0);
  136.        
  137.         private var _df2:b2Vec2 = new b2Vec2(0, 0);
  138.        
  139.        
  140.         private var c1:b2Vec2;
  141.         private var a1:Number;
  142.        
  143.         private var c2:b2Vec2;
  144.         private var a2:Number;
  145.  
  146.         private var linearError:Number;
  147.         private var angularError:Number;
  148.         private var active:Boolean;
  149.         private var C2:Number;
  150.        
  151.         private var limitC:Number;
  152.         private var oldLimitImpulse:Number;
  153.        
  154.         private var R1:b2Mat22;
  155.         private var R2:b2Mat22;
  156.  
  157.         private var gravity:b2Vec2;
  158.        
  159.         private var impulse3:b2Vec3;  
  160.        
  161.         private var C1X:Number;
  162.         private var C1Y:Number;
  163.        
  164.        
  165.         private var k11:Number;
  166.         private var k12:Number;
  167.         private var k22:Number;
  168.         private var impulse1:b2Vec2;
  169.        
  170.        
  171.         /**
  172.         * Get the current joint translation, usually in meters.
  173.         */
  174.         [Inline]
  175.         final public function GetJointTranslation():Number
  176.         {
  177.             var bA:b2Body = m_bodyA;
  178.             var bB:b2Body = m_bodyB;
  179.  
  180.             p1 = b2Pool.getVector2(0, 0);
  181.            
  182.             bA.GetWorldPoint(m_localAnchor1, p1);
  183.            
  184.             p2 = b2Pool.getVector2(0, 0);
  185.            
  186.             bB.GetWorldPoint(m_localAnchor2, p2);
  187.  
  188.             dX = p2.x - p1.x;
  189.             dY = p2.y - p1.y;
  190.  
  191.             axis = b2Pool.getVector2(0, 0);
  192.            
  193.             bA.GetWorldVector(m_localXAxis1, axis);
  194.  
  195.             translation = axis.x*dX + axis.y*dY;
  196.            
  197.             b2Pool.putVector2(p1);
  198.             b2Pool.putVector2(p2);
  199.             b2Pool.putVector2(axis);
  200.            
  201.             return translation;
  202.         }
  203.        
  204.         /**
  205.         * Get the current joint translation speed, usually in meters per second.
  206.         */
  207.         [Inline]
  208.         final public function GetJointSpeed():Number
  209.         {
  210.             var bA:b2Body = m_bodyA;
  211.             var bB:b2Body = m_bodyB;
  212.  
  213.             tMat = bA.m_xf.R;
  214.            
  215.             r1X = m_localAnchor1.x - bA.m_sweep.localCenter.x;
  216.             r1Y = m_localAnchor1.y - bA.m_sweep.localCenter.y;
  217.             tX =  (tMat.col1.x * r1X + tMat.col2.x * r1Y);
  218.            
  219.             r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
  220.             r1X = tX;
  221.  
  222.             tMat = bB.m_xf.R;
  223.            
  224.             r2X = m_localAnchor2.x - bB.m_sweep.localCenter.x;
  225.             r2Y = m_localAnchor2.y - bB.m_sweep.localCenter.y;
  226.            
  227.             tX =  (tMat.col1.x * r2X + tMat.col2.x * r2Y);
  228.             r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
  229.             r2X = tX;
  230.  
  231.             p1X = bA.m_sweep.c.x + r1X;
  232.             p1Y = bA.m_sweep.c.y + r1Y;
  233.  
  234.             p2X = bB.m_sweep.c.x + r2X;
  235.             p2Y = bB.m_sweep.c.y + r2Y;
  236.  
  237.             dX = p2X - p1X;
  238.             dY = p2Y - p1Y;
  239.  
  240.             axis = b2Pool.getVector2(0, 0);
  241.            
  242.             bA.GetWorldVector(m_localXAxis1, axis);    
  243.    
  244.             v1 = bA.m_linearVelocity;
  245.             v2 = bB.m_linearVelocity;
  246.             w1 = bA.m_angularVelocity;
  247.             w2 = bB.m_angularVelocity;
  248.  
  249.             speed = (dX*(-w1 * axis.y) + dY*(w1 * axis.x)) + (axis.x * ((( v2.x + (-w2 * r2Y)) - v1.x) - (-w1 * r1Y)) + axis.y * ((( v2.y + (w2 * r2X)) - v1.y) - (w1 * r1X)));
  250.            
  251.             b2Pool.putVector2(axis);
  252.    
  253.             return speed;
  254.         }
  255.        
  256.         /**
  257.         * Is the joint limit enabled?
  258.         */
  259.         [Inline]
  260.         final public function IsLimitEnabled():Boolean
  261.         {
  262.             return m_enableLimit;
  263.         }
  264.  
  265.         /**
  266.         * Enable/disable the joint limit.
  267.         */
  268.         public function EnableLimit(flag:Boolean):void
  269.         {
  270.             m_bodyA.SetAwake(true);
  271.             m_bodyB.SetAwake(true);
  272.             m_enableLimit = flag;
  273.         }
  274.  
  275.         /**
  276.         * Get the lower joint limit, usually in meters.
  277.         */
  278.         [Inline]
  279.         final public function GetLowerLimit():Number
  280.         {
  281.             return m_lowerTranslation;
  282.         }
  283.  
  284.         /**
  285.         * Get the upper joint limit, usually in meters.
  286.         */
  287.         [Inline]
  288.         final public function GetUpperLimit():Number
  289.         {
  290.             return m_upperTranslation;
  291.         }
  292.        
  293.         /**
  294.         * Set the joint limits, usually in meters.
  295.         */
  296.         public function SetLimits(lower:Number, upper:Number) : void
  297.         {
  298.             //b2Settings.b2Assert(lower <= upper);
  299.             m_bodyA.SetAwake(true);
  300.             m_bodyB.SetAwake(true);
  301.             m_lowerTranslation = lower;
  302.             m_upperTranslation = upper;
  303.         }
  304.         /**
  305.         * Is the joint motor enabled?
  306.         */
  307.         [Inline]
  308.         final public function IsMotorEnabled():Boolean
  309.         {
  310.             return m_enableMotor;
  311.         }
  312.  
  313.         /**
  314.         * Enable/disable the joint motor.
  315.         */
  316.         public function EnableMotor(flag:Boolean) : void
  317.         {
  318.             m_bodyA.SetAwake(true);
  319.             m_bodyB.SetAwake(true);
  320.             m_enableMotor = flag;
  321.         }
  322.         /**
  323.         * Set the motor speed, usually in meters per second.
  324.         */
  325.         public function SetMotorSpeed(speed:Number) : void
  326.         {
  327.             m_bodyA.SetAwake(true);
  328.             m_bodyB.SetAwake(true);
  329.             m_motorSpeed = speed;
  330.         }
  331.         /**
  332.         * Get the motor speed, usually in meters per second.
  333.         */
  334.         [Inline]
  335.         final public function GetMotorSpeed():Number
  336.         {
  337.             return m_motorSpeed;
  338.         }
  339.        
  340.         /**
  341.         * Set the maximum motor force, usually in N.
  342.         */
  343.         [Inline]
  344.         final public function SetMaxMotorForce(force:Number):void
  345.         {
  346.             m_bodyA.SetAwake(true);
  347.             m_bodyB.SetAwake(true);
  348.             m_maxMotorForce = force;
  349.         }
  350.        
  351.         public function GetMaxMotorForce():Number
  352.         {
  353.             return m_maxMotorForce;
  354.         }
  355.        
  356.        
  357.         /**
  358.         * Get the current motor force, usually in N.
  359.         */
  360.         public function GetMotorForce():Number
  361.         {
  362.             return m_motorImpulse;
  363.         }
  364.        
  365.    
  366.         //--------------- Internals Below -------------------
  367.    
  368.         /** @private */
  369.         public function b2PrismaticJoint(def:b2PrismaticJointDef)
  370.         {
  371.             super(def);
  372.            
  373.             var tMat:b2Mat22;
  374.             var tX:Number;
  375.             var tY:Number;
  376.            
  377.             m_localAnchor1.SetV(def.localAnchorA);
  378.             m_localAnchor2.SetV(def.localAnchorB);
  379.             m_localXAxis1.SetV(def.localAxisA);
  380.            
  381.             //m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
  382.             m_localYAxis1.x = -m_localXAxis1.y;
  383.             m_localYAxis1.y = m_localXAxis1.x;
  384.            
  385.             m_refAngle = def.referenceAngle;
  386.            
  387.             m_impulse.SetZero();
  388.             m_motorMass = 0.0;
  389.             m_motorImpulse = 0.0;
  390.            
  391.             m_lowerTranslation = def.lowerTranslation;
  392.             m_upperTranslation = def.upperTranslation;
  393.             m_maxMotorForce = def.maxMotorForce;
  394.             m_motorSpeed = def.motorSpeed;
  395.             m_enableLimit = def.enableLimit;
  396.             m_enableMotor = def.enableMotor;
  397.             m_limitState = e_inactiveLimit;
  398.            
  399.             m_axis.SetZero();
  400.             m_perp.SetZero();
  401.         }
  402.    
  403.         [Inline]
  404.         final b2internal override function InitVelocityConstraints(step:b2TimeStep):void
  405.         {
  406.             var bA:b2Body = m_bodyA;
  407.             var bB:b2Body = m_bodyB;
  408.  
  409.             m_localCenterA.SetV(bA.GetLocalCenter());
  410.             m_localCenterB.SetV(bB.GetLocalCenter());
  411.    
  412.             xf1 = bA.GetTransform();
  413.             xf2 = bB.GetTransform();
  414.  
  415.             tMat = bA.m_xf.R;
  416.             r1X = m_localAnchor1.x - m_localCenterA.x;
  417.             r1Y = m_localAnchor1.y - m_localCenterA.y;
  418.             tX =  (tMat.col1.x * r1X + tMat.col2.x * r1Y);
  419.             r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
  420.             r1X = tX;
  421.  
  422.             tMat = bB.m_xf.R;
  423.             r2X = m_localAnchor2.x - m_localCenterB.x;
  424.             r2Y = m_localAnchor2.y - m_localCenterB.y;
  425.             tX =  (tMat.col1.x * r2X + tMat.col2.x * r2Y);
  426.             r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
  427.             r2X = tX;
  428.  
  429.             dX = bB.m_sweep.c.x + r2X - bA.m_sweep.c.x - r1X;
  430.             dY = bB.m_sweep.c.y + r2Y - bA.m_sweep.c.y - r1Y;
  431.    
  432.             m_invMassA = bA.m_invMass;
  433.             m_invMassB = bB.m_invMass;
  434.             m_invIA = bA.m_invI;
  435.             m_invIB = bB.m_invI;
  436.    
  437.             // Compute motor Jacobian and effective mass.
  438.             {
  439.                 b2Math.MulMV(xf1.R, m_localXAxis1, m_axis);
  440.    
  441.                 //m_a1 = b2Math.b2Cross(d + r1, m_axis);
  442.                 m_a1 = (dX + r1X) * m_axis.y - (dY + r1Y) * m_axis.x;
  443.                 //m_a2 = b2Math.b2Cross(r2, m_axis);
  444.                 m_a2 = r2X * m_axis.y - r2Y * m_axis.x;
  445.                
  446.                 m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
  447.                 if(m_motorMass > Number.MIN_VALUE)
  448.                     m_motorMass = 1.0 / m_motorMass;
  449.             }
  450.    
  451.             // Prismatic constraint.
  452.             {
  453.                 b2Math.MulMV(xf1.R, m_localYAxis1, m_perp);
  454.                 //m_s1 = b2Math.b2Cross(d + r1, m_perp);
  455.                 m_s1 = (dX + r1X) * m_perp.y - (dY + r1Y) * m_perp.x;
  456.                 //m_s2 = b2Math.b2Cross(r2, m_perp);
  457.                 m_s2 = r2X * m_perp.y - r2Y * m_perp.x;
  458.                
  459.                 m1 = m_invMassA;
  460.                 m2 = m_invMassB;
  461.                 i1 = m_invIA;
  462.                 i2 = m_invIB;
  463.                
  464.                 m_K.col1.x = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
  465.                 m_K.col1.y = i1 * m_s1 + i2 * m_s2;
  466.                 m_K.col1.z = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
  467.                 m_K.col2.x = m_K.col1.y;
  468.                 m_K.col2.y = i1 + i2;
  469.                 m_K.col2.z = i1 * m_a1 + i2 * m_a2;
  470.                 m_K.col3.x = m_K.col1.z;
  471.                 m_K.col3.y = m_K.col2.z;
  472.                 m_K.col3.z = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
  473.             }
  474.            
  475.             // Compute motor and limit terms
  476.             if (m_enableLimit)
  477.             {
  478.                 jointTransition = m_axis.x * dX + m_axis.y * dY;
  479.  
  480.                 if (b2Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0 * b2Settings.b2_linearSlop)
  481.                 {
  482.                     m_limitState = e_equalLimits;
  483.                 }
  484.                 else if (jointTransition <= m_lowerTranslation)
  485.                 {
  486.                     if (m_limitState != e_atLowerLimit)
  487.                     {
  488.                         m_limitState = e_atLowerLimit;
  489.                         m_impulse.z = 0.0;
  490.                     }
  491.                 }
  492.                 else if (jointTransition >= m_upperTranslation)
  493.                 {
  494.                     if (m_limitState != e_atUpperLimit)
  495.                     {
  496.                         m_limitState = e_atUpperLimit;
  497.                         m_impulse.z = 0.0;
  498.                     }
  499.                 }
  500.                 else
  501.                 {
  502.                     m_limitState = e_inactiveLimit;
  503.                     m_impulse.z = 0.0;
  504.                 }
  505.             }
  506.             else
  507.             {
  508.                 m_limitState = e_inactiveLimit;
  509.             }
  510.            
  511.             if (m_enableMotor === false)
  512.             {
  513.                 m_motorImpulse = 0.0;
  514.             }
  515.    
  516.             if (step.warmStarting)
  517.             {
  518.                 // Account for variable time step.
  519.                 m_impulse.x *= step.dtRatio;
  520.                 m_impulse.y *= step.dtRatio;
  521.                 m_motorImpulse *= step.dtRatio;
  522.  
  523.                 PX = m_impulse.x * m_perp.x + (m_motorImpulse + m_impulse.z) * m_axis.x;
  524.                 PY = m_impulse.x * m_perp.y + (m_motorImpulse + m_impulse.z) * m_axis.y;
  525.                 L1 = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
  526.                 L2 = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
  527.    
  528.                 //bA->m_linearVelocity -= m_invMassA * P;
  529.                 bA.m_linearVelocity.x -= m_invMassA * PX;
  530.                 bA.m_linearVelocity.y -= m_invMassA * PY;
  531.                 //bA->m_angularVelocity -= m_invIA * L1;
  532.                 bA.m_angularVelocity -= m_invIA * L1;
  533.    
  534.                 //bB->m_linearVelocity += m_invMassB * P;
  535.                 bB.m_linearVelocity.x += m_invMassB * PX;
  536.                 bB.m_linearVelocity.y += m_invMassB * PY;
  537.                 //bB->m_angularVelocity += m_invIB * L2;
  538.                 bB.m_angularVelocity += m_invIB * L2;
  539.             }
  540.             else
  541.             {
  542.                 m_impulse.SetZero();
  543.                 m_motorImpulse = 0.0;
  544.             }
  545.         }
  546.        
  547.         [Inline]
  548.         final b2internal override function SolveVelocityConstraints(step:b2TimeStep):void
  549.         {
  550.             v1 = m_bodyA.m_linearVelocity;
  551.             w1 = m_bodyA.m_angularVelocity;
  552.             v2 = m_bodyB.m_linearVelocity;
  553.             w2 = m_bodyB.m_angularVelocity;
  554.            
  555.             // Solve linear motor constraint
  556.             if (m_enableMotor && m_limitState !== e_equalLimits)
  557.             {
  558.                 //float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
  559.                 Cdot = m_axis.x * (v2.x -v1.x) + m_axis.y * (v2.y - v1.y) + m_a2 * w2 - m_a1 * w1;
  560.                 impulse = m_motorMass * (m_motorSpeed - Cdot);
  561.                 oldImpulse = m_motorImpulse;
  562.                 maxImpulse = step.dt * m_maxMotorForce;
  563.                
  564.                 m_motorImpulse = b2Math.Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
  565.                 impulse = m_motorImpulse - oldImpulse;
  566.                
  567.                 PX = impulse * m_axis.x;
  568.                 PY = impulse * m_axis.y;
  569.                 L1 = impulse * m_a1;
  570.                 L2 = impulse * m_a2;
  571.                
  572.                 v1.x -= m_invMassA * PX;
  573.                 v1.y -= m_invMassA * PY;
  574.                 w1 -= m_invIA * L1;
  575.                
  576.                 v2.x += m_invMassB * PX;
  577.                 v2.y += m_invMassB * PY;
  578.                 w2 += m_invIB * L2;
  579.             }
  580.            
  581.             //Cdot1.x = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
  582.             Cdot1X = m_perp.x * (v2.x - v1.x) + m_perp.y * (v2.y - v1.y) + m_s2 * w2 - m_s1 * w1;
  583.             Cdot1Y = w2 - w1;
  584.    
  585.             if (m_enableLimit && m_limitState !== e_inactiveLimit)
  586.             {
  587.                 Cdot2 = m_axis.x * (v2.x - v1.x) + m_axis.y * (v2.y - v1.y) + m_a2 * w2 - m_a1 * w1;
  588.                
  589.                 _f1.SetV(m_impulse);
  590.                    
  591.                 m_K.Solve33(_df, -Cdot1X, -Cdot1Y, -Cdot2);
  592.                
  593.                 m_impulse.Add(_df);
  594.                
  595.                 if (m_limitState === e_atLowerLimit)
  596.                 {
  597.                     m_impulse.z = b2Math.Max(m_impulse.z, 0.0);
  598.                 }
  599.                 else if (m_limitState === e_atUpperLimit)
  600.                 {
  601.                     m_impulse.z = b2Math.Min(m_impulse.z, 0.0);
  602.                 }
  603.  
  604.                 bX = -Cdot1X - (m_impulse.z - _f1.z) * m_K.col3.x;
  605.                 bY = -Cdot1Y - (m_impulse.z - _f1.z) * m_K.col3.y;
  606.                    
  607.                 m_K.Solve22(_f2r, bX, bY);
  608.                    
  609.                 _f2r.x += _f1.x;
  610.                 _f2r.y += _f1.y;
  611.                
  612.                 m_impulse.x = _f2r.x;
  613.                 m_impulse.y = _f2r.y;
  614.                
  615.                 _df.x = m_impulse.x - _f1.x;
  616.                 _df.y = m_impulse.y - _f1.y;
  617.                 _df.z = m_impulse.z - _f1.z;
  618.                
  619.                 PX = _df.x * m_perp.x + _df.z * m_axis.x;
  620.                 PY = _df.x * m_perp.y + _df.z * m_axis.y;
  621.                 L1 = _df.x * m_s1 + _df.y + _df.z * m_a1;
  622.                 L2 = _df.x * m_s2 + _df.y + _df.z * m_a2;
  623.                
  624.                 v1.x -= m_invMassA * PX;
  625.                 v1.y -= m_invMassA * PY;
  626.                 w1 -= m_invIA * L1;
  627.                
  628.                 v2.x += m_invMassB * PX;
  629.                 v2.y += m_invMassB * PY;
  630.                 w2 += m_invIB * L2;
  631.             }
  632.             else
  633.             {  
  634.                 m_K.Solve22(_df2, -Cdot1X, -Cdot1Y);
  635.                
  636.                 m_impulse.x += _df2.x;
  637.                 m_impulse.y += _df2.y;
  638.                
  639.                 PX = _df2.x * m_perp.x;
  640.                 PY = _df2.x * m_perp.y;
  641.                 L1 = _df2.x * m_s1 + _df2.y;
  642.                 L2 = _df2.x * m_s2 + _df2.y;
  643.                
  644.                 v1.x -= m_invMassA * PX;
  645.                 v1.y -= m_invMassA * PY;
  646.                 w1 -= m_invIA * L1;
  647.                
  648.                 v2.x += m_invMassB * PX;
  649.                 v2.y += m_invMassB * PY;
  650.                 w2 += m_invIB * L2;
  651.             }
  652.    
  653.             m_bodyA.m_linearVelocity.SetV(v1);
  654.             m_bodyA.m_angularVelocity = w1;
  655.             m_bodyB.m_linearVelocity.SetV(v2);
  656.             m_bodyB.m_angularVelocity = w2;
  657.         }
  658.        
  659.         [Inline]
  660.         final b2internal override function SolvePositionConstraints(baumgarte:Number):Boolean
  661.         {
  662.             //B2_NOT_USED(baumgarte);
  663.            
  664.             m_bodyA = m_bodyA;
  665.             m_bodyB = m_bodyB;
  666.            
  667.             c1 = m_bodyA.m_sweep.c;
  668.             a1 = m_bodyA.m_sweep.a;
  669.            
  670.             c2 = m_bodyB.m_sweep.c;
  671.             a2 = m_bodyB.m_sweep.a;
  672.  
  673.             linearError = 0.0;
  674.             angularError = 0.0;
  675.             active = false;
  676.             C2 = 0.0;
  677.            
  678.            
  679.            
  680.             R1 = b2Pool.getMat22();
  681.             R1.Set(a1);
  682.            
  683.             R2 = b2Pool.getMat22();
  684.             R2.Set(a2);
  685.            
  686.             tMat = R1;
  687.            
  688.             r1X = m_localAnchor1.x - m_localCenterA.x;
  689.             r1Y = m_localAnchor1.y - m_localCenterA.y;
  690.            
  691.             tX =  (tMat.col1.x * r1X + tMat.col2.x * r1Y);
  692.            
  693.             r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
  694.             r1X = tX;
  695.  
  696.             tMat = R2;
  697.            
  698.             r2X = m_localAnchor2.x - m_localCenterB.x;
  699.             r2Y = m_localAnchor2.y - m_localCenterB.y;
  700.            
  701.             tX =  (tMat.col1.x * r2X + tMat.col2.x * r2Y);
  702.             r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
  703.             r2X = tX;
  704.            
  705.             dX = c2.x + r2X - c1.x - r1X;
  706.             dY = c2.y + r2Y - c1.y - r1Y;
  707.            
  708.             if (m_enableLimit)
  709.             {
  710.                 b2Math.MulMV(R1, m_localXAxis1, m_axis);
  711.    
  712.                 //m_a1 = b2Math.b2Cross(d + r1, m_axis);
  713.                 m_a1 = (dX + r1X) * m_axis.y - (dY + r1Y) * m_axis.x;
  714.                 //m_a2 = b2Math.b2Cross(r2, m_axis);
  715.                 m_a2 = r2X * m_axis.y - r2Y * m_axis.x;
  716.                
  717.                 translation = m_axis.x * dX + m_axis.y * dY;
  718.                 gravity = GetBodyA().GetWorld().GetGravity();
  719.    
  720.                 if (b2Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0 * b2Settings.b2_linearSlop)
  721.                 {
  722.                     // Prevent large angular corrections.
  723.                     C2 = b2Math.Clamp(translation, -b2Settings.b2_maxLinearCorrection, b2Settings.b2_maxLinearCorrection);
  724.                     linearError = b2Math.Abs(translation);
  725.                     active = true;
  726.                 }
  727.                 else if(translation <= m_lowerTranslation)
  728.                 {
  729.                     // Prevent large angular corrections and allow some slop.
  730.                     C2 = b2Math.Clamp(translation - m_lowerTranslation + b2Settings.b2_linearSlop, -b2Settings.b2_maxLinearCorrection, 0.0);
  731.                     linearError = m_lowerTranslation - translation;
  732.    
  733.                     if(gravity.x === 0)
  734.                     {
  735.                         if(gravity.y < 0)
  736.                         {
  737.                             active = false;
  738.                         }
  739.                         else
  740.                         {
  741.                             active = true;
  742.                         }
  743.                     }
  744.                     else
  745.                     {
  746.                         if(gravity.x < 0)
  747.                         {
  748.                             active = false;
  749.                         }
  750.                         else
  751.                         {
  752.                             active = true;
  753.                         }
  754.                     }
  755.                 }
  756.                 else if (translation >= m_upperTranslation)
  757.                 {
  758.                     // Prevent large angular corrections and allow some slop.
  759.                     C2 = b2Math.Clamp(translation - m_upperTranslation + b2Settings.b2_linearSlop, 0.0, b2Settings.b2_maxLinearCorrection);
  760.                     linearError = translation - m_upperTranslation;
  761.    
  762.                     if(gravity.x === 0)
  763.                     {
  764.                         if(gravity.y > 0)
  765.                         {
  766.                             active = false;
  767.                         }
  768.                         else
  769.                         {
  770.                             active = true;
  771.                         }  
  772.                     }
  773.                     else
  774.                     {
  775.                         if(gravity.x > 0)
  776.                         {
  777.                             active = false;
  778.                         }
  779.                         else
  780.                         {
  781.                             active = true;
  782.                         }
  783.                     }
  784.                 }
  785.             }
  786.    
  787.             b2Math.MulMV(R1, m_localYAxis1, m_perp);
  788.    
  789.             //m_s1 = b2Cross(d + r1, m_perp);
  790.             m_s1 = (dX + r1X) * m_perp.y - (dY + r1Y) * m_perp.x;
  791.             //m_s2 = b2Cross(r2, m_perp);
  792.             m_s2 = r2X * m_perp.y - r2Y * m_perp.x;
  793.            
  794.             impulse3 = b2Pool.getVector3(0, 0, 0);  
  795.                
  796.             C1X = m_perp.x * dX + m_perp.y * dY;
  797.             C1Y = a2 - a1 - m_refAngle;
  798.            
  799.             linearError = b2Math.Max(linearError, b2Math.Abs(C1X));
  800.             angularError = b2Math.Abs(C1Y);
  801.    
  802.             if(active)
  803.             {
  804.                 m1 = m_invMassA;
  805.                 m2 = m_invMassB;
  806.                 i1 = m_invIA;
  807.                 i2 = m_invIB;
  808.                
  809.                 m_K.col1.x = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
  810.                 m_K.col1.y = i1 * m_s1 + i2 * m_s2;
  811.                 m_K.col1.z = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
  812.                 m_K.col2.x = m_K.col1.y;
  813.                 m_K.col2.y = i1 + i2;
  814.                 m_K.col2.z = i1 * m_a1 + i2 * m_a2;
  815.                 m_K.col3.x = m_K.col1.z;
  816.                 m_K.col3.y = m_K.col2.z;
  817.                 m_K.col3.z = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
  818.                
  819.                 m_K.Solve33(impulse3, -C1X, -C1Y, -C2);
  820.             }
  821.             else
  822.             {
  823.                 m1 = m_invMassA;
  824.                 m2 = m_invMassB;
  825.                 i1 = m_invIA;
  826.                 i2 = m_invIB;
  827.                
  828.                 k11  = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
  829.                 k12 = i1 * m_s1 + i2 * m_s2;
  830.                 k22 = i1 + i2;
  831.                
  832.                 m_K.col1.Set(k11, k12, 0.0);
  833.                 m_K.col2.Set(k12, k22, 0.0);
  834.                
  835.                 impulse1 = b2Pool.getVector2(0, 0);
  836.                    
  837.                 m_K.Solve22(impulse1, -C1X, -C1Y);
  838.                
  839.                 impulse3.x = impulse1.x;
  840.                 impulse3.y = impulse1.y;
  841.                 impulse3.z = 0.0;
  842.                
  843.                 b2Pool.putVector2(impulse1);
  844.             }
  845.            
  846.             PX = impulse3.x * m_perp.x + impulse3.z * m_axis.x;
  847.             PY = impulse3.x * m_perp.y + impulse3.z * m_axis.y;
  848.             L1 = impulse3.x * m_s1 + impulse3.y + impulse3.z * m_a1;
  849.             L2 = impulse3.x * m_s2 + impulse3.y + impulse3.z * m_a2;
  850.            
  851.             b2Pool.putVector3(impulse3);
  852.            
  853.             c1.x -= m_invMassA * PX;
  854.             c1.y -= m_invMassA * PY;
  855.             a1 -= m_invIA * L1;
  856.            
  857.             c2.x += m_invMassB * PX;
  858.             c2.y += m_invMassB * PY;
  859.             a2 += m_invIB * L2;
  860.            
  861.             // TODO_ERIN remove need for this
  862.             //bA.m_sweep.c = c1;    //Already done by reference
  863.             m_bodyA.m_sweep.a = a1;
  864.             //bB.m_sweep.c = c2;    //Already done by reference
  865.             m_bodyB.m_sweep.a = a2;
  866.            
  867.            
  868.             m_bodyA.SynchronizeTransform();
  869.             m_bodyB.SynchronizeTransform();
  870.    
  871.            
  872.             b2Pool.putMat22(R1);
  873.             b2Pool.putMat22(R2);
  874.            
  875.             return linearError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
  876.            
  877.         }
  878.    
  879.         b2internal var m_localAnchor1:b2Vec2 = new b2Vec2();
  880.         b2internal var m_localAnchor2:b2Vec2 = new b2Vec2();
  881.         b2internal var m_localXAxis1:b2Vec2 = new b2Vec2();
  882.         private var m_localYAxis1:b2Vec2 = new b2Vec2();
  883.         private var m_refAngle:Number;
  884.    
  885.         private var m_axis:b2Vec2 = new b2Vec2();
  886.         private var m_perp:b2Vec2 = new b2Vec2();
  887.         private var m_s1:Number;
  888.         private var m_s2:Number;
  889.         private var m_a1:Number;
  890.         private var m_a2:Number;
  891.        
  892.         private var m_K:b2Mat33 = new b2Mat33();
  893.         private var m_impulse:b2Vec3 = new b2Vec3();
  894.    
  895.         private var m_motorMass:Number;         // effective mass for motor/limit translational constraint.
  896.         private var m_motorImpulse:Number;
  897.    
  898.         private var m_lowerTranslation:Number;
  899.         private var m_upperTranslation:Number;
  900.         private var m_maxMotorForce:Number;
  901.         private var m_motorSpeed:Number;
  902.        
  903.         private var m_enableLimit:Boolean;
  904.         private var m_enableMotor:Boolean;
  905.         private var m_limitState:int;
  906.     };
  907. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!