1/*
2* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
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
19#include <Box2D/Dynamics/Joints/b2MouseJoint.h>
20#include <Box2D/Dynamics/b2Body.h>
21#include <Box2D/Dynamics/b2TimeStep.h>
22
23// p = attached point, m = mouse point
24// C = p - m
25// Cdot = v
26// = v + cross(w, r)
27// J = [I r_skew]
28// Identity used:
29// w k % (rx i + ry j) = w * (-ry i + rx j)
30
31b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
32: b2Joint(def)
33{
34 b2Assert(def->target.IsValid());
35 b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);
36 b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
37 b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
38
39 m_targetA = def->target;
40 m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);
41
42 m_maxForce = def->maxForce;
43 m_impulse.SetZero();
44
45 m_frequencyHz = def->frequencyHz;
46 m_dampingRatio = def->dampingRatio;
47
48 m_beta = 0.0f;
49 m_gamma = 0.0f;
50}
51
52void b2MouseJoint::SetTarget(const b2Vec2& target)
53{
54 if (m_bodyB->IsAwake() == false)
55 {
56 m_bodyB->SetAwake(true);
57 }
58 m_targetA = target;
59}
60
61const b2Vec2& b2MouseJoint::GetTarget() const
62{
63 return m_targetA;
64}
65
66void b2MouseJoint::SetMaxForce(float32 force)
67{
68 m_maxForce = force;
69}
70
71float32 b2MouseJoint::GetMaxForce() const
72{
73 return m_maxForce;
74}
75
76void b2MouseJoint::SetFrequency(float32 hz)
77{
78 m_frequencyHz = hz;
79}
80
81float32 b2MouseJoint::GetFrequency() const
82{
83 return m_frequencyHz;
84}
85
86void b2MouseJoint::SetDampingRatio(float32 ratio)
87{
88 m_dampingRatio = ratio;
89}
90
91float32 b2MouseJoint::GetDampingRatio() const
92{
93 return m_dampingRatio;
94}
95
96void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
97{
98 m_indexB = m_bodyB->m_islandIndex;
99 m_localCenterB = m_bodyB->m_sweep.localCenter;
100 m_invMassB = m_bodyB->m_invMass;
101 m_invIB = m_bodyB->m_invI;
102
103 b2Vec2 cB = data.positions[m_indexB].c;
104 float32 aB = data.positions[m_indexB].a;
105 b2Vec2 vB = data.velocities[m_indexB].v;
106 float32 wB = data.velocities[m_indexB].w;
107
108 b2Rot qB(aB);
109
110 float32 mass = m_bodyB->GetMass();
111
112 // Frequency
113 float32 omega = 2.0f * b2_pi * m_frequencyHz;
114
115 // Damping coefficient
116 float32 d = 2.0f * mass * m_dampingRatio * omega;
117
118 // Spring stiffness
119 float32 k = mass * (omega * omega);
120
121 // magic formulas
122 // gamma has units of inverse mass.
123 // beta has units of inverse time.
124 float32 h = data.step.dt;
125 b2Assert(d + h * k > b2_epsilon);
126 m_gamma = h * (d + h * k);
127 if (m_gamma != 0.0f)
128 {
129 m_gamma = 1.0f / m_gamma;
130 }
131 m_beta = h * k * m_gamma;
132
133 // Compute the effective mass matrix.
134 m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
135
136 // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
137 // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
138 // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
139 b2Mat22 K;
140 K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
141 K.ex.y = -m_invIB * m_rB.x * m_rB.y;
142 K.ey.x = K.ex.y;
143 K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
144
145 m_mass = K.GetInverse();
146
147 m_C = cB + m_rB - m_targetA;
148 m_C *= m_beta;
149
150 // Cheat with some damping
151 wB *= 0.98f;
152
153 if (data.step.warmStarting)
154 {
155 m_impulse *= data.step.dtRatio;
156 vB += m_invMassB * m_impulse;
157 wB += m_invIB * b2Cross(m_rB, m_impulse);
158 }
159 else
160 {
161 m_impulse.SetZero();
162 }
163
164 data.velocities[m_indexB].v = vB;
165 data.velocities[m_indexB].w = wB;
166}
167
168void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)
169{
170 b2Vec2 vB = data.velocities[m_indexB].v;
171 float32 wB = data.velocities[m_indexB].w;
172
173 // Cdot = v + cross(w, r)
174 b2Vec2 Cdot = vB + b2Cross(wB, m_rB);
175 b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse));
176
177 b2Vec2 oldImpulse = m_impulse;
178 m_impulse += impulse;
179 float32 maxImpulse = data.step.dt * m_maxForce;
180 if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
181 {
182 m_impulse *= maxImpulse / m_impulse.Length();
183 }
184 impulse = m_impulse - oldImpulse;
185
186 vB += m_invMassB * impulse;
187 wB += m_invIB * b2Cross(m_rB, impulse);
188
189 data.velocities[m_indexB].v = vB;
190 data.velocities[m_indexB].w = wB;
191}
192
193bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data)
194{
195 B2_NOT_USED(data);
196 return true;
197}
198
199b2Vec2 b2MouseJoint::GetAnchorA() const
200{
201 return m_targetA;
202}
203
204b2Vec2 b2MouseJoint::GetAnchorB() const
205{
206 return m_bodyB->GetWorldPoint(m_localAnchorB);
207}
208
209b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const
210{
211 return inv_dt * m_impulse;
212}
213
214float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
215{
216 return inv_dt * 0.0f;
217}
218
219void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin)
220{
221 m_targetA -= newOrigin;
222}
223