b2WeldJoint.cpp
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:6k
源码类别:

游戏引擎

开发平台:

Visual C++

  1. /*
  2. * Copyright (c) 2006-2009 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. #include <Box2D/Dynamics/Joints/b2WeldJoint.h>
  19. #include <Box2D/Dynamics/b2Body.h>
  20. #include <Box2D/Dynamics/b2TimeStep.h>
  21. // Point-to-point constraint
  22. // C = p2 - p1
  23. // Cdot = v2 - v1
  24. //      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
  25. // J = [-I -r1_skew I r2_skew ]
  26. // Identity used:
  27. // w k % (rx i + ry j) = w * (-ry i + rx j)
  28. // Angle constraint
  29. // C = angle2 - angle1 - referenceAngle
  30. // Cdot = w2 - w1
  31. // J = [0 0 -1 0 0 1]
  32. // K = invI1 + invI2
  33. void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
  34. {
  35. bodyA = bA;
  36. bodyB = bB;
  37. localAnchorA = bodyA->GetLocalPoint(anchor);
  38. localAnchorB = bodyB->GetLocalPoint(anchor);
  39. referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
  40. }
  41. b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)
  42. : b2Joint(def)
  43. {
  44. m_localAnchorA = def->localAnchorA;
  45. m_localAnchorB = def->localAnchorB;
  46. m_referenceAngle = def->referenceAngle;
  47. m_impulse.SetZero();
  48. }
  49. void b2WeldJoint::InitVelocityConstraints(const b2TimeStep& step)
  50. {
  51. b2Body* bA = m_bodyA;
  52. b2Body* bB = m_bodyB;
  53. // Compute the effective mass matrix.
  54. b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
  55. b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
  56. // J = [-I -r1_skew I r2_skew]
  57. //     [ 0       -1 0       1]
  58. // r_skew = [-ry; rx]
  59. // Matlab
  60. // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
  61. //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
  62. //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
  63. float32 mA = bA->m_invMass, mB = bB->m_invMass;
  64. float32 iA = bA->m_invI, iB = bB->m_invI;
  65. m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
  66. m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
  67. m_mass.col3.x = -rA.y * iA - rB.y * iB;
  68. m_mass.col1.y = m_mass.col2.x;
  69. m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
  70. m_mass.col3.y = rA.x * iA + rB.x * iB;
  71. m_mass.col1.z = m_mass.col3.x;
  72. m_mass.col2.z = m_mass.col3.y;
  73. m_mass.col3.z = iA + iB;
  74. if (step.warmStarting)
  75. {
  76. // Scale impulses to support a variable time step.
  77. m_impulse *= step.dtRatio;
  78. b2Vec2 P(m_impulse.x, m_impulse.y);
  79. bA->m_linearVelocity -= mA * P;
  80. bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_impulse.z);
  81. bB->m_linearVelocity += mB * P;
  82. bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_impulse.z);
  83. }
  84. else
  85. {
  86. m_impulse.SetZero();
  87. }
  88. }
  89. void b2WeldJoint::SolveVelocityConstraints(const b2TimeStep& step)
  90. {
  91. B2_NOT_USED(step);
  92. b2Body* bA = m_bodyA;
  93. b2Body* bB = m_bodyB;
  94. b2Vec2 vA = bA->m_linearVelocity;
  95. float32 wA = bA->m_angularVelocity;
  96. b2Vec2 vB = bB->m_linearVelocity;
  97. float32 wB = bB->m_angularVelocity;
  98. float32 mA = bA->m_invMass, mB = bB->m_invMass;
  99. float32 iA = bA->m_invI, iB = bB->m_invI;
  100. b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
  101. b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
  102. // Solve point-to-point constraint
  103. b2Vec2 Cdot1 = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
  104. float32 Cdot2 = wB - wA;
  105. b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
  106. b2Vec3 impulse = m_mass.Solve33(-Cdot);
  107. m_impulse += impulse;
  108. b2Vec2 P(impulse.x, impulse.y);
  109. vA -= mA * P;
  110. wA -= iA * (b2Cross(rA, P) + impulse.z);
  111. vB += mB * P;
  112. wB += iB * (b2Cross(rB, P) + impulse.z);
  113. bA->m_linearVelocity = vA;
  114. bA->m_angularVelocity = wA;
  115. bB->m_linearVelocity = vB;
  116. bB->m_angularVelocity = wB;
  117. }
  118. bool b2WeldJoint::SolvePositionConstraints(float32 baumgarte)
  119. {
  120. B2_NOT_USED(baumgarte);
  121. b2Body* bA = m_bodyA;
  122. b2Body* bB = m_bodyB;
  123. float32 mA = bA->m_invMass, mB = bB->m_invMass;
  124. float32 iA = bA->m_invI, iB = bB->m_invI;
  125. b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
  126. b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
  127. b2Vec2 C1 =  bB->m_sweep.c + rB - bA->m_sweep.c - rA;
  128. float32 C2 = bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
  129. // Handle large detachment.
  130. const float32 k_allowedStretch = 10.0f * b2_linearSlop;
  131. float32 positionError = C1.Length();
  132. float32 angularError = b2Abs(C2);
  133. if (positionError > k_allowedStretch)
  134. {
  135. iA *= 1.0f;
  136. iB *= 1.0f;
  137. }
  138. m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
  139. m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
  140. m_mass.col3.x = -rA.y * iA - rB.y * iB;
  141. m_mass.col1.y = m_mass.col2.x;
  142. m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
  143. m_mass.col3.y = rA.x * iA + rB.x * iB;
  144. m_mass.col1.z = m_mass.col3.x;
  145. m_mass.col2.z = m_mass.col3.y;
  146. m_mass.col3.z = iA + iB;
  147. b2Vec3 C(C1.x, C1.y, C2);
  148. b2Vec3 impulse = m_mass.Solve33(-C);
  149. b2Vec2 P(impulse.x, impulse.y);
  150. bA->m_sweep.c -= mA * P;
  151. bA->m_sweep.a -= iA * (b2Cross(rA, P) + impulse.z);
  152. bB->m_sweep.c += mB * P;
  153. bB->m_sweep.a += iB * (b2Cross(rB, P) + impulse.z);
  154. bA->SynchronizeTransform();
  155. bB->SynchronizeTransform();
  156. return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
  157. }
  158. b2Vec2 b2WeldJoint::GetAnchorA() const
  159. {
  160. return m_bodyA->GetWorldPoint(m_localAnchorA);
  161. }
  162. b2Vec2 b2WeldJoint::GetAnchorB() const
  163. {
  164. return m_bodyB->GetWorldPoint(m_localAnchorB);
  165. }
  166. b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const
  167. {
  168. b2Vec2 P(m_impulse.x, m_impulse.y);
  169. return inv_dt * P;
  170. }
  171. float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const
  172. {
  173. return inv_dt * m_impulse.z;
  174. }