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_block_allocator.h"
24 #include "box2d/b2_body.h"
25 #include "box2d/b2_distance_joint.h"
26 #include "box2d/b2_draw.h"
27 #include "box2d/b2_friction_joint.h"
28 #include "box2d/b2_gear_joint.h"
29 #include "box2d/b2_motor_joint.h"
30 #include "box2d/b2_mouse_joint.h"
31 #include "box2d/b2_prismatic_joint.h"
32 #include "box2d/b2_pulley_joint.h"
33 #include "box2d/b2_revolute_joint.h"
34 #include "box2d/b2_weld_joint.h"
35 #include "box2d/b2_wheel_joint.h"
36 #include "box2d/b2_world.h"
37
38 #include <new>
39
b2LinearStiffness(float & stiffness,float & damping,float frequencyHertz,float dampingRatio,const b2Body * bodyA,const b2Body * bodyB)40 void b2LinearStiffness(float& stiffness, float& damping,
41 float frequencyHertz, float dampingRatio,
42 const b2Body* bodyA, const b2Body* bodyB)
43 {
44 float massA = bodyA->GetMass();
45 float massB = bodyB->GetMass();
46 float mass;
47 if (massA > 0.0f && massB > 0.0f)
48 {
49 mass = massA * massB / (massA + massB);
50 }
51 else if (massA > 0.0f)
52 {
53 mass = massA;
54 }
55 else
56 {
57 mass = massB;
58 }
59
60 float omega = 2.0f * b2_pi * frequencyHertz;
61 stiffness = mass * omega * omega;
62 damping = 2.0f * mass * dampingRatio * omega;
63 }
64
b2AngularStiffness(float & stiffness,float & damping,float frequencyHertz,float dampingRatio,const b2Body * bodyA,const b2Body * bodyB)65 void b2AngularStiffness(float& stiffness, float& damping,
66 float frequencyHertz, float dampingRatio,
67 const b2Body* bodyA, const b2Body* bodyB)
68 {
69 float IA = bodyA->GetInertia();
70 float IB = bodyB->GetInertia();
71 float I;
72 if (IA > 0.0f && IB > 0.0f)
73 {
74 I = IA * IB / (IA + IB);
75 }
76 else if (IA > 0.0f)
77 {
78 I = IA;
79 }
80 else
81 {
82 I = IB;
83 }
84
85 float omega = 2.0f * b2_pi * frequencyHertz;
86 stiffness = I * omega * omega;
87 damping = 2.0f * I * dampingRatio * omega;
88 }
89
Create(const b2JointDef * def,b2BlockAllocator * allocator)90 b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
91 {
92 b2Joint* joint = nullptr;
93
94 switch (def->type)
95 {
96 case e_distanceJoint:
97 {
98 void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
99 joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def));
100 }
101 break;
102
103 case e_mouseJoint:
104 {
105 void* mem = allocator->Allocate(sizeof(b2MouseJoint));
106 joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def));
107 }
108 break;
109
110 case e_prismaticJoint:
111 {
112 void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
113 joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def));
114 }
115 break;
116
117 case e_revoluteJoint:
118 {
119 void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
120 joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def));
121 }
122 break;
123
124 case e_pulleyJoint:
125 {
126 void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
127 joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def));
128 }
129 break;
130
131 case e_gearJoint:
132 {
133 void* mem = allocator->Allocate(sizeof(b2GearJoint));
134 joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def));
135 }
136 break;
137
138 case e_wheelJoint:
139 {
140 void* mem = allocator->Allocate(sizeof(b2WheelJoint));
141 joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def));
142 }
143 break;
144
145 case e_weldJoint:
146 {
147 void* mem = allocator->Allocate(sizeof(b2WeldJoint));
148 joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def));
149 }
150 break;
151
152 case e_frictionJoint:
153 {
154 void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
155 joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def));
156 }
157 break;
158
159 case e_motorJoint:
160 {
161 void* mem = allocator->Allocate(sizeof(b2MotorJoint));
162 joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(def));
163 }
164 break;
165
166 default:
167 b2Assert(false);
168 break;
169 }
170
171 return joint;
172 }
173
Destroy(b2Joint * joint,b2BlockAllocator * allocator)174 void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
175 {
176 joint->~b2Joint();
177 switch (joint->m_type)
178 {
179 case e_distanceJoint:
180 allocator->Free(joint, sizeof(b2DistanceJoint));
181 break;
182
183 case e_mouseJoint:
184 allocator->Free(joint, sizeof(b2MouseJoint));
185 break;
186
187 case e_prismaticJoint:
188 allocator->Free(joint, sizeof(b2PrismaticJoint));
189 break;
190
191 case e_revoluteJoint:
192 allocator->Free(joint, sizeof(b2RevoluteJoint));
193 break;
194
195 case e_pulleyJoint:
196 allocator->Free(joint, sizeof(b2PulleyJoint));
197 break;
198
199 case e_gearJoint:
200 allocator->Free(joint, sizeof(b2GearJoint));
201 break;
202
203 case e_wheelJoint:
204 allocator->Free(joint, sizeof(b2WheelJoint));
205 break;
206
207 case e_weldJoint:
208 allocator->Free(joint, sizeof(b2WeldJoint));
209 break;
210
211 case e_frictionJoint:
212 allocator->Free(joint, sizeof(b2FrictionJoint));
213 break;
214
215 case e_motorJoint:
216 allocator->Free(joint, sizeof(b2MotorJoint));
217 break;
218
219 default:
220 b2Assert(false);
221 break;
222 }
223 }
224
b2Joint(const b2JointDef * def)225 b2Joint::b2Joint(const b2JointDef* def)
226 {
227 b2Assert(def->bodyA != def->bodyB);
228
229 m_type = def->type;
230 m_prev = nullptr;
231 m_next = nullptr;
232 m_bodyA = def->bodyA;
233 m_bodyB = def->bodyB;
234 m_index = 0;
235 m_collideConnected = def->collideConnected;
236 m_islandFlag = false;
237 m_userData = def->userData;
238
239 m_edgeA.joint = nullptr;
240 m_edgeA.other = nullptr;
241 m_edgeA.prev = nullptr;
242 m_edgeA.next = nullptr;
243
244 m_edgeB.joint = nullptr;
245 m_edgeB.other = nullptr;
246 m_edgeB.prev = nullptr;
247 m_edgeB.next = nullptr;
248 }
249
IsEnabled() const250 bool b2Joint::IsEnabled() const
251 {
252 return m_bodyA->IsEnabled() && m_bodyB->IsEnabled();
253 }
254
Draw(b2Draw * draw) const255 void b2Joint::Draw(b2Draw* draw) const
256 {
257 const b2Transform& xf1 = m_bodyA->GetTransform();
258 const b2Transform& xf2 = m_bodyB->GetTransform();
259 b2Vec2 x1 = xf1.p;
260 b2Vec2 x2 = xf2.p;
261 b2Vec2 p1 = GetAnchorA();
262 b2Vec2 p2 = GetAnchorB();
263
264 b2Color color(0.5f, 0.8f, 0.8f);
265
266 switch (m_type)
267 {
268 case e_distanceJoint:
269 draw->DrawSegment(p1, p2, color);
270 break;
271
272 case e_pulleyJoint:
273 {
274 b2PulleyJoint* pulley = (b2PulleyJoint*)this;
275 b2Vec2 s1 = pulley->GetGroundAnchorA();
276 b2Vec2 s2 = pulley->GetGroundAnchorB();
277 draw->DrawSegment(s1, p1, color);
278 draw->DrawSegment(s2, p2, color);
279 draw->DrawSegment(s1, s2, color);
280 }
281 break;
282
283 case e_mouseJoint:
284 {
285 b2Color c;
286 c.Set(0.0f, 1.0f, 0.0f);
287 draw->DrawPoint(p1, 4.0f, c);
288 draw->DrawPoint(p2, 4.0f, c);
289
290 c.Set(0.8f, 0.8f, 0.8f);
291 draw->DrawSegment(p1, p2, c);
292
293 }
294 break;
295
296 default:
297 draw->DrawSegment(x1, p1, color);
298 draw->DrawSegment(p1, p2, color);
299 draw->DrawSegment(x2, p2, color);
300 }
301 }
302