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

游戏引擎

开发平台:

Visual C++

  1. /*
  2. * Copyright (c) 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. #include <Box2D/Dynamics/Joints/b2GearJoint.h>
  19. #include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
  20. #include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
  21. #include <Box2D/Dynamics/b2Body.h>
  22. #include <Box2D/Dynamics/b2TimeStep.h>
  23. // Gear Joint:
  24. // C0 = (coordinate1 + ratio * coordinate2)_initial
  25. // C = C0 - (cordinate1 + ratio * coordinate2) = 0
  26. // Cdot = -(Cdot1 + ratio * Cdot2)
  27. // J = -[J1 ratio * J2]
  28. // K = J * invM * JT
  29. //   = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
  30. //
  31. // Revolute:
  32. // coordinate = rotation
  33. // Cdot = angularVelocity
  34. // J = [0 0 1]
  35. // K = J * invM * JT = invI
  36. //
  37. // Prismatic:
  38. // coordinate = dot(p - pg, ug)
  39. // Cdot = dot(v + cross(w, r), ug)
  40. // J = [ug cross(r, ug)]
  41. // K = J * invM * JT = invMass + invI * cross(r, ug)^2
  42. b2GearJoint::b2GearJoint(const b2GearJointDef* def)
  43. : b2Joint(def)
  44. {
  45. b2JointType type1 = def->joint1->GetType();
  46. b2JointType type2 = def->joint2->GetType();
  47. b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint);
  48. b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint);
  49. b2Assert(def->joint1->GetBodyA()->GetType() == b2_staticBody);
  50. b2Assert(def->joint2->GetBodyA()->GetType() == b2_staticBody);
  51. m_revolute1 = NULL;
  52. m_prismatic1 = NULL;
  53. m_revolute2 = NULL;
  54. m_prismatic2 = NULL;
  55. float32 coordinate1, coordinate2;
  56. m_ground1 = def->joint1->GetBodyA();
  57. m_bodyA = def->joint1->GetBodyB();
  58. if (type1 == e_revoluteJoint)
  59. {
  60. m_revolute1 = (b2RevoluteJoint*)def->joint1;
  61. m_groundAnchor1 = m_revolute1->m_localAnchor1;
  62. m_localAnchor1 = m_revolute1->m_localAnchor2;
  63. coordinate1 = m_revolute1->GetJointAngle();
  64. }
  65. else
  66. {
  67. m_prismatic1 = (b2PrismaticJoint*)def->joint1;
  68. m_groundAnchor1 = m_prismatic1->m_localAnchor1;
  69. m_localAnchor1 = m_prismatic1->m_localAnchor2;
  70. coordinate1 = m_prismatic1->GetJointTranslation();
  71. }
  72. m_ground2 = def->joint2->GetBodyA();
  73. m_bodyB = def->joint2->GetBodyB();
  74. if (type2 == e_revoluteJoint)
  75. {
  76. m_revolute2 = (b2RevoluteJoint*)def->joint2;
  77. m_groundAnchor2 = m_revolute2->m_localAnchor1;
  78. m_localAnchor2 = m_revolute2->m_localAnchor2;
  79. coordinate2 = m_revolute2->GetJointAngle();
  80. }
  81. else
  82. {
  83. m_prismatic2 = (b2PrismaticJoint*)def->joint2;
  84. m_groundAnchor2 = m_prismatic2->m_localAnchor1;
  85. m_localAnchor2 = m_prismatic2->m_localAnchor2;
  86. coordinate2 = m_prismatic2->GetJointTranslation();
  87. }
  88. m_ratio = def->ratio;
  89. m_constant = coordinate1 + m_ratio * coordinate2;
  90. m_impulse = 0.0f;
  91. }
  92. void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step)
  93. {
  94. b2Body* g1 = m_ground1;
  95. b2Body* g2 = m_ground2;
  96. b2Body* b1 = m_bodyA;
  97. b2Body* b2 = m_bodyB;
  98. float32 K = 0.0f;
  99. m_J.SetZero();
  100. if (m_revolute1)
  101. {
  102. m_J.angularA = -1.0f;
  103. K += b1->m_invI;
  104. }
  105. else
  106. {
  107. b2Vec2 ug = b2Mul(g1->GetTransform().R, m_prismatic1->m_localXAxis1);
  108. b2Vec2 r = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
  109. float32 crug = b2Cross(r, ug);
  110. m_J.linearA = -ug;
  111. m_J.angularA = -crug;
  112. K += b1->m_invMass + b1->m_invI * crug * crug;
  113. }
  114. if (m_revolute2)
  115. {
  116. m_J.angularB = -m_ratio;
  117. K += m_ratio * m_ratio * b2->m_invI;
  118. }
  119. else
  120. {
  121. b2Vec2 ug = b2Mul(g2->GetTransform().R, m_prismatic2->m_localXAxis1);
  122. b2Vec2 r = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
  123. float32 crug = b2Cross(r, ug);
  124. m_J.linearB = -m_ratio * ug;
  125. m_J.angularB = -m_ratio * crug;
  126. K += m_ratio * m_ratio * (b2->m_invMass + b2->m_invI * crug * crug);
  127. }
  128. // Compute effective mass.
  129. m_mass = K > 0.0f ? 1.0f / K : 0.0f;
  130. if (step.warmStarting)
  131. {
  132. // Warm starting.
  133. b1->m_linearVelocity += b1->m_invMass * m_impulse * m_J.linearA;
  134. b1->m_angularVelocity += b1->m_invI * m_impulse * m_J.angularA;
  135. b2->m_linearVelocity += b2->m_invMass * m_impulse * m_J.linearB;
  136. b2->m_angularVelocity += b2->m_invI * m_impulse * m_J.angularB;
  137. }
  138. else
  139. {
  140. m_impulse = 0.0f;
  141. }
  142. }
  143. void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step)
  144. {
  145. B2_NOT_USED(step);
  146. b2Body* b1 = m_bodyA;
  147. b2Body* b2 = m_bodyB;
  148. float32 Cdot = m_J.Compute( b1->m_linearVelocity, b1->m_angularVelocity,
  149. b2->m_linearVelocity, b2->m_angularVelocity);
  150. float32 impulse = m_mass * (-Cdot);
  151. m_impulse += impulse;
  152. b1->m_linearVelocity += b1->m_invMass * impulse * m_J.linearA;
  153. b1->m_angularVelocity += b1->m_invI * impulse * m_J.angularA;
  154. b2->m_linearVelocity += b2->m_invMass * impulse * m_J.linearB;
  155. b2->m_angularVelocity += b2->m_invI * impulse * m_J.angularB;
  156. }
  157. bool b2GearJoint::SolvePositionConstraints(float32 baumgarte)
  158. {
  159. B2_NOT_USED(baumgarte);
  160. float32 linearError = 0.0f;
  161. b2Body* b1 = m_bodyA;
  162. b2Body* b2 = m_bodyB;
  163. float32 coordinate1, coordinate2;
  164. if (m_revolute1)
  165. {
  166. coordinate1 = m_revolute1->GetJointAngle();
  167. }
  168. else
  169. {
  170. coordinate1 = m_prismatic1->GetJointTranslation();
  171. }
  172. if (m_revolute2)
  173. {
  174. coordinate2 = m_revolute2->GetJointAngle();
  175. }
  176. else
  177. {
  178. coordinate2 = m_prismatic2->GetJointTranslation();
  179. }
  180. float32 C = m_constant - (coordinate1 + m_ratio * coordinate2);
  181. float32 impulse = m_mass * (-C);
  182. b1->m_sweep.c += b1->m_invMass * impulse * m_J.linearA;
  183. b1->m_sweep.a += b1->m_invI * impulse * m_J.angularA;
  184. b2->m_sweep.c += b2->m_invMass * impulse * m_J.linearB;
  185. b2->m_sweep.a += b2->m_invI * impulse * m_J.angularB;
  186. b1->SynchronizeTransform();
  187. b2->SynchronizeTransform();
  188. // TODO_ERIN not implemented
  189. return linearError < b2_linearSlop;
  190. }
  191. b2Vec2 b2GearJoint::GetAnchorA() const
  192. {
  193. return m_bodyA->GetWorldPoint(m_localAnchor1);
  194. }
  195. b2Vec2 b2GearJoint::GetAnchorB() const
  196. {
  197. return m_bodyB->GetWorldPoint(m_localAnchor2);
  198. }
  199. b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const
  200. {
  201. // TODO_ERIN not tested
  202. b2Vec2 P = m_impulse * m_J.linearB;
  203. return inv_dt * P;
  204. }
  205. float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const
  206. {
  207. // TODO_ERIN not tested
  208. b2Vec2 r = b2Mul(m_bodyB->GetTransform().R, m_localAnchor2 - m_bodyB->GetLocalCenter());
  209. b2Vec2 P = m_impulse * m_J.linearB;
  210. float32 L = m_impulse * m_J.angularB - b2Cross(r, P);
  211. return inv_dt * L;
  212. }
  213. void b2GearJoint::SetRatio(float32 ratio)
  214. {
  215. b2Assert(b2IsValid(ratio));
  216. m_ratio = ratio;
  217. }
  218. float32 b2GearJoint::GetRatio() const
  219. {
  220. return m_ratio;
  221. }