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