1 // MIT License
2
3 // Copyright (c) 2019 Erin Catto
4
5 // Permission is hereby granted, free of charge, to any person obtaining a copy
6 // of this software and associated documentation files (the "Software"), to deal
7 // in the Software without restriction, including without limitation the rights
8 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 // copies of the Software, and to permit persons to whom the Software is
10 // furnished to do so, subject to the following conditions:
11
12 // The above copyright notice and this permission notice shall be included in all
13 // copies or substantial portions of the Software.
14
15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 // SOFTWARE.
22
23 #include "box2d/b2_body.h"
24 #include "box2d/b2_mouse_joint.h"
25 #include "box2d/b2_time_step.h"
26
27 // p = attached point, m = mouse point
28 // C = p - m
29 // Cdot = v
30 // = v + cross(w, r)
31 // J = [I r_skew]
32 // Identity used:
33 // w k % (rx i + ry j) = w * (-ry i + rx j)
34
b2MouseJoint(const b2MouseJointDef * def)35 b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
36 : b2Joint(def)
37 {
38 m_targetA = def->target;
39 m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);
40 m_maxForce = def->maxForce;
41 m_stiffness = def->stiffness;
42 m_damping = def->damping;
43
44 m_impulse.SetZero();
45 m_beta = 0.0f;
46 m_gamma = 0.0f;
47 }
48
SetTarget(const b2Vec2 & target)49 void b2MouseJoint::SetTarget(const b2Vec2& target)
50 {
51 if (target != m_targetA)
52 {
53 m_bodyB->SetAwake(true);
54 m_targetA = target;
55 }
56 }
57
GetTarget() const58 const b2Vec2& b2MouseJoint::GetTarget() const
59 {
60 return m_targetA;
61 }
62
SetMaxForce(float force)63 void b2MouseJoint::SetMaxForce(float force)
64 {
65 m_maxForce = force;
66 }
67
GetMaxForce() const68 float b2MouseJoint::GetMaxForce() const
69 {
70 return m_maxForce;
71 }
72
InitVelocityConstraints(const b2SolverData & data)73 void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
74 {
75 m_indexB = m_bodyB->m_islandIndex;
76 m_localCenterB = m_bodyB->m_sweep.localCenter;
77 m_invMassB = m_bodyB->m_invMass;
78 m_invIB = m_bodyB->m_invI;
79
80 b2Vec2 cB = data.positions[m_indexB].c;
81 float aB = data.positions[m_indexB].a;
82 b2Vec2 vB = data.velocities[m_indexB].v;
83 float wB = data.velocities[m_indexB].w;
84
85 b2Rot qB(aB);
86
87 float mass = m_bodyB->GetMass();
88
89 float d = m_damping;
90 float k = m_stiffness;
91
92 // magic formulas
93 // gamma has units of inverse mass.
94 // beta has units of inverse time.
95 float h = data.step.dt;
96 m_gamma = h * (d + h * k);
97 if (m_gamma != 0.0f)
98 {
99 m_gamma = 1.0f / m_gamma;
100 }
101 m_beta = h * k * m_gamma;
102
103 // Compute the effective mass matrix.
104 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
105
106 // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
107 // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
108 // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
109 b2Mat22 K;
110 K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
111 K.ex.y = -m_invIB * m_rB.x * m_rB.y;
112 K.ey.x = K.ex.y;
113 K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
114
115 m_mass = K.GetInverse();
116
117 m_C = cB + m_rB - m_targetA;
118 m_C *= m_beta;
119
120 // Cheat with some damping
121 wB *= 0.98f;
122
123 if (data.step.warmStarting)
124 {
125 m_impulse *= data.step.dtRatio;
126 vB += m_invMassB * m_impulse;
127 wB += m_invIB * b2Cross(m_rB, m_impulse);
128 }
129 else
130 {
131 m_impulse.SetZero();
132 }
133
134 data.velocities[m_indexB].v = vB;
135 data.velocities[m_indexB].w = wB;
136 }
137
SolveVelocityConstraints(const b2SolverData & data)138 void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)
139 {
140 b2Vec2 vB = data.velocities[m_indexB].v;
141 float wB = data.velocities[m_indexB].w;
142
143 // Cdot = v + cross(w, r)
144 b2Vec2 Cdot = vB + b2Cross(wB, m_rB);
145 b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse));
146
147 b2Vec2 oldImpulse = m_impulse;
148 m_impulse += impulse;
149 float maxImpulse = data.step.dt * m_maxForce;
150 if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
151 {
152 m_impulse *= maxImpulse / m_impulse.Length();
153 }
154 impulse = m_impulse - oldImpulse;
155
156 vB += m_invMassB * impulse;
157 wB += m_invIB * b2Cross(m_rB, impulse);
158
159 data.velocities[m_indexB].v = vB;
160 data.velocities[m_indexB].w = wB;
161 }
162
SolvePositionConstraints(const b2SolverData & data)163 bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data)
164 {
165 B2_NOT_USED(data);
166 return true;
167 }
168
GetAnchorA() const169 b2Vec2 b2MouseJoint::GetAnchorA() const
170 {
171 return m_targetA;
172 }
173
GetAnchorB() const174 b2Vec2 b2MouseJoint::GetAnchorB() const
175 {
176 return m_bodyB->GetWorldPoint(m_localAnchorB);
177 }
178
GetReactionForce(float inv_dt) const179 b2Vec2 b2MouseJoint::GetReactionForce(float inv_dt) const
180 {
181 return inv_dt * m_impulse;
182 }
183
GetReactionTorque(float inv_dt) const184 float b2MouseJoint::GetReactionTorque(float inv_dt) const
185 {
186 return inv_dt * 0.0f;
187 }
188
ShiftOrigin(const b2Vec2 & newOrigin)189 void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin)
190 {
191 m_targetA -= newOrigin;
192 }
193