FabGL
ESP32 Display Controller and Graphics Library
collisiondetector.cpp
1 /*
2  Created by Fabrizio Di Vittorio (fdivitto2013@gmail.com) - <http://www.fabgl.com>
3  Copyright (c) 2019-2020 Fabrizio Di Vittorio.
4  All rights reserved.
5 
6  This file is part of FabGL Library.
7 
8  FabGL is free software: you can redistribute it and/or modify
9  it under the terms of the GNU General Public License as published by
10  the Free Software Foundation, either version 3 of the License, or
11  (at your option) any later version.
12 
13  FabGL is distributed in the hope that it will be useful,
14  but WITHOUT ANY WARRANTY; without even the implied warranty of
15  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  GNU General Public License for more details.
17 
18  You should have received a copy of the GNU General Public License
19  along with FabGL. If not, see <http://www.gnu.org/licenses/>.
20  */
21 
22 
23 #include "freertos/FreeRTOS.h"
24 #include "freertos/task.h"
25 
26 
27 #include "collisiondetector.h"
28 #include "fabutils.h"
29 
30 
31 
32 namespace fabgl {
33 
34 
35 
36 
39 // QuadTree implementation
40 
41 
42 QuadTree::QuadTree(CollisionDetector * collisionDetector, QuadTree * parent, QuadTreeQuadrant quadrant, int x, int y, int width, int height)
43 {
44  m_collisionDetector = collisionDetector;
45  m_parent = parent;
46  m_quadrant = quadrant;
47  m_x = x;
48  m_y = y;
49  m_width = width;
50  m_height = height;
51  m_objects = nullptr;
52  m_objectsCount = 0;
53  m_children[TopLeft] = nullptr;
54  m_children[TopRight] = nullptr;
55  m_children[BottomLeft] = nullptr;
56  m_children[BottomRight] = nullptr;
57 }
58 
59 
60 bool QuadTree::isEmpty()
61 {
62  return m_objectsCount == 0 &&
63  m_children[TopLeft] == nullptr &&
64  m_children[TopRight] == nullptr &&
65  m_children[BottomLeft] == nullptr &&
66  m_children[BottomRight] == nullptr;
67 }
68 
69 
70 void QuadTree::detachFromParent()
71 {
72  if (m_parent) {
73  m_parent->m_children[m_quadrant] = nullptr;
74  m_parent = nullptr;
75  }
76 }
77 
78 
79 void QuadTree::insert(QuadTreeObject * object)
80 {
81  QuadTreeQuadrant quadrant = getQuadrant(object);
82  if (quadrant != None && m_children[quadrant]) {
83  m_children[quadrant]->insert(object);
84  return;
85  }
86 
87  object->owner = this;
88  object->next = m_objects;
89  m_objects = object;
90  ++m_objectsCount;
91 
92  if (m_objectsCount < QUADTREE_LEVEL_SPLIT_THRESHOLD)
93  return;
94 
95  // split m_objects inside sub trees (4 quadrants)
96 
97  QuadTreeObject * obj = m_objects;
98  QuadTreeObject * prev = nullptr;
99  while (obj) {
100  QuadTreeObject * next = obj->next;
101  QuadTreeQuadrant quadrant = getQuadrant(obj);
102  if (quadrant != None) {
103  createQuadrant(quadrant);
104  m_children[quadrant]->insert(obj);
105  --m_objectsCount;
106  if (prev == nullptr)
107  m_objects = next;
108  else
109  prev->next = next;
110  } else {
111  prev = obj;
112  }
113  obj = next;
114  }
115 }
116 
117 
118 void QuadTree::remove(QuadTreeObject * object)
119 {
120  // rebuild the list removing this object
121  QuadTreeObject * obj = object->owner->m_objects;
122  QuadTreeObject * prev = nullptr;
123  while (obj) {
124  if (obj == object) {
125  if (prev == nullptr)
126  object->owner->m_objects = object->next;
127  else
128  prev->next = object->next;
129  break;
130  }
131  prev = obj;
132  obj = obj->next;
133  }
134  object->owner->m_objectsCount -= 1;
135  object->owner = nullptr;
136  object->next = nullptr;
137 }
138 
139 
140 void QuadTree::createQuadrant(QuadTreeQuadrant quadrant)
141 {
142  if (m_children[quadrant] == nullptr) {
143  int halfWidth = m_width >> 1;
144  int halfHeight = m_height >> 1;
145  switch (quadrant) {
146  case TopLeft:
147  m_children[TopLeft] = m_collisionDetector->initEmptyQuadTree(this, TopLeft, m_x, m_y, halfWidth, halfHeight);
148  break;
149  case TopRight:
150  m_children[TopRight] = m_collisionDetector->initEmptyQuadTree(this, TopRight, m_x + halfWidth, m_y, halfWidth, halfHeight);
151  break;
152  case BottomLeft:
153  m_children[BottomLeft] = m_collisionDetector->initEmptyQuadTree(this, BottomLeft, m_x, m_y + halfHeight, halfWidth, halfHeight);
154  break;
155  case BottomRight:
156  m_children[BottomRight] = m_collisionDetector->initEmptyQuadTree(this, BottomRight, m_x + halfWidth, m_y + halfHeight, halfWidth, halfHeight);
157  break;
158  default:
159  break;
160  }
161  }
162 }
163 
164 
165 // check if object is inside rect
166 bool QuadTree::objectInRect(QuadTreeObject * object, int x, int y, int width, int height)
167 {
168  return (object->sprite->x >= x) &&
169  (object->sprite->y >= y) &&
170  (object->sprite->x + object->sprite->getWidth() <= x + width) &&
171  (object->sprite->y + object->sprite->getHeight() <= y + height);
172 }
173 
174 
175 QuadTreeQuadrant QuadTree::getQuadrant(QuadTreeObject * object)
176 {
177  int hWidth = m_width >> 1;
178  int hHeight = m_height >> 1;
179  if (objectInRect(object, m_x, m_y, hWidth, hHeight))
180  return TopLeft;
181  if (objectInRect(object, m_x + hWidth, m_y, hWidth, hHeight))
182  return TopRight;
183  if (objectInRect(object, m_x, m_y + hHeight, hWidth, hHeight))
184  return BottomLeft;
185  if (objectInRect(object, m_x + hWidth, m_y + hHeight, hWidth, hHeight))
186  return BottomRight;
187  return None;
188 }
189 
190 
191 void QuadTree::update(QuadTreeObject * object)
192 {
193  QuadTree * qtree = object->owner;
194  while (true) {
195  if (qtree->m_parent == nullptr || objectInRect(object, qtree->m_x, qtree->m_y, qtree->m_width, qtree->m_height)) {
196  // need to reinsert?
197  QuadTreeQuadrant quadrant = qtree->getQuadrant(object);
198  if (qtree == object->owner && qtree->m_children[quadrant] == nullptr)
199  return; // don't need to reinsert
200 
201  // need to be reinserted, remove from owner...
202  remove(object);
203 
204  // ...and reinsert in qtree node
205  qtree->insert(object);
206  break;
207  }
208  qtree = qtree->m_parent;
209  }
210 }
211 
212 
213 // get the first detected collision
214 // ret nullptr = no collision detected
215 QuadTreeObject * QuadTree::detectCollision(QuadTreeObject * object, CollisionDetectionCallback callbackFunc, void * callbackObj)
216 {
217  if (!object->sprite->visible)
218  return nullptr;
219 
220  // find rectangle level collision with objects of this level
221  QuadTreeObject * obj = m_objects;
222  while (obj) {
223  QuadTreeObject * next = obj->next; // this allows object to be removed if collision is detected
224  // check intersection and masks collision
225  Point collisionPoint;
226  if (object != obj && obj->sprite->visible && objectsIntersect(object, obj) && checkMaskCollision(object, obj, &collisionPoint)) {
227  // collision!
228  if (callbackFunc)
229  callbackFunc(callbackObj, object->sprite, obj->sprite, collisionPoint); // call function and continue
230  else
231  return obj; // return first detected object and stop
232  }
233  obj = next;
234  }
235 
236  // check in children
237  QuadTreeQuadrant quadrant = getQuadrant(object);
238  if (quadrant != None) {
239  // look in a specific quadrant
240  if (m_children[quadrant])
241  return m_children[quadrant]->detectCollision(object, callbackFunc, callbackObj);
242  } else {
243  // look in all quadrants
244  for (int i = 0; i < 4; ++i) {
245  QuadTreeObject * obj = m_children[i] && objectIntersectsQuadTree(object, m_children[i]) ? m_children[i]->detectCollision(object, callbackFunc, callbackObj) : nullptr;
246  if (obj && !callbackFunc)
247  return obj;
248  }
249  }
250 
251  return nullptr;
252 }
253 
254 
255 bool QuadTree::objectsIntersect(QuadTreeObject * objectA, QuadTreeObject * objectB)
256 {
257  return objectA->sprite->x + objectA->sprite->getWidth() >= objectB->sprite->x &&
258  objectB->sprite->x + objectB->sprite->getWidth() >= objectA->sprite->x &&
259  objectA->sprite->y + objectA->sprite->getHeight() >= objectB->sprite->y &&
260  objectB->sprite->y + objectB->sprite->getHeight() >= objectA->sprite->y;
261 }
262 
263 
264 
265 bool QuadTree::objectIntersectsQuadTree(QuadTreeObject * object, QuadTree * quadTree)
266 {
267  return object->sprite->x + object->sprite->getWidth() >= quadTree->m_x &&
268  quadTree->m_x + quadTree->m_width >= object->sprite->x &&
269  object->sprite->y + object->sprite->getHeight() >= quadTree->m_y &&
270  quadTree->m_y + quadTree->m_height >= object->sprite->y;
271 }
272 
273 
274 bool QuadTree::checkMaskCollision(QuadTreeObject * objectA, QuadTreeObject * objectB, Point * collisionPoint)
275 {
276  // intersection rectangle
277  int x1 = tmax(objectA->sprite->x, objectB->sprite->x);
278  int y1 = tmax(objectA->sprite->y, objectB->sprite->y);
279  int x2 = tmin(objectA->sprite->x + objectA->sprite->getWidth() - 1, objectB->sprite->x + objectB->sprite->getWidth() - 1);
280  int y2 = tmin(objectA->sprite->y + objectA->sprite->getHeight() - 1, objectB->sprite->y + objectB->sprite->getHeight() - 1);
281 
282  // look for matching non trasparent pixels inside the intersection area
283  Bitmap * bitmapA = objectA->sprite->getFrame();
284  Bitmap * bitmapB = objectB->sprite->getFrame();
285  if (bitmapA->format == PixelFormat::RGBA2222 && bitmapB->format == PixelFormat::RGBA2222) {
286  // bitmaps have same pixel format, quick compare
287  for (int y = y1; y <= y2; ++y) {
288  uint8_t const * rowA = bitmapA->data + bitmapA->width * (y - objectA->sprite->y);
289  uint8_t const * rowB = bitmapB->data + bitmapB->width * (y - objectB->sprite->y);
290  for (int x = x1; x <= x2; ++x) {
291  int alphaA = rowA[x - objectA->sprite->x] >> 6;
292  int alphaB = rowB[x - objectB->sprite->x] >> 6;
293  if (alphaA && alphaB) {
294  *collisionPoint = (Point){(int16_t)x, (int16_t)y};
295  return true; // collision
296  }
297  }
298  }
299  } else {
300  // different pixel formats, slow compare
301  for (int y = y1; y <= y2; ++y) {
302  for (int x = x1; x <= x2; ++x) {
303  int alphaA = bitmapA->getAlpha(x - objectA->sprite->x, y - objectA->sprite->y);
304  int alphaB = bitmapB->getAlpha(x - objectB->sprite->x, y - objectB->sprite->y);
305  if (alphaA && alphaB) {
306  *collisionPoint = (Point){(int16_t)x, (int16_t)y};
307  return true; // collision
308  }
309  }
310  }
311  }
312 
313  return false;
314 }
315 
316 
317 /*
318 void QuadTree::dump(int level)
319 {
320  printf("%*sLevel %d x:%d y:%d w:%d h:%d\n", level, "", level, m_x, m_y, m_width, m_height);
321  printf("%*sObjects: ", level, "");
322  QuadTreeObject * obj = m_objects;
323  while (obj) {
324  printf("[%p x:%d y:%d w:%d h:%d] ", obj, obj->sprite->x, obj->sprite->y, obj->sprite->getWidth(), obj->sprite->getHeight());
325  obj = obj->next;
326  }
327  printf("\n");
328  printf("%*sChildren:\n", level, "");
329  for (int i = TopLeft; i <= BottomRight; ++i)
330  if (m_children[i])
331  m_children[i]->dump(level + 1);
332 }
333 */
334 
335 
338 // CollisionDetector implementation
339 
340 
341 CollisionDetector::CollisionDetector(int maxObjectsCount, int width, int height)
342  : m_quadTreePool(nullptr), m_objectPool(nullptr)
343 {
344  m_objectPoolSize = maxObjectsCount;
345  m_quadTreePoolSize = (5 * maxObjectsCount + 1) / 3;
346 
347  if (maxObjectsCount > 0) {
348 
349  // preallocate and initialize quad trees
350  m_quadTreePool = (QuadTree*) malloc(sizeof(QuadTree) * m_quadTreePoolSize);
351 
352  // initialize root quadtree
353  m_quadTreePool[0] = QuadTree(this, nullptr, None, 0, 0, width, height);
354  m_rootQuadTree = m_quadTreePool;
355 
356  // initialize other quadtrees
357  for (int i = 1; i < m_quadTreePoolSize; ++i)
358  m_quadTreePool[i] = QuadTree(this, nullptr, None, 0, 0, 0, 0);
359 
360  // preallocate and initialize objects
361  m_objectPool = (QuadTreeObject*) malloc(sizeof(QuadTreeObject) * m_objectPoolSize);
362  for (int i = 0; i < m_objectPoolSize; ++i)
363  m_objectPool[i] = QuadTreeObject(nullptr, nullptr);
364 
365  }
366 }
367 
368 
369 CollisionDetector::~CollisionDetector()
370 {
371  free(m_quadTreePool);
372  free(m_objectPool);
373 }
374 
375 
376 QuadTree * CollisionDetector::initEmptyQuadTree(QuadTree * parent, QuadTreeQuadrant quadrant, int x, int y, int width, int height)
377 {
378  // look for a unused quadtree inside the pool
379  for (int i = 1; i < m_quadTreePoolSize; ++i) {
380  if (m_quadTreePool[i].isEmpty()) {
381  m_quadTreePool[i].detachFromParent();
382  m_quadTreePool[i] = QuadTree(this, parent, quadrant, x, y, width, height);
383  return &m_quadTreePool[i];
384  }
385  }
386  assert(false && "No enough quadtrees"); // should never happen
387 }
388 
389 
391 {
392  // look for unused object inside the pool
393  for (int i = 0; i < m_objectPoolSize; ++i)
394  if (m_objectPool[i].sprite == nullptr) {
395  QuadTreeObject * obj = &m_objectPool[i];
396  obj->sprite = sprite;
397  sprite->collisionDetectorObject = obj;
398  m_rootQuadTree->insert(obj);
399  return;
400  }
401  assert(false && "No enough objects"); // should never happen
402 }
403 
404 
406 {
407  QuadTree::remove(sprite->collisionDetectorObject);
408  sprite->collisionDetectorObject->sprite = nullptr;
409 }
410 
411 
412 /*
413 void CollisionDetector::dump()
414 {
415  m_rootQuadTree->dump();
416 }
417 */
418 
419 
420 Sprite * CollisionDetector::detectCollision(Sprite * sprite, bool removeCollidingSprites)
421 {
422  QuadTreeObject * obj = m_rootQuadTree->detectCollision(sprite->collisionDetectorObject);
423  if (obj) {
424  Sprite * cSprite = obj->sprite;
425  removeSprite(sprite);
426  removeSprite(cSprite);
427  return cSprite;
428  } else
429  return nullptr;
430 }
431 
432 
433 void CollisionDetector::detectCollision(Sprite * sprite, CollisionDetectionCallback callbackFunc, void * callbackObj)
434 {
435  m_rootQuadTree->detectCollision(sprite->collisionDetectorObject, callbackFunc, callbackObj);
436 }
437 
438 
440 {
441  m_rootQuadTree->update(sprite->collisionDetectorObject);
442 }
443 
444 
445 Sprite * CollisionDetector::updateAndDetectCollision(Sprite * sprite, bool removeCollidingSprites)
446 {
447  update(sprite);
448  return detectCollision(sprite, removeCollidingSprites);
449 }
450 
451 
452 void CollisionDetector::updateAndDetectCollision(Sprite * sprite, CollisionDetectionCallback callbackFunc, void * callbackObj)
453 {
454  update(sprite);
455  detectCollision(sprite, callbackFunc, callbackObj);
456 }
457 
458 
459 
460 
461 } // end of namespace
This file contains fabgl::CollisionDetector class definition.
Represents a sprite.
void addSprite(Sprite *sprite)
Adds the specified sprite to collision detector.
CollisionDetector(int maxObjectsCount, int width, int height)
Creates an instance of CollisionDetector.
void removeSprite(Sprite *sprite)
Removes the specified sprite from collision detector.
This file contains some utility classes and functions.
Definition: canvas.cpp:31
Sprite * detectCollision(Sprite *sprite, bool removeCollidingSprites=true)
Detects first collision with the specified sprite.
Sprite * updateAndDetectCollision(Sprite *sprite, bool removeCollidingSprites=true)
Updates collision detector and detect collision with the specified sprite.
void update(Sprite *sprite)
Updates collision detector.
uint8_t height
uint8_t width