API tools faq
paste
Advertisement
Guest User

b2WeldJoint

a guest
Apr 13th, 2018
132
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
ActionScript 3 9.44 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.     import Box2D.Common.b2Settings;
  22.     import Box2D.Common.b2internal;
  23.     import Box2D.Common.Math.b2Mat22;
  24.     import Box2D.Common.Math.b2Mat33;
  25.     import Box2D.Common.Math.b2Math;
  26.     import Box2D.Common.Math.b2Vec2;
  27.     import Box2D.Common.Math.b2Vec3;
  28.     import Box2D.Dynamics.b2TimeStep;
  29.     use namespace b2internal;
  30.    
  31.     // Point-to-point constraint
  32.     // Cdot = v2 - v1
  33.     //      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
  34.     // J = [-I -r1_skew I r2_skew ]
  35.     // Identity used:
  36.     // w k % (rx i + ry j) = w * (-ry i + rx j)
  37.    
  38.     // Angle constraint
  39.     // Cdot = w2 - w1
  40.     // J = [0 0 -1 0 0 1]
  41.     // K = invI1 + invI2
  42.    
  43.     /**
  44.      * A weld joint essentially glues two bodies together. A weld joint may
  45.      * distort somewhat because the island constraint solver is approximate.
  46.      */
  47.     public class b2WeldJoint extends b2Joint
  48.     {
  49.         /** @inheritDoc */
  50.         public override function GetAnchorA(result:b2Vec2):void
  51.         {
  52.             m_bodyA.GetWorldPoint(m_localAnchorA, result);
  53.         }
  54.    
  55.         /** @inheritDoc */
  56.         public override function GetAnchorB(result:b2Vec2):void
  57.         {
  58.             return m_bodyB.GetWorldPoint(m_localAnchorB, result);
  59.         }
  60.        
  61.         /** @inheritDoc */
  62.         public override function GetReactionForce(inv_dt:Number):b2Vec2
  63.         {
  64.             return new b2Vec2(inv_dt * m_impulse.x, inv_dt * m_impulse.y);
  65.         }
  66.    
  67.         /** @inheritDoc */
  68.         public override function GetReactionTorque(inv_dt:Number):Number
  69.         {
  70.             return inv_dt * m_impulse.z;
  71.         }
  72.        
  73.         //--------------- Internals Below -------------------
  74.    
  75.         /** @private */
  76.         public function b2WeldJoint(def:b2WeldJointDef)
  77.         {
  78.             super(def);
  79.            
  80.             m_localAnchorA.SetV(def.localAnchorA);
  81.             m_localAnchorB.SetV(def.localAnchorB);
  82.             m_referenceAngle = def.referenceAngle;
  83.    
  84.             m_impulse.SetZero();
  85.             m_mass = new b2Mat33();
  86.         }
  87.    
  88.         [Inline]
  89.         final b2internal override function InitVelocityConstraints(step:b2TimeStep):void
  90.         {
  91.             tMat = m_bodyA.m_xf.R;
  92.             rAX = m_localAnchorA.x - m_bodyA.m_sweep.localCenter.x;
  93.             rAY = m_localAnchorA.y - m_bodyA.m_sweep.localCenter.y;
  94.            
  95.             tX =  (tMat.col1.x * rAX + tMat.col2.x * rAY);
  96.             rAY = (tMat.col1.y * rAX + tMat.col2.y * rAY);
  97.             rAX = tX;
  98.  
  99.             tMat = m_bodyB.m_xf.R;
  100.  
  101.             rBX = m_localAnchorB.x - m_bodyB.m_sweep.localCenter.x;
  102.             rBY = m_localAnchorB.y - m_bodyB.m_sweep.localCenter.y;
  103.  
  104.             tX =  (tMat.col1.x * rBX + tMat.col2.x * rBY);
  105.             rBY = (tMat.col1.y * rBX + tMat.col2.y * rBY);
  106.             rBX = tX;
  107.  
  108.             mA = m_bodyA.m_invMass
  109.             mB = m_bodyB.m_invMass;
  110.             iA = m_bodyA.m_invI
  111.             iB = m_bodyB.m_invI;
  112.            
  113.             m_mass.col1.x = mA + mB + rAY * rAY * iA + rBY * rBY * iB;
  114.             m_mass.col2.x = -rAY * rAX * iA - rBY * rBX * iB;
  115.             m_mass.col3.x = -rAY * iA - rBY * iB;
  116.             m_mass.col1.y = m_mass.col2.x;
  117.             m_mass.col2.y = mA + mB + rAX * rAX * iA + rBX * rBX * iB;
  118.             m_mass.col3.y = rAX * iA + rBX * iB;
  119.             m_mass.col1.z = m_mass.col3.x;
  120.             m_mass.col2.z = m_mass.col3.y;
  121.             m_mass.col3.z = iA + iB;
  122.            
  123.             if (step.warmStarting)
  124.             {
  125.                 // Scale impulses to support a variable time step.
  126.                 m_impulse.x *= step.dtRatio;
  127.                 m_impulse.y *= step.dtRatio;
  128.                 m_impulse.z *= step.dtRatio;
  129.    
  130.                 m_bodyA.m_linearVelocity.x -= mA * m_impulse.x;
  131.                 m_bodyA.m_linearVelocity.y -= mA * m_impulse.y;
  132.                 m_bodyA.m_angularVelocity -= iA * (rAX * m_impulse.y - rAY * m_impulse.x + m_impulse.z);
  133.    
  134.                 m_bodyB.m_linearVelocity.x += mB * m_impulse.x;
  135.                 m_bodyB.m_linearVelocity.y += mB * m_impulse.y;
  136.                 m_bodyB.m_angularVelocity += iB * (rBX * m_impulse.y - rBY * m_impulse.x + m_impulse.z);
  137.             }
  138.             else
  139.             {
  140.                 m_impulse.SetZero();
  141.             }
  142.         }
  143.        
  144.         private var _velocityImpulse:b2Vec3 = new b2Vec3(0, 0, 0);
  145.        
  146.         [Inline]
  147.         final b2internal override function SolveVelocityConstraints(step:b2TimeStep):void
  148.         {
  149.             vA = m_bodyA.m_linearVelocity;
  150.             vB = m_bodyB.m_linearVelocity;
  151.  
  152.             tMat = m_bodyA.m_xf.R;
  153.            
  154.             rAX = m_localAnchorA.x - m_bodyA.m_sweep.localCenter.x;
  155.             rAY = m_localAnchorA.y - m_bodyA.m_sweep.localCenter.y;
  156.            
  157.             tX =  (tMat.col1.x * rAX + tMat.col2.x * rAY);
  158.             rAY = (tMat.col1.y * rAX + tMat.col2.y * rAY);
  159.             rAX = tX;
  160.  
  161.             tMat = m_bodyB.m_xf.R;
  162.            
  163.             rBX = m_localAnchorB.x - m_bodyB.m_sweep.localCenter.x;
  164.             rBY = m_localAnchorB.y - m_bodyB.m_sweep.localCenter.y;
  165.            
  166.             tX =  (tMat.col1.x * rBX + tMat.col2.x * rBY);
  167.             rBY = (tMat.col1.y * rBX + tMat.col2.y * rBY);
  168.             rBX = tX;
  169.            
  170.             // Solve point-to-point constraint
  171.             Cdot1X = vB.x - m_bodyB.m_angularVelocity * rBY - vA.x + m_bodyA.m_angularVelocity * rAY;
  172.             Cdot1Y = vB.y + m_bodyB.m_angularVelocity * rBX - vA.y - m_bodyA.m_angularVelocity * rAX;
  173.             Cdot2 = m_bodyB.m_angularVelocity - m_bodyA.m_angularVelocity;
  174.    
  175.             m_mass.Solve33(_velocityImpulse, -Cdot1X, -Cdot1Y, -Cdot2);
  176.            
  177.             m_impulse.Add(_velocityImpulse);
  178.            
  179.             vA.x -= m_bodyA.m_invMass * _velocityImpulse.x;
  180.             vA.y -= m_bodyA.m_invMass * _velocityImpulse.y;
  181.  
  182.             m_bodyA.m_angularVelocity -= m_bodyA.m_invI * (rAX * _velocityImpulse.y - rAY * _velocityImpulse.x + _velocityImpulse.z);
  183.    
  184.             vB.x += m_bodyB.m_invMass * _velocityImpulse.x;
  185.             vB.y += m_bodyB.m_invMass * _velocityImpulse.y;
  186.  
  187.             m_bodyB.m_angularVelocity += m_bodyB.m_invI * (rBX * _velocityImpulse.y - rBY * _velocityImpulse.x + _velocityImpulse.z);
  188.  
  189.             m_bodyA.m_angularVelocity = m_bodyA.m_angularVelocity;
  190.             m_bodyB.m_angularVelocity = m_bodyB.m_angularVelocity;
  191.         }
  192.        
  193.        
  194.         private var tMat:b2Mat22;
  195.         private var tX:Number;
  196.  
  197.         private var rAX:Number;
  198.         private var rAY:Number;
  199.        
  200.         private var rBX:Number;
  201.         private var rBY:Number;
  202.        
  203.         private var mA:Number;
  204.         private var mB:Number;
  205.         private var iA:Number;
  206.         private var iB:Number;
  207.        
  208.         private var C1X:Number;
  209.         private var C1Y:Number;
  210.         private var C2:Number;
  211.        
  212.         // Handle large detachment.
  213.         private var k_allowedStretch:Number;
  214.         private var positionError:Number;
  215.         private var angularError:Number;
  216.        
  217.         private var _positionImpulse:b2Vec3 = new b2Vec3(0, 0, 0);
  218.        
  219.         private var vA:b2Vec2;
  220.         private var wA:Number;
  221.         private var vB:b2Vec2;
  222.         private var wB:Number;
  223.        
  224.         private var Cdot1X:Number;
  225.         private var Cdot1Y:Number;
  226.         private var Cdot2:Number;
  227.  
  228.        
  229.         [Inline]
  230.         final b2internal override function SolvePositionConstraints(baumgarte:Number):Boolean
  231.         {
  232.             tMat = m_bodyA.m_xf.R;
  233.            
  234.             rAX = m_localAnchorA.x - m_bodyA.m_sweep.localCenter.x;
  235.             rAY = m_localAnchorA.y - m_bodyA.m_sweep.localCenter.y;
  236.            
  237.             tX =  (tMat.col1.x * rAX + tMat.col2.x * rAY);
  238.             rAY = (tMat.col1.y * rAX + tMat.col2.y * rAY);
  239.             rAX = tX;
  240.  
  241.             tMat = m_bodyB.m_xf.R;
  242.             rBX = m_localAnchorB.x - m_bodyB.m_sweep.localCenter.x;
  243.             rBY = m_localAnchorB.y - m_bodyB.m_sweep.localCenter.y;
  244.             tX =  (tMat.col1.x * rBX + tMat.col2.x * rBY);
  245.             rBY = (tMat.col1.y * rBX + tMat.col2.y * rBY);
  246.             rBX = tX;
  247.  
  248.             mA = m_bodyA.m_invMass
  249.             mB = m_bodyB.m_invMass;
  250.             iA = m_bodyA.m_invI
  251.             iB = m_bodyB.m_invI;
  252.  
  253.             C1X =  m_bodyB.m_sweep.c.x + rBX - m_bodyA.m_sweep.c.x - rAX;
  254.             C1Y =  m_bodyB.m_sweep.c.y + rBY - m_bodyA.m_sweep.c.y - rAY;
  255.             C2 = m_bodyB.m_sweep.a - m_bodyA.m_sweep.a - m_referenceAngle;
  256.    
  257.             // Handle large detachment.
  258.             k_allowedStretch = 10.0 * b2Settings.b2_linearSlop;
  259.             positionError = Math.sqrt(C1X * C1X + C1Y * C1Y);
  260.             angularError = b2Math.Abs(C2);
  261.    
  262.             if(positionError > k_allowedStretch)
  263.             {
  264.                 iA *= 1.0;
  265.                 iB *= 1.0;
  266.             }
  267.            
  268.             m_mass.col1.x = mA + mB + rAY * rAY * iA + rBY * rBY * iB;
  269.             m_mass.col2.x = -rAY * rAX * iA - rBY * rBX * iB;
  270.             m_mass.col3.x = -rAY * iA - rBY * iB;
  271.             m_mass.col1.y = m_mass.col2.x;
  272.             m_mass.col2.y = mA + mB + rAX * rAX * iA + rBX * rBX * iB;
  273.             m_mass.col3.y = rAX * iA + rBX * iB;
  274.             m_mass.col1.z = m_mass.col3.x;
  275.             m_mass.col2.z = m_mass.col3.y;
  276.             m_mass.col3.z = iA + iB;
  277.            
  278.             m_mass.Solve33(_positionImpulse, -C1X, -C1Y, -C2);
  279.            
  280.    
  281.             m_bodyA.m_sweep.c.x -= mA * _positionImpulse.x;
  282.             m_bodyA.m_sweep.c.y -= mA * _positionImpulse.y;
  283.             m_bodyA.m_sweep.a -= iA * (rAX * _positionImpulse.y - rAY * _positionImpulse.x + _positionImpulse.z);
  284.    
  285.             m_bodyB.m_sweep.c.x += mB * _positionImpulse.x;
  286.             m_bodyB.m_sweep.c.y += mB * _positionImpulse.y;
  287.             m_bodyB.m_sweep.a += iB * (rBX * _positionImpulse.y - rBY * _positionImpulse.x + _positionImpulse.z);
  288.    
  289.             m_bodyA.SynchronizeTransform();
  290.             m_bodyB.SynchronizeTransform();
  291.    
  292.             return positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
  293.    
  294.         }
  295.    
  296.         private var m_localAnchorA:b2Vec2 = new b2Vec2();
  297.         private var m_localAnchorB:b2Vec2 = new b2Vec2();
  298.         private var m_referenceAngle:Number;
  299.        
  300.         private var m_impulse:b2Vec3 = new b2Vec3();
  301.         private var m_mass:b2Mat33 = new b2Mat33();
  302.     };
  303. }
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!