void ObjectBase::onSimulation(float timeStep) { Point2 delta = m_velocityRelativeToParent * timeStep; if (! m_collidesWithCells || (delta.squaredLength() < 1e-6f)) { // Case without collisions m_frameRelativeToJoint.translation += delta; } else { if (! isRoot()) { report(format("ObjectBase::collidesWithCells must be false for non-root objects. (%d)", m_id.c_str()), ReportLevel::ERROR); m_frameRelativeToJoint.translation += delta; return; } // Collision case const float speed = m_velocityRelativeToParent.length(); // Algorithm: Find all tiles within the maximum extent of the object's // movement (i.e., ignoring direction) plus its radius. Reduce to // the edges (line segments) of non-traversable tiles. // A static object with default radius can hit four tiles; // one that is moving might typically hit nine, and there is no // upper bound. Map::SmallEdgeArray edgesInDisk; const Frame2D& frame = this->frame(); m_world.map->getEdgesInDisk(frame.translation, delta.length() + m_collisionRadius, m_elevation, LayerIndex(1), edgesInDisk); // Transform the edges into the parent's object space const Frame2D& parentFrame = (isRoot() ? Frame2D() : m_world.objectTable.valueFromKey(m_parent)->frame()); for (int e = 0; e < edgesInDisk.size(); ++e) { LineSegment2D& edge = edgesInDisk[e]; debugDrawEdgeArray.append(edge); // TODO: Remove edge = parentFrame.bringFromParentSpace(edge); } const int maxIterations = 20; // Check for collisions with the map edges, adjusting velocity into the unblocked direction each time int iterations; for (iterations = 0; (iterations < maxIterations) && (timeStep > 1e-6); ++iterations) { // Find the first collision float firstCollisionTime = finf(); Point2 firstCollisionLocation; for (int e = 0; e < edgesInDisk.size(); ++e) { const LineSegment2D& edge = edgesInDisk[e]; // Recall that everything is in body space, now Point2 collisionLocation; const float collisionTime = movingDiskFixedLineSegmentCollisionTime(m_frameRelativeToJoint.translation, m_collisionRadius, m_velocityRelativeToParent, edge, collisionLocation); if (collisionTime < inf()) { debugDrawPointArray.append(collisionLocation); } if (collisionTime < firstCollisionTime) { firstCollisionTime = collisionTime; firstCollisionLocation = collisionLocation; } } // Resolve the collision if it happens before the end of the time step if (firstCollisionTime < timeStep) { // Advance to just before the collision firstCollisionTime = max(firstCollisionTime - 1e-5f, 0.0f); m_frameRelativeToJoint.translation += m_velocityRelativeToParent * firstCollisionTime; timeStep -= firstCollisionTime; const Vector2 normal = (m_frameRelativeToJoint.translation - firstCollisionLocation).directionOrZero(); // Alter velocity at the collision by removing the component of the velocity along the collision normal m_velocityRelativeToParent -= normal * min(normal.dot(m_velocityRelativeToParent), 0.0f); // Restore full speed, deflecting movement m_velocityRelativeToParent = m_velocityRelativeToParent.directionOrZero() * speed; if (m_velocityRelativeToParent.squaredLength() < 1e-6f) { // Unable to proceed with movement because there is no velocity left timeStep = 0; } } else { // Go to the end of the time step firstCollisionTime = timeStep; m_frameRelativeToJoint.translation += m_velocityRelativeToParent * timeStep; timeStep = 0; } } if (iterations == maxIterations) { report("Hit maximum number of iterations in ObjectBase::onSimulation collision resolution.", ReportLevel::WARNING); } } }