1 #include "CollisionDetector.h"
2 #include "BoundingBox.h"
3 #include "QuadTree.h"
4 #include "LineSegment.h"
5 #include "MathManager.h"
6 
7 #include "../sketch/PathComponent.h"
8 #include "../sketch/Group.h"
9 using namespace jvgs::sketch;
10 
11 #include <iostream>
12 using namespace std;
13 
14 namespace jvgs
15 {
16     namespace math
17     {
CollisionDetector(Sketch * sketch)18         CollisionDetector::CollisionDetector(Sketch *sketch)
19         {
20             Group *root = sketch->getRoot();
21             addLinesFromGroup(root);
22 
23             tree = new QuadTree(&objects);
24             mathManager = MathManager::getInstance();
25         }
26 
~CollisionDetector()27         CollisionDetector::~CollisionDetector()
28         {
29             delete tree;
30             for(vector<BoundedObject*>::iterator iterator = objects.begin();
31                     iterator != objects.end(); iterator++) {
32                 delete (*iterator);
33             }
34         }
35 
getClosestCollision(const Vector2D & radius,const Vector2D & position,const Vector2D & velocity,float * time,Vector2D * collision)36         bool CollisionDetector::getClosestCollision(
37                 const Vector2D &radius, const Vector2D &position,
38                 const Vector2D &velocity, float *time, Vector2D *collision)
39         {
40             bool hasCollision = false;
41 
42             Vector2D ePosition = toEllipseSpace(radius, position);
43             Vector2D eVelocity = toEllipseSpace(radius, velocity);
44 
45             Vector2D destination = position + velocity;
46             BoundingBox boundingBox(
47                     BoundingBox(position - radius,
48                     position + radius),
49                     BoundingBox(destination - radius,
50                     destination + radius));
51 
52             /* The result from the tree search. */
53             /* TODO When there are multiple calls in one frame, it will
54              * calculate the result every time. We don't want that. */
55             vector<BoundedObject*> result;
56             tree->findObjects(&boundingBox, &result);
57 
58             /* Loop over all lines to find closest collision. */
59             for(vector<BoundedObject*>::iterator iterator = result.begin();
60                     iterator != result.end(); iterator++) {
61 
62                 /* Get the line segment. */
63                 LineSegment *segment = (LineSegment*) *iterator;
64 
65                 /* Convert the segment to ellipse space. */
66                 LineSegment *eSegment = new LineSegment(
67                         toEllipseSpace(radius, segment->getStart()),
68                         toEllipseSpace(radius, segment->getEnd()));
69 
70                 /* Temporary result variables. */
71                 Vector2D tmpCollision;
72                 float tmpTime;
73 
74                 /* Check against single segment. */
75                 if(getClosestCollision(eSegment, ePosition, eVelocity,
76                         &tmpTime, &tmpCollision)) {
77                     /* If this collision is the closest, store it, erasing
78                      * the previously remembered collision. */
79                     if(!hasCollision || tmpTime < *time) {
80                         hasCollision = true;
81                         *collision = fromEllipseSpace(radius, tmpCollision);
82                         *time = tmpTime;
83                     }
84                 }
85 
86                 /* Delete the ellipse space segment. */
87                 delete eSegment;
88             }
89 
90             return hasCollision;
91         }
92 
addLinesFromGroup(Group * group)93         void CollisionDetector::addLinesFromGroup(Group *group)
94         {
95             for(int i = 0; i < group->getNumberOfSketchElements(); i++) {
96                 SketchElement *element = group->getSketchElement(i);
97                 if(element->getType() == SKETCHELEMENTTYPE_GROUP)
98                     addLinesFromGroup((Group*) element);
99                 else if(element->getType() == SKETCHELEMENTTYPE_PATH)
100                     addLinesFromPath((Path*) element);
101             }
102         }
103 
addLinesFromPath(Path * path)104         void CollisionDetector::addLinesFromPath(Path *path)
105         {
106             for(int i = 0; i < path->getNumberOfComponents(); i++) {
107                 PathComponent *component = path->getComponent(i);
108                 for(int j = 0; j < component->getNumberOfSegments(); j++) {
109                     PathSegment *segment = component->getSegment(j);
110                     float increment = Path::getSubdivideLength() /
111                             segment->getLength();
112                     Vector2D previous = segment->getPoint(0.0f), current;
113                     for(float t = increment; t <= 1.0f; t += increment) {
114                         current = segment->getPoint(t);
115                         objects.push_back(new LineSegment(current, previous));
116                         previous = current;
117                     }
118 
119                     /* Last segment for sure. */
120                     objects.push_back(new LineSegment(previous,
121                                 segment->getPoint(1.0f)));
122                 }
123             }
124         }
125 
getClosestCollision(LineSegment * segment,const Vector2D & position,const Vector2D & velocity,float * time,Vector2D * collision) const126         bool CollisionDetector::getClosestCollision(LineSegment *segment,
127                 const Vector2D &position, const Vector2D &velocity,
128                 float *time, Vector2D *collision) const
129         {
130             Line line = segment->getLine();
131             float signedDistanceToLine = line.getSignedDistance(position);
132             float normalDotVelocity = line.getNormal() * velocity;
133 
134             /* Collision times. */
135             float t0, t1;
136             bool embeddedInLine = false;
137 
138             /* Special case: normalDotVelocity == 0.0f */
139             if(normalDotVelocity == 0.0f) {
140                 /* Collision. */
141                 if(line.getDistance(position) < 1.0f) {
142                     t0 = 0.0f;
143                     t1 = 1.0f;
144                     embeddedInLine = true;
145                 } else {
146                     return false;
147                 }
148             } else {
149                 /* Calculate times. */
150                 t0 = (1.0f - signedDistanceToLine) / normalDotVelocity;
151                 t1 = (-1.0f - signedDistanceToLine) / normalDotVelocity;
152 
153                 /* Sort. */
154                 if(t0 > t1) {
155                     float tmp = t0;
156                     t0 = t1;
157                     t1 = tmp;
158                 }
159 
160                 /* Check that at least one result is within range. */
161                 if(t0 > 1.0f || t1 < 0.0f)
162                     return false;
163 
164                 /* Clamp to [0, 1] */
165                 t0 = mathManager->clamp(t0, 0.0f, 1.0f);
166                 t1 = mathManager->clamp(t1, 0.0f, 1.0f);
167             }
168 
169             /* Will contain the solution of the equation. */
170             bool foundCollision = false;
171             *time = 1.0f;
172             float root;
173 
174             /* Check start. */
175             if(pointCollision(position, velocity, segment->getStart(), *time,
176                     &root, collision)) {
177                 foundCollision = true;
178                 *time = root;
179             }
180 
181             /* Check end. */
182             if(pointCollision(position, velocity, segment->getEnd(), *time,
183                     &root, collision)) {
184                 foundCollision = true;
185                 *time = root;
186             }
187 
188             /* Check line segment between start and end. */
189             BoundingBox *segmentBox = segment->getBoundingBox();
190             Vector2D collisionPoint =
191                     line.getClosestPoint(position + velocity * t0);
192             if(segmentBox->hasPoint(collisionPoint) && t0 <= *time
193                     && t0 >= 0.0f) {
194                 *time = t0;
195                 foundCollision = true;
196                 *collision = collisionPoint;
197             } else {
198                 collisionPoint =
199                         line.getClosestPoint(position + velocity * t1);
200                 if(segmentBox->hasPoint(collisionPoint) && t1 <= *time
201                     && t1 >= 0.0f) {
202                     *time = t1;
203                     foundCollision = true;
204                     *collision = collisionPoint;
205                 }
206             }
207 
208             return foundCollision;
209         }
210 
pointCollision(const Vector2D & position,const Vector2D & velocity,const Vector2D & point,float treshold,float * time,Vector2D * collision) const211         bool CollisionDetector::pointCollision(
212                 const Vector2D &position, const Vector2D &velocity,
213                 const Vector2D &point, float treshold, float *time,
214                 Vector2D *collision) const
215         {
216             float root;
217             if(mathManager->getLowestPositiveRoot(velocity.getSquaredLength(),
218                     2.0f * (velocity * (position - point)),
219                     (point - position).getSquaredLength() - 1.0f, treshold,
220                     &root)) {
221                 *time = root;
222                 *collision = point;
223                 return true;
224             } else {
225                 return false;
226             }
227         }
228 
toEllipseSpace(const Vector2D & radius,const Vector2D & v) const229         Vector2D CollisionDetector::toEllipseSpace(const Vector2D &radius,
230                 const Vector2D &v) const
231         {
232             return Vector2D(v.getX() / radius.getX(), v.getY() / radius.getY());
233         }
234 
fromEllipseSpace(const Vector2D & radius,const Vector2D & v) const235         Vector2D CollisionDetector::fromEllipseSpace(const Vector2D &radius,
236                 const Vector2D &v) const
237         {
238             return Vector2D(v.getX() * radius.getX(), v.getY() * radius.getY());
239         }
240     }
241 }
242