b2FrictionJoint.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/b2FrictionJoint.h>
  19. #include <Box2D/Dynamics/b2Body.h>
  20. #include <Box2D/Dynamics/b2TimeStep.h>
  21. // Point-to-point constraint
  22. // Cdot = v2 - v1
  23. //      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
  24. // J = [-I -r1_skew I r2_skew ]
  25. // Identity used:
  26. // w k % (rx i + ry j) = w * (-ry i + rx j)
  27. // Angle constraint
  28. // Cdot = w2 - w1
  29. // J = [0 0 -1 0 0 1]
  30. // K = invI1 + invI2
  31. void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
  32. {
  33. bodyA = bA;
  34. bodyB = bB;
  35. localAnchorA = bodyA->GetLocalPoint(anchor);
  36. localAnchorB = bodyB->GetLocalPoint(anchor);
  37. }
  38. b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
  39. : b2Joint(def)
  40. {
  41. m_localAnchorA = def->localAnchorA;
  42. m_localAnchorB = def->localAnchorB;
  43. m_linearImpulse.SetZero();
  44. m_angularImpulse = 0.0f;
  45. m_maxForce = def->maxForce;
  46. m_maxTorque = def->maxTorque;
  47. }
  48. void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step)
  49. {
  50. b2Body* bA = m_bodyA;
  51. b2Body* bB = m_bodyB;
  52. // Compute the effective mass matrix.
  53. b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
  54. b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
  55. // J = [-I -r1_skew I r2_skew]
  56. //     [ 0       -1 0       1]
  57. // r_skew = [-ry; rx]
  58. // Matlab
  59. // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
  60. //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
  61. //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
  62. float32 mA = bA->m_invMass, mB = bB->m_invMass;
  63. float32 iA = bA->m_invI, iB = bB->m_invI;
  64. b2Mat22 K1;
  65. K1.col1.x = mA + mB; K1.col2.x = 0.0f;
  66. K1.col1.y = 0.0f; K1.col2.y = mA + mB;
  67. b2Mat22 K2;
  68. K2.col1.x =  iA * rA.y * rA.y; K2.col2.x = -iA * rA.x * rA.y;
  69. K2.col1.y = -iA * rA.x * rA.y; K2.col2.y =  iA * rA.x * rA.x;
  70. b2Mat22 K3;
  71. K3.col1.x =  iB * rB.y * rB.y; K3.col2.x = -iB * rB.x * rB.y;
  72. K3.col1.y = -iB * rB.x * rB.y; K3.col2.y =  iB * rB.x * rB.x;
  73. b2Mat22 K = K1 + K2 + K3;
  74. m_linearMass = K.GetInverse();
  75. m_angularMass = iA + iB;
  76. if (m_angularMass > 0.0f)
  77. {
  78. m_angularMass = 1.0f / m_angularMass;
  79. }
  80. if (step.warmStarting)
  81. {
  82. // Scale impulses to support a variable time step.
  83. m_linearImpulse *= step.dtRatio;
  84. m_angularImpulse *= step.dtRatio;
  85. b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
  86. bA->m_linearVelocity -= mA * P;
  87. bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_angularImpulse);
  88. bB->m_linearVelocity += mB * P;
  89. bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_angularImpulse);
  90. }
  91. else
  92. {
  93. m_linearImpulse.SetZero();
  94. m_angularImpulse = 0.0f;
  95. }
  96. }
  97. void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step)
  98. {
  99. B2_NOT_USED(step);
  100. b2Body* bA = m_bodyA;
  101. b2Body* bB = m_bodyB;
  102. b2Vec2 vA = bA->m_linearVelocity;
  103. float32 wA = bA->m_angularVelocity;
  104. b2Vec2 vB = bB->m_linearVelocity;
  105. float32 wB = bB->m_angularVelocity;
  106. float32 mA = bA->m_invMass, mB = bB->m_invMass;
  107. float32 iA = bA->m_invI, iB = bB->m_invI;
  108. b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
  109. b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
  110. // Solve angular friction
  111. {
  112. float32 Cdot = wB - wA;
  113. float32 impulse = -m_angularMass * Cdot;
  114. float32 oldImpulse = m_angularImpulse;
  115. float32 maxImpulse = step.dt * m_maxTorque;
  116. m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
  117. impulse = m_angularImpulse - oldImpulse;
  118. wA -= iA * impulse;
  119. wB += iB * impulse;
  120. }
  121. // Solve linear friction
  122. {
  123. b2Vec2 Cdot = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
  124. b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
  125. b2Vec2 oldImpulse = m_linearImpulse;
  126. m_linearImpulse += impulse;
  127. float32 maxImpulse = step.dt * m_maxForce;
  128. if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
  129. {
  130. m_linearImpulse.Normalize();
  131. m_linearImpulse *= maxImpulse;
  132. }
  133. impulse = m_linearImpulse - oldImpulse;
  134. vA -= mA * impulse;
  135. wA -= iA * b2Cross(rA, impulse);
  136. vB += mB * impulse;
  137. wB += iB * b2Cross(rB, impulse);
  138. }
  139. bA->m_linearVelocity = vA;
  140. bA->m_angularVelocity = wA;
  141. bB->m_linearVelocity = vB;
  142. bB->m_angularVelocity = wB;
  143. }
  144. bool b2FrictionJoint::SolvePositionConstraints(float32 baumgarte)
  145. {
  146. B2_NOT_USED(baumgarte);
  147. return true;
  148. }
  149. b2Vec2 b2FrictionJoint::GetAnchorA() const
  150. {
  151. return m_bodyA->GetWorldPoint(m_localAnchorA);
  152. }
  153. b2Vec2 b2FrictionJoint::GetAnchorB() const
  154. {
  155. return m_bodyB->GetWorldPoint(m_localAnchorB);
  156. }
  157. b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const
  158. {
  159. return inv_dt * m_linearImpulse;
  160. }
  161. float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const
  162. {
  163. return inv_dt * m_angularImpulse;
  164. }
  165. void b2FrictionJoint::SetMaxForce(float32 force)
  166. {
  167. b2Assert(b2IsValid(force) && force >= 0.0f);
  168. m_maxForce = force;
  169. }
  170. float32 b2FrictionJoint::GetMaxForce() const
  171. {
  172. return m_maxForce;
  173. }
  174. void b2FrictionJoint::SetMaxTorque(float32 torque)
  175. {
  176. b2Assert(b2IsValid(torque) && torque >= 0.0f);
  177. m_maxTorque = torque;
  178. }
  179. float32 b2FrictionJoint::GetMaxTorque() const
  180. {
  181. return m_maxTorque;
  182. }