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