1 //
2 // Copyright (c) 2008-2017 the Urho3D project.
3 //
4 // Permission is hereby granted, free of charge, to any person obtaining a copy
5 // of this software and associated documentation files (the "Software"), to deal
6 // in the Software without restriction, including without limitation the rights
7 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8 // copies of the Software, and to permit persons to whom the Software is
9 // furnished to do so, subject to the following conditions:
10 //
11 // The above copyright notice and this permission notice shall be included in
12 // all copies or substantial portions of the Software.
13 //
14 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20 // THE SOFTWARE.
21 //
22
23 #include "../Precompiled.h"
24
25 #include "../Core/Context.h"
26 #include "../Core/Profiler.h"
27 #include "../Graphics/DebugRenderer.h"
28 #include "../Graphics/Graphics.h"
29 #include "../Graphics/Renderer.h"
30 #include "../IO/Log.h"
31 #include "../Scene/Scene.h"
32 #include "../Scene/SceneEvents.h"
33 #include "../Urho2D/CollisionShape2D.h"
34 #include "../Urho2D/PhysicsEvents2D.h"
35 #include "../Urho2D/PhysicsUtils2D.h"
36 #include "../Urho2D/PhysicsWorld2D.h"
37 #include "../Urho2D/RigidBody2D.h"
38
39 #include "../DebugNew.h"
40
41 namespace Urho3D
42 {
43
44 extern const char* SUBSYSTEM_CATEGORY;
45 static const Vector2 DEFAULT_GRAVITY(0.0f, -9.81f);
46 static const int DEFAULT_VELOCITY_ITERATIONS = 8;
47 static const int DEFAULT_POSITION_ITERATIONS = 3;
48
PhysicsWorld2D(Context * context)49 PhysicsWorld2D::PhysicsWorld2D(Context* context) :
50 Component(context),
51 gravity_(DEFAULT_GRAVITY),
52 velocityIterations_(DEFAULT_VELOCITY_ITERATIONS),
53 positionIterations_(DEFAULT_POSITION_ITERATIONS),
54 debugRenderer_(0),
55 physicsStepping_(false),
56 applyingTransforms_(false),
57 updateEnabled_(true)
58 {
59 // Set default debug draw flags
60 m_drawFlags = e_shapeBit;
61
62 // Create Box2D world
63 world_ = new b2World(ToB2Vec2(gravity_));
64 // Set contact listener
65 world_->SetContactListener(this);
66 // Set debug draw
67 world_->SetDebugDraw(this);
68 }
69
~PhysicsWorld2D()70 PhysicsWorld2D::~PhysicsWorld2D()
71 {
72 for (unsigned i = 0; i < rigidBodies_.Size(); ++i)
73 if (rigidBodies_[i])
74 rigidBodies_[i]->ReleaseBody();
75 }
76
RegisterObject(Context * context)77 void PhysicsWorld2D::RegisterObject(Context* context)
78 {
79 context->RegisterFactory<PhysicsWorld2D>(SUBSYSTEM_CATEGORY);
80
81 URHO3D_ACCESSOR_ATTRIBUTE("Draw Shape", GetDrawShape, SetDrawShape, bool, false, AM_DEFAULT);
82 URHO3D_ACCESSOR_ATTRIBUTE("Draw Joint", GetDrawJoint, SetDrawJoint, bool, false, AM_DEFAULT);
83 URHO3D_ACCESSOR_ATTRIBUTE("Draw Aabb", GetDrawAabb, SetDrawAabb, bool, false, AM_DEFAULT);
84 URHO3D_ACCESSOR_ATTRIBUTE("Draw Pair", GetDrawPair, SetDrawPair, bool, false, AM_DEFAULT);
85 URHO3D_ACCESSOR_ATTRIBUTE("Draw CenterOfMass", GetDrawCenterOfMass, SetDrawCenterOfMass, bool, false, AM_DEFAULT);
86 URHO3D_ACCESSOR_ATTRIBUTE("Allow Sleeping", GetAllowSleeping, SetAllowSleeping, bool, false, AM_DEFAULT);
87 URHO3D_ACCESSOR_ATTRIBUTE("Warm Starting", GetWarmStarting, SetWarmStarting, bool, false, AM_DEFAULT);
88 URHO3D_ACCESSOR_ATTRIBUTE("Continuous Physics", GetContinuousPhysics, SetContinuousPhysics, bool, true, AM_DEFAULT);
89 URHO3D_ACCESSOR_ATTRIBUTE("Sub Stepping", GetSubStepping, SetSubStepping, bool, false, AM_DEFAULT);
90 URHO3D_ACCESSOR_ATTRIBUTE("Gravity", GetGravity, SetGravity, Vector2, DEFAULT_GRAVITY, AM_DEFAULT);
91 URHO3D_ACCESSOR_ATTRIBUTE("Auto Clear Forces", GetAutoClearForces, SetAutoClearForces, bool, false, AM_DEFAULT);
92 URHO3D_ACCESSOR_ATTRIBUTE("Velocity Iterations", GetVelocityIterations, SetVelocityIterations, int, DEFAULT_VELOCITY_ITERATIONS,
93 AM_DEFAULT);
94 URHO3D_ACCESSOR_ATTRIBUTE("Position Iterations", GetPositionIterations, SetPositionIterations, int, DEFAULT_POSITION_ITERATIONS,
95 AM_DEFAULT);
96 }
97
DrawDebugGeometry(DebugRenderer * debug,bool depthTest)98 void PhysicsWorld2D::DrawDebugGeometry(DebugRenderer* debug, bool depthTest)
99 {
100 if (debug)
101 {
102 URHO3D_PROFILE(Physics2DDrawDebug);
103
104 debugRenderer_ = debug;
105 debugDepthTest_ = depthTest;
106 world_->DrawDebugData();
107 debugRenderer_ = 0;
108 }
109 }
110
BeginContact(b2Contact * contact)111 void PhysicsWorld2D::BeginContact(b2Contact* contact)
112 {
113 // Only handle contact event while stepping the physics simulation
114 if (!physicsStepping_)
115 return;
116
117 b2Fixture* fixtureA = contact->GetFixtureA();
118 b2Fixture* fixtureB = contact->GetFixtureB();
119 if (!fixtureA || !fixtureB)
120 return;
121
122 beginContactInfos_.Push(ContactInfo(contact));
123 }
124
EndContact(b2Contact * contact)125 void PhysicsWorld2D::EndContact(b2Contact* contact)
126 {
127 if (!physicsStepping_)
128 return;
129
130 b2Fixture* fixtureA = contact->GetFixtureA();
131 b2Fixture* fixtureB = contact->GetFixtureB();
132 if (!fixtureA || !fixtureB)
133 return;
134
135 endContactInfos_.Push(ContactInfo(contact));
136 }
137
PreSolve(b2Contact * contact,const b2Manifold * oldManifold)138 void PhysicsWorld2D::PreSolve(b2Contact* contact, const b2Manifold* oldManifold)
139 {
140 b2Fixture* fixtureA = contact->GetFixtureA();
141 b2Fixture* fixtureB = contact->GetFixtureB();
142 if (!fixtureA || !fixtureB)
143 return;
144
145 ContactInfo contactInfo(contact);
146
147 // Send global event
148 VariantMap& eventData = GetEventDataMap();
149 eventData[PhysicsUpdateContact2D::P_WORLD] = this;
150 eventData[PhysicsUpdateContact2D::P_ENABLED] = contact->IsEnabled();
151
152 eventData[PhysicsUpdateContact2D::P_BODYA] = contactInfo.bodyA_.Get();
153 eventData[PhysicsUpdateContact2D::P_BODYB] = contactInfo.bodyB_.Get();
154 eventData[PhysicsUpdateContact2D::P_NODEA] = contactInfo.nodeA_.Get();
155 eventData[PhysicsUpdateContact2D::P_NODEB] = contactInfo.nodeB_.Get();
156 eventData[PhysicsUpdateContact2D::P_CONTACTS] = contactInfo.Serialize(contacts_);
157 eventData[PhysicsUpdateContact2D::P_SHAPEA] = contactInfo.shapeA_.Get();
158 eventData[PhysicsUpdateContact2D::P_SHAPEB] = contactInfo.shapeB_.Get();
159
160 SendEvent(E_PHYSICSUPDATECONTACT2D, eventData);
161 contact->SetEnabled(eventData[PhysicsUpdateContact2D::P_ENABLED].GetBool());
162 eventData.Clear();
163
164 // Send node event
165 eventData[NodeUpdateContact2D::P_ENABLED] = contact->IsEnabled();
166 eventData[NodeUpdateContact2D::P_CONTACTS] = contactInfo.Serialize(contacts_);
167
168 if (contactInfo.nodeA_)
169 {
170 eventData[NodeUpdateContact2D::P_BODY] = contactInfo.bodyA_.Get();
171 eventData[NodeUpdateContact2D::P_OTHERNODE] = contactInfo.nodeB_.Get();
172 eventData[NodeUpdateContact2D::P_OTHERBODY] = contactInfo.bodyB_.Get();
173 eventData[NodeUpdateContact2D::P_SHAPE] = contactInfo.shapeA_.Get();
174 eventData[NodeUpdateContact2D::P_OTHERSHAPE] = contactInfo.shapeB_.Get();
175
176 contactInfo.nodeA_->SendEvent(E_NODEUPDATECONTACT2D, eventData);
177 }
178
179 if (contactInfo.nodeB_)
180 {
181 eventData[NodeUpdateContact2D::P_BODY] = contactInfo.bodyB_.Get();
182 eventData[NodeUpdateContact2D::P_OTHERNODE] = contactInfo.nodeA_.Get();
183 eventData[NodeUpdateContact2D::P_OTHERBODY] = contactInfo.bodyA_.Get();
184 eventData[NodeUpdateContact2D::P_SHAPE] = contactInfo.shapeB_.Get();
185 eventData[NodeUpdateContact2D::P_OTHERSHAPE] = contactInfo.shapeA_.Get();
186
187 contactInfo.nodeB_->SendEvent(E_NODEUPDATECONTACT2D, eventData);
188 }
189
190 contact->SetEnabled(eventData[NodeUpdateContact2D::P_ENABLED].GetBool());
191 }
192
DrawPolygon(const b2Vec2 * vertices,int32 vertexCount,const b2Color & color)193 void PhysicsWorld2D::DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color)
194 {
195 if (!debugRenderer_)
196 return;
197
198 Color c = ToColor(color);
199 for (int i = 0; i < vertexCount - 1; ++i)
200 debugRenderer_->AddLine(ToVector3(vertices[i]), ToVector3(vertices[i + 1]), c, debugDepthTest_);
201
202 debugRenderer_->AddLine(ToVector3(vertices[vertexCount - 1]), ToVector3(vertices[0]), c, debugDepthTest_);
203 }
204
DrawSolidPolygon(const b2Vec2 * vertices,int32 vertexCount,const b2Color & color)205 void PhysicsWorld2D::DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color)
206 {
207 if (!debugRenderer_)
208 return;
209
210 Vector3 v = ToVector3(vertices[0]);
211 Color c(color.r, color.g, color.b, 0.5f);
212 for (int i = 1; i < vertexCount - 1; ++i)
213 debugRenderer_->AddTriangle(v, ToVector3(vertices[i]), ToVector3(vertices[i + 1]), c, debugDepthTest_);
214 }
215
DrawCircle(const b2Vec2 & center,float32 radius,const b2Color & color)216 void PhysicsWorld2D::DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color)
217 {
218 if (!debugRenderer_)
219 return;
220
221 Vector3 p = ToVector3(center);
222 Color c = ToColor(color);
223 for (unsigned i = 0; i < 360; i += 30)
224 {
225 unsigned j = i + 30;
226 float x1 = radius * Cos((float)i);
227 float y1 = radius * Sin((float)i);
228 float x2 = radius * Cos((float)j);
229 float y2 = radius * Sin((float)j);
230
231 debugRenderer_->AddLine(p + Vector3(x1, y1, 0.0f), p + Vector3(x2, y2, 0.0f), c, debugDepthTest_);
232 }
233 }
234
235 extern URHO3D_API const float PIXEL_SIZE;
236
DrawPoint(const b2Vec2 & p,float32 size,const b2Color & color)237 void PhysicsWorld2D::DrawPoint(const b2Vec2& p, float32 size, const b2Color& color)
238 {
239 DrawSolidCircle(p, size * 0.5f * PIXEL_SIZE, b2Vec2(), color);
240 }
241
DrawSolidCircle(const b2Vec2 & center,float32 radius,const b2Vec2 & axis,const b2Color & color)242 void PhysicsWorld2D::DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color)
243 {
244 if (!debugRenderer_)
245 return;
246
247 Vector3 p = ToVector3(center);
248 Color c(color.r, color.g, color.b, 0.5f);
249
250 for (unsigned i = 0; i < 360; i += 30)
251 {
252 unsigned j = i + 30;
253 float x1 = radius * Cos((float)i);
254 float y1 = radius * Sin((float)i);
255 float x2 = radius * Cos((float)j);
256 float y2 = radius * Sin((float)j);
257
258 debugRenderer_->AddTriangle(p, p + Vector3(x1, y1, 0.0f), p + Vector3(x2, y2, 0.0f), c, debugDepthTest_);
259 }
260 }
261
DrawSegment(const b2Vec2 & p1,const b2Vec2 & p2,const b2Color & color)262 void PhysicsWorld2D::DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color)
263 {
264 if (debugRenderer_)
265 debugRenderer_->AddLine(ToVector3(p1), ToVector3(p2), ToColor(color), debugDepthTest_);
266 }
267
DrawTransform(const b2Transform & xf)268 void PhysicsWorld2D::DrawTransform(const b2Transform& xf)
269 {
270 if (!debugRenderer_)
271 return;
272
273 const float32 axisScale = 0.4f;
274
275 b2Vec2 p1 = xf.p, p2;
276 p2 = p1 + axisScale * xf.q.GetXAxis();
277 debugRenderer_->AddLine(Vector3(p1.x, p1.y, 0.0f), Vector3(p2.x, p2.y, 0.0f), Color::RED, debugDepthTest_);
278
279 p2 = p1 + axisScale * xf.q.GetYAxis();
280 debugRenderer_->AddLine(Vector3(p1.x, p1.y, 0.0f), Vector3(p2.x, p2.y, 0.0f), Color::GREEN, debugDepthTest_);
281 }
282
Update(float timeStep)283 void PhysicsWorld2D::Update(float timeStep)
284 {
285 URHO3D_PROFILE(UpdatePhysics2D);
286
287 using namespace PhysicsPreStep;
288
289 VariantMap& eventData = GetEventDataMap();
290 eventData[P_WORLD] = this;
291 eventData[P_TIMESTEP] = timeStep;
292 SendEvent(E_PHYSICSPRESTEP, eventData);
293
294 physicsStepping_ = true;
295 world_->Step(timeStep, velocityIterations_, positionIterations_);
296 physicsStepping_ = false;
297
298 // Apply world transforms. Unparented transforms first
299 for (unsigned i = 0; i < rigidBodies_.Size();)
300 {
301 if (rigidBodies_[i])
302 {
303 rigidBodies_[i]->ApplyWorldTransform();
304 ++i;
305 }
306 else
307 {
308 // Erase possible stale weak pointer
309 rigidBodies_.Erase(i);
310 }
311 }
312
313 // Apply delayed (parented) world transforms now, if any
314 while (!delayedWorldTransforms_.Empty())
315 {
316 for (HashMap<RigidBody2D*, DelayedWorldTransform2D>::Iterator i = delayedWorldTransforms_.Begin();
317 i != delayedWorldTransforms_.End();)
318 {
319 const DelayedWorldTransform2D& transform = i->second_;
320
321 // If parent's transform has already been assigned, can proceed
322 if (!delayedWorldTransforms_.Contains(transform.parentRigidBody_))
323 {
324 transform.rigidBody_->ApplyWorldTransform(transform.worldPosition_, transform.worldRotation_);
325 i = delayedWorldTransforms_.Erase(i);
326 }
327 else
328 ++i;
329 }
330 }
331
332 SendBeginContactEvents();
333 SendEndContactEvents();
334
335 using namespace PhysicsPostStep;
336 SendEvent(E_PHYSICSPOSTSTEP, eventData);
337 }
338
DrawDebugGeometry()339 void PhysicsWorld2D::DrawDebugGeometry()
340 {
341 DebugRenderer* debug = GetComponent<DebugRenderer>();
342 if (debug)
343 DrawDebugGeometry(debug, false);
344 }
345
SetUpdateEnabled(bool enable)346 void PhysicsWorld2D::SetUpdateEnabled(bool enable)
347 {
348 updateEnabled_ = enable;
349 }
350
SetDrawShape(bool drawShape)351 void PhysicsWorld2D::SetDrawShape(bool drawShape)
352 {
353 if (drawShape)
354 m_drawFlags |= e_shapeBit;
355 else
356 m_drawFlags &= ~e_shapeBit;
357
358 }
359
SetDrawJoint(bool drawJoint)360 void PhysicsWorld2D::SetDrawJoint(bool drawJoint)
361 {
362 if (drawJoint)
363 m_drawFlags |= e_jointBit;
364 else
365 m_drawFlags &= ~e_jointBit;
366 }
367
SetDrawAabb(bool drawAabb)368 void PhysicsWorld2D::SetDrawAabb(bool drawAabb)
369 {
370 if (drawAabb)
371 m_drawFlags |= e_aabbBit;
372 else
373 m_drawFlags &= ~e_aabbBit;
374 }
375
SetDrawPair(bool drawPair)376 void PhysicsWorld2D::SetDrawPair(bool drawPair)
377 {
378 if (drawPair)
379 m_drawFlags |= e_pairBit;
380 else
381 m_drawFlags &= ~e_pairBit;
382 }
383
SetDrawCenterOfMass(bool drawCenterOfMass)384 void PhysicsWorld2D::SetDrawCenterOfMass(bool drawCenterOfMass)
385 {
386 if (drawCenterOfMass)
387 m_drawFlags |= e_centerOfMassBit;
388 else
389 m_drawFlags &= ~e_centerOfMassBit;
390 }
391
SetAllowSleeping(bool enable)392 void PhysicsWorld2D::SetAllowSleeping(bool enable)
393 {
394 world_->SetAllowSleeping(enable);
395 }
396
SetWarmStarting(bool enable)397 void PhysicsWorld2D::SetWarmStarting(bool enable)
398 {
399 world_->SetWarmStarting(enable);
400 }
401
SetContinuousPhysics(bool enable)402 void PhysicsWorld2D::SetContinuousPhysics(bool enable)
403 {
404 world_->SetContinuousPhysics(enable);
405 }
406
SetSubStepping(bool enable)407 void PhysicsWorld2D::SetSubStepping(bool enable)
408 {
409 world_->SetSubStepping(enable);
410 }
411
SetGravity(const Vector2 & gravity)412 void PhysicsWorld2D::SetGravity(const Vector2& gravity)
413 {
414 gravity_ = gravity;
415
416 world_->SetGravity(ToB2Vec2(gravity_));
417 }
418
SetAutoClearForces(bool enable)419 void PhysicsWorld2D::SetAutoClearForces(bool enable)
420 {
421 world_->SetAutoClearForces(enable);
422 }
423
SetVelocityIterations(int velocityIterations)424 void PhysicsWorld2D::SetVelocityIterations(int velocityIterations)
425 {
426 velocityIterations_ = velocityIterations;
427 }
428
SetPositionIterations(int positionIterations)429 void PhysicsWorld2D::SetPositionIterations(int positionIterations)
430 {
431 positionIterations_ = positionIterations;
432 }
433
AddRigidBody(RigidBody2D * rigidBody)434 void PhysicsWorld2D::AddRigidBody(RigidBody2D* rigidBody)
435 {
436 if (!rigidBody)
437 return;
438
439 WeakPtr<RigidBody2D> rigidBodyPtr(rigidBody);
440 if (rigidBodies_.Contains(rigidBodyPtr))
441 return;
442
443 rigidBodies_.Push(rigidBodyPtr);
444 }
445
RemoveRigidBody(RigidBody2D * rigidBody)446 void PhysicsWorld2D::RemoveRigidBody(RigidBody2D* rigidBody)
447 {
448 if (!rigidBody)
449 return;
450
451 WeakPtr<RigidBody2D> rigidBodyPtr(rigidBody);
452 rigidBodies_.Remove(rigidBodyPtr);
453 }
454
AddDelayedWorldTransform(const DelayedWorldTransform2D & transform)455 void PhysicsWorld2D::AddDelayedWorldTransform(const DelayedWorldTransform2D& transform)
456 {
457 delayedWorldTransforms_[transform.rigidBody_] = transform;
458 }
459
460 // Ray cast call back class.
461 class RayCastCallback : public b2RayCastCallback
462 {
463 public:
464 // Construct.
RayCastCallback(PODVector<PhysicsRaycastResult2D> & results,const Vector2 & startPoint,unsigned collisionMask)465 RayCastCallback(PODVector<PhysicsRaycastResult2D>& results, const Vector2& startPoint, unsigned collisionMask) :
466 results_(results),
467 startPoint_(startPoint),
468 collisionMask_(collisionMask)
469 {
470 }
471
472 // Called for each fixture found in the query.
ReportFixture(b2Fixture * fixture,const b2Vec2 & point,const b2Vec2 & normal,float32 fraction)473 virtual float32 ReportFixture(b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float32 fraction)
474 {
475 // Ignore sensor
476 if (fixture->IsSensor())
477 return true;
478
479 if ((fixture->GetFilterData().maskBits & collisionMask_) == 0)
480 return true;
481
482 PhysicsRaycastResult2D result;
483 result.position_ = ToVector2(point);
484 result.normal_ = ToVector2(normal);
485 result.distance_ = (result.position_ - startPoint_).Length();
486 result.body_ = (RigidBody2D*)(fixture->GetBody()->GetUserData());
487
488 results_.Push(result);
489 return true;
490 }
491
492 protected:
493 // Physics raycast results.
494 PODVector<PhysicsRaycastResult2D>& results_;
495 // Start point.
496 Vector2 startPoint_;
497 // Collision mask.
498 unsigned collisionMask_;
499 };
500
Raycast(PODVector<PhysicsRaycastResult2D> & results,const Vector2 & startPoint,const Vector2 & endPoint,unsigned collisionMask)501 void PhysicsWorld2D::Raycast(PODVector<PhysicsRaycastResult2D>& results, const Vector2& startPoint, const Vector2& endPoint,
502 unsigned collisionMask)
503 {
504 results.Clear();
505
506 RayCastCallback callback(results, startPoint, collisionMask);
507 world_->RayCast(&callback, ToB2Vec2(startPoint), ToB2Vec2(endPoint));
508 }
509
510 // Single ray cast call back class.
511 class SingleRayCastCallback : public b2RayCastCallback
512 {
513 public:
514 // Construct.
SingleRayCastCallback(PhysicsRaycastResult2D & result,const Vector2 & startPoint,unsigned collisionMask)515 SingleRayCastCallback(PhysicsRaycastResult2D& result, const Vector2& startPoint, unsigned collisionMask) :
516 result_(result),
517 startPoint_(startPoint),
518 collisionMask_(collisionMask),
519 minDistance_(M_INFINITY)
520 {
521 }
522
523 // Called for each fixture found in the query.
ReportFixture(b2Fixture * fixture,const b2Vec2 & point,const b2Vec2 & normal,float32 fraction)524 virtual float32 ReportFixture(b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float32 fraction)
525 {
526 // Ignore sensor
527 if (fixture->IsSensor())
528 return true;
529
530 if ((fixture->GetFilterData().maskBits & collisionMask_) == 0)
531 return true;
532
533 float distance = (ToVector2(point) - startPoint_).Length();
534 if (distance < minDistance_)
535 {
536 minDistance_ = distance;
537
538 result_.position_ = ToVector2(point);
539 result_.normal_ = ToVector2(normal);
540 result_.distance_ = distance;
541 result_.body_ = (RigidBody2D*)(fixture->GetBody()->GetUserData());
542 }
543
544 return true;
545 }
546
547 private:
548 // Physics raycast result.
549 PhysicsRaycastResult2D& result_;
550 // Start point.
551 Vector2 startPoint_;
552 // Collision mask.
553 unsigned collisionMask_;
554 // Minimum distance.
555 float minDistance_;
556 };
557
RaycastSingle(PhysicsRaycastResult2D & result,const Vector2 & startPoint,const Vector2 & endPoint,unsigned collisionMask)558 void PhysicsWorld2D::RaycastSingle(PhysicsRaycastResult2D& result, const Vector2& startPoint, const Vector2& endPoint,
559 unsigned collisionMask)
560 {
561 result.body_ = 0;
562
563 SingleRayCastCallback callback(result, startPoint, collisionMask);
564 world_->RayCast(&callback, ToB2Vec2(startPoint), ToB2Vec2(endPoint));
565 }
566
567 // Point query callback class.
568 class PointQueryCallback : public b2QueryCallback
569 {
570 public:
571 // Construct.
PointQueryCallback(const b2Vec2 & point,unsigned collisionMask)572 PointQueryCallback(const b2Vec2& point, unsigned collisionMask) :
573 point_(point),
574 collisionMask_(collisionMask),
575 rigidBody_(0)
576 {
577 }
578
579 // Called for each fixture found in the query AABB.
ReportFixture(b2Fixture * fixture)580 virtual bool ReportFixture(b2Fixture* fixture)
581 {
582 // Ignore sensor
583 if (fixture->IsSensor())
584 return true;
585
586 if ((fixture->GetFilterData().maskBits & collisionMask_) == 0)
587 return true;
588
589 if (fixture->TestPoint(point_))
590 {
591 rigidBody_ = (RigidBody2D*)(fixture->GetBody()->GetUserData());
592 return false;
593 }
594
595 return true;
596 }
597
598 // Return rigid body.
GetRigidBody() const599 RigidBody2D* GetRigidBody() const { return rigidBody_; }
600
601 private:
602 // Point.
603 b2Vec2 point_;
604 // Collision mask.
605 unsigned collisionMask_;
606 // Rigid body.
607 RigidBody2D* rigidBody_;
608 };
609
GetRigidBody(const Vector2 & point,unsigned collisionMask)610 RigidBody2D* PhysicsWorld2D::GetRigidBody(const Vector2& point, unsigned collisionMask)
611 {
612 PointQueryCallback callback(ToB2Vec2(point), collisionMask);
613
614 b2AABB b2Aabb;
615 Vector2 delta(M_EPSILON, M_EPSILON);
616 b2Aabb.lowerBound = ToB2Vec2(point - delta);
617 b2Aabb.upperBound = ToB2Vec2(point + delta);
618
619 world_->QueryAABB(&callback, b2Aabb);
620 return callback.GetRigidBody();
621 }
622
GetRigidBody(int screenX,int screenY,unsigned collisionMask)623 RigidBody2D* PhysicsWorld2D::GetRigidBody(int screenX, int screenY, unsigned collisionMask)
624 {
625 Renderer* renderer = GetSubsystem<Renderer>();
626 for (unsigned i = 0; i < renderer->GetNumViewports(); ++i)
627 {
628 Viewport* viewport = renderer->GetViewport(i);
629 // Find a viewport with same scene
630 if (viewport && viewport->GetScene() == GetScene())
631 {
632 Vector3 worldPoint = viewport->ScreenToWorldPoint(screenX, screenY, 0.0f);
633 return GetRigidBody(Vector2(worldPoint.x_, worldPoint.y_), collisionMask);
634 }
635 }
636
637 return 0;
638 }
639
640 // Aabb query callback class.
641 class AabbQueryCallback : public b2QueryCallback
642 {
643 public:
644 // Construct.
AabbQueryCallback(PODVector<RigidBody2D * > & results,unsigned collisionMask)645 AabbQueryCallback(PODVector<RigidBody2D*>& results, unsigned collisionMask) :
646 results_(results),
647 collisionMask_(collisionMask)
648 {
649 }
650
651 // Called for each fixture found in the query AABB.
ReportFixture(b2Fixture * fixture)652 virtual bool ReportFixture(b2Fixture* fixture)
653 {
654 // Ignore sensor
655 if (fixture->IsSensor())
656 return true;
657
658 if ((fixture->GetFilterData().maskBits & collisionMask_) == 0)
659 return true;
660
661 results_.Push((RigidBody2D*)(fixture->GetBody()->GetUserData()));
662 return true;
663 }
664
665 private:
666 // Results.
667 PODVector<RigidBody2D*>& results_;
668 // Collision mask.
669 unsigned collisionMask_;
670 };
671
GetRigidBodies(PODVector<RigidBody2D * > & results,const Rect & aabb,unsigned collisionMask)672 void PhysicsWorld2D::GetRigidBodies(PODVector<RigidBody2D*>& results, const Rect& aabb, unsigned collisionMask)
673 {
674 AabbQueryCallback callback(results, collisionMask);
675
676 b2AABB b2Aabb;
677 Vector2 delta(M_EPSILON, M_EPSILON);
678 b2Aabb.lowerBound = ToB2Vec2(aabb.min_ - delta);
679 b2Aabb.upperBound = ToB2Vec2(aabb.max_ + delta);
680
681 world_->QueryAABB(&callback, b2Aabb);
682 }
683
GetAllowSleeping() const684 bool PhysicsWorld2D::GetAllowSleeping() const
685 {
686 return world_->GetAllowSleeping();
687 }
688
GetWarmStarting() const689 bool PhysicsWorld2D::GetWarmStarting() const
690 {
691 return world_->GetWarmStarting();
692 }
693
GetContinuousPhysics() const694 bool PhysicsWorld2D::GetContinuousPhysics() const
695 {
696 return world_->GetContinuousPhysics();
697 }
698
GetSubStepping() const699 bool PhysicsWorld2D::GetSubStepping() const
700 {
701 return world_->GetSubStepping();
702 }
703
GetAutoClearForces() const704 bool PhysicsWorld2D::GetAutoClearForces() const
705 {
706 return world_->GetAutoClearForces();
707 }
708
OnSceneSet(Scene * scene)709 void PhysicsWorld2D::OnSceneSet(Scene* scene)
710 {
711 // Subscribe to the scene subsystem update, which will trigger the physics simulation step
712 if (scene)
713 SubscribeToEvent(scene, E_SCENESUBSYSTEMUPDATE, URHO3D_HANDLER(PhysicsWorld2D, HandleSceneSubsystemUpdate));
714 else
715 UnsubscribeFromEvent(E_SCENESUBSYSTEMUPDATE);
716 }
717
HandleSceneSubsystemUpdate(StringHash eventType,VariantMap & eventData)718 void PhysicsWorld2D::HandleSceneSubsystemUpdate(StringHash eventType, VariantMap& eventData)
719 {
720 if (!updateEnabled_)
721 return;
722
723 using namespace SceneSubsystemUpdate;
724 Update(eventData[P_TIMESTEP].GetFloat());
725 }
726
SendBeginContactEvents()727 void PhysicsWorld2D::SendBeginContactEvents()
728 {
729 if (beginContactInfos_.Empty())
730 return;
731
732 using namespace PhysicsBeginContact2D;
733 VariantMap& eventData = GetEventDataMap();
734 VariantMap nodeEventData;
735 eventData[P_WORLD] = this;
736
737 for (unsigned i = 0; i < beginContactInfos_.Size(); ++i)
738 {
739 ContactInfo& contactInfo = beginContactInfos_[i];
740 eventData[P_BODYA] = contactInfo.bodyA_.Get();
741 eventData[P_BODYB] = contactInfo.bodyB_.Get();
742 eventData[P_NODEA] = contactInfo.nodeA_.Get();
743 eventData[P_NODEB] = contactInfo.nodeB_.Get();
744 eventData[P_CONTACTS] = contactInfo.Serialize(contacts_);
745 eventData[P_SHAPEA] = contactInfo.shapeA_.Get();
746 eventData[P_SHAPEB] = contactInfo.shapeB_.Get();
747
748 SendEvent(E_PHYSICSBEGINCONTACT2D, eventData);
749
750 nodeEventData[NodeBeginContact2D::P_CONTACTS] = contactInfo.Serialize(contacts_);
751
752 if (contactInfo.nodeA_)
753 {
754 nodeEventData[NodeBeginContact2D::P_BODY] = contactInfo.bodyA_.Get();
755 nodeEventData[NodeBeginContact2D::P_OTHERNODE] = contactInfo.nodeB_.Get();
756 nodeEventData[NodeBeginContact2D::P_OTHERBODY] = contactInfo.bodyB_.Get();
757 nodeEventData[NodeBeginContact2D::P_SHAPE] = contactInfo.shapeA_.Get();
758 nodeEventData[NodeBeginContact2D::P_OTHERSHAPE] = contactInfo.shapeB_.Get();
759
760 contactInfo.nodeA_->SendEvent(E_NODEBEGINCONTACT2D, nodeEventData);
761 }
762
763 if (contactInfo.nodeB_)
764 {
765 nodeEventData[NodeBeginContact2D::P_BODY] = contactInfo.bodyB_.Get();
766 nodeEventData[NodeBeginContact2D::P_OTHERNODE] = contactInfo.nodeA_.Get();
767 nodeEventData[NodeBeginContact2D::P_OTHERBODY] = contactInfo.bodyA_.Get();
768 nodeEventData[NodeBeginContact2D::P_SHAPE] = contactInfo.shapeB_.Get();
769 nodeEventData[NodeBeginContact2D::P_OTHERSHAPE] = contactInfo.shapeA_.Get();
770
771 contactInfo.nodeB_->SendEvent(E_NODEBEGINCONTACT2D, nodeEventData);
772 }
773 }
774
775 beginContactInfos_.Clear();
776 }
777
SendEndContactEvents()778 void PhysicsWorld2D::SendEndContactEvents()
779 {
780 if (endContactInfos_.Empty())
781 return;
782
783 using namespace PhysicsEndContact2D;
784 VariantMap& eventData = GetEventDataMap();
785 VariantMap nodeEventData;
786 eventData[P_WORLD] = this;
787
788 for (unsigned i = 0; i < endContactInfos_.Size(); ++i)
789 {
790 ContactInfo& contactInfo = endContactInfos_[i];
791 eventData[P_BODYA] = contactInfo.bodyA_.Get();
792 eventData[P_BODYB] = contactInfo.bodyB_.Get();
793 eventData[P_NODEA] = contactInfo.nodeA_.Get();
794 eventData[P_NODEB] = contactInfo.nodeB_.Get();
795 eventData[P_CONTACTS] = contactInfo.Serialize(contacts_);
796 eventData[P_SHAPEA] = contactInfo.shapeA_.Get();
797 eventData[P_SHAPEB] = contactInfo.shapeB_.Get();
798
799 SendEvent(E_PHYSICSENDCONTACT2D, eventData);
800
801 nodeEventData[NodeEndContact2D::P_CONTACTS] = contactInfo.Serialize(contacts_);
802
803 if (contactInfo.nodeA_)
804 {
805 nodeEventData[NodeEndContact2D::P_BODY] = contactInfo.bodyA_.Get();
806 nodeEventData[NodeEndContact2D::P_OTHERNODE] = contactInfo.nodeB_.Get();
807 nodeEventData[NodeEndContact2D::P_OTHERBODY] = contactInfo.bodyB_.Get();
808 nodeEventData[NodeEndContact2D::P_SHAPE] = contactInfo.shapeA_.Get();
809 nodeEventData[NodeEndContact2D::P_OTHERSHAPE] = contactInfo.shapeB_.Get();
810
811 contactInfo.nodeA_->SendEvent(E_NODEENDCONTACT2D, nodeEventData);
812 }
813
814 if (contactInfo.nodeB_)
815 {
816 nodeEventData[NodeEndContact2D::P_BODY] = contactInfo.bodyB_.Get();
817 nodeEventData[NodeEndContact2D::P_OTHERNODE] = contactInfo.nodeA_.Get();
818 nodeEventData[NodeEndContact2D::P_OTHERBODY] = contactInfo.bodyA_.Get();
819 nodeEventData[NodeEndContact2D::P_SHAPE] = contactInfo.shapeB_.Get();
820 nodeEventData[NodeEndContact2D::P_OTHERSHAPE] = contactInfo.shapeA_.Get();
821
822 contactInfo.nodeB_->SendEvent(E_NODEENDCONTACT2D, nodeEventData);
823 }
824 }
825
826 endContactInfos_.Clear();
827 }
828
ContactInfo()829 PhysicsWorld2D::ContactInfo::ContactInfo()
830 {
831 }
832
ContactInfo(b2Contact * contact)833 PhysicsWorld2D::ContactInfo::ContactInfo(b2Contact* contact)
834 {
835 b2Fixture* fixtureA = contact->GetFixtureA();
836 b2Fixture* fixtureB = contact->GetFixtureB();
837 bodyA_ = (RigidBody2D*)(fixtureA->GetBody()->GetUserData());
838 bodyB_ = (RigidBody2D*)(fixtureB->GetBody()->GetUserData());
839 nodeA_ = bodyA_->GetNode();
840 nodeB_ = bodyB_->GetNode();
841 shapeA_ = (CollisionShape2D*)fixtureA->GetUserData();
842 shapeB_ = (CollisionShape2D*)fixtureB->GetUserData();
843
844 b2WorldManifold worldManifold;
845 contact->GetWorldManifold(&worldManifold);
846 numPoints_ = contact->GetManifold()->pointCount;
847 worldNormal_ = Vector2(worldManifold.normal.x, worldManifold.normal.y);
848 for (int i = 0; i < numPoints_; ++i)
849 {
850 worldPositions_[i] = Vector2(worldManifold.points[i].x, worldManifold.points[i].y);
851 separations_[i] = worldManifold.separations[i];
852 }
853 }
854
Serialize(VectorBuffer & buffer) const855 const Urho3D::PODVector<unsigned char>& PhysicsWorld2D::ContactInfo::Serialize(VectorBuffer& buffer) const
856 {
857 buffer.Clear();
858 for (int i = 0; i < numPoints_; ++i)
859 {
860 buffer.WriteVector2(worldPositions_[i]);
861 buffer.WriteVector2(worldNormal_);
862 buffer.WriteFloat(separations_[i]);
863 }
864 return buffer.GetBuffer();
865 }
866
867 }
868