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

游戏引擎

开发平台:

Visual C++

  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. #include <Box2D/Dynamics/Joints/b2MouseJoint.h>
  19. #include <Box2D/Dynamics/b2Body.h>
  20. #include <Box2D/Dynamics/b2TimeStep.h>
  21. // p = attached point, m = mouse point
  22. // C = p - m
  23. // Cdot = v
  24. //      = v + cross(w, r)
  25. // J = [I r_skew]
  26. // Identity used:
  27. // w k % (rx i + ry j) = w * (-ry i + rx j)
  28. b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
  29. : b2Joint(def)
  30. {
  31. b2Assert(def->target.IsValid());
  32. b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);
  33. b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
  34. b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
  35. m_target = def->target;
  36. m_localAnchor = b2MulT(m_bodyB->GetTransform(), m_target);
  37. m_maxForce = def->maxForce;
  38. m_impulse.SetZero();
  39. m_frequencyHz = def->frequencyHz;
  40. m_dampingRatio = def->dampingRatio;
  41. m_beta = 0.0f;
  42. m_gamma = 0.0f;
  43. }
  44. void b2MouseJoint::SetTarget(const b2Vec2& target)
  45. {
  46. if (m_bodyB->IsAwake() == false)
  47. {
  48. m_bodyB->SetAwake(true);
  49. }
  50. m_target = target;
  51. }
  52. const b2Vec2& b2MouseJoint::GetTarget() const
  53. {
  54. return m_target;
  55. }
  56. void b2MouseJoint::SetMaxForce(float32 force)
  57. {
  58. m_maxForce = force;
  59. }
  60. float32 b2MouseJoint::GetMaxForce() const
  61. {
  62. return m_maxForce;
  63. }
  64. void b2MouseJoint::SetFrequency(float32 hz)
  65. {
  66. m_frequencyHz = hz;
  67. }
  68. float32 b2MouseJoint::GetFrequency() const
  69. {
  70. return m_frequencyHz;
  71. }
  72. void b2MouseJoint::SetDampingRatio(float32 ratio)
  73. {
  74. m_dampingRatio = ratio;
  75. }
  76. float32 b2MouseJoint::GetDampingRatio() const
  77. {
  78. return m_dampingRatio;
  79. }
  80. void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
  81. {
  82. b2Body* b = m_bodyB;
  83. float32 mass = b->GetMass();
  84. // Frequency
  85. float32 omega = 2.0f * b2_pi * m_frequencyHz;
  86. // Damping coefficient
  87. float32 d = 2.0f * mass * m_dampingRatio * omega;
  88. // Spring stiffness
  89. float32 k = mass * (omega * omega);
  90. // magic formulas
  91. // gamma has units of inverse mass.
  92. // beta has units of inverse time.
  93. b2Assert(d + step.dt * k > b2_epsilon);
  94. m_gamma = step.dt * (d + step.dt * k);
  95. if (m_gamma != 0.0f)
  96. {
  97. m_gamma = 1.0f / m_gamma;
  98. }
  99. m_beta = step.dt * k * m_gamma;
  100. // Compute the effective mass matrix.
  101. b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter());
  102. // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
  103. //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
  104. //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
  105. float32 invMass = b->m_invMass;
  106. float32 invI = b->m_invI;
  107. b2Mat22 K1;
  108. K1.col1.x = invMass; K1.col2.x = 0.0f;
  109. K1.col1.y = 0.0f; K1.col2.y = invMass;
  110. b2Mat22 K2;
  111. K2.col1.x =  invI * r.y * r.y; K2.col2.x = -invI * r.x * r.y;
  112. K2.col1.y = -invI * r.x * r.y; K2.col2.y =  invI * r.x * r.x;
  113. b2Mat22 K = K1 + K2;
  114. K.col1.x += m_gamma;
  115. K.col2.y += m_gamma;
  116. m_mass = K.GetInverse();
  117. m_C = b->m_sweep.c + r - m_target;
  118. // Cheat with some damping
  119. b->m_angularVelocity *= 0.98f;
  120. // Warm starting.
  121. m_impulse *= step.dtRatio;
  122. b->m_linearVelocity += invMass * m_impulse;
  123. b->m_angularVelocity += invI * b2Cross(r, m_impulse);
  124. }
  125. void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step)
  126. {
  127. b2Body* b = m_bodyB;
  128. b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter());
  129. // Cdot = v + cross(w, r)
  130. b2Vec2 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r);
  131. b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_beta * m_C + m_gamma * m_impulse));
  132. b2Vec2 oldImpulse = m_impulse;
  133. m_impulse += impulse;
  134. float32 maxImpulse = step.dt * m_maxForce;
  135. if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
  136. {
  137. m_impulse *= maxImpulse / m_impulse.Length();
  138. }
  139. impulse = m_impulse - oldImpulse;
  140. b->m_linearVelocity += b->m_invMass * impulse;
  141. b->m_angularVelocity += b->m_invI * b2Cross(r, impulse);
  142. }
  143. b2Vec2 b2MouseJoint::GetAnchorA() const
  144. {
  145. return m_target;
  146. }
  147. b2Vec2 b2MouseJoint::GetAnchorB() const
  148. {
  149. return m_bodyB->GetWorldPoint(m_localAnchor);
  150. }
  151. b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const
  152. {
  153. return inv_dt * m_impulse;
  154. }
  155. float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
  156. {
  157. return inv_dt * 0.0f;
  158. }