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