C++ Point2::squaredLength方法代码示例

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);
        }
    }
}