C++ BoundingBox类代码示例

//=====================================================================
bool MouseBoxZoomer::eventFilter(QObject *widget, QEvent *e) {
  GlMainWidget *glw = static_cast<GlMainWidget *>(widget);
  GlGraphInputData *inputData = glw->getScene()->getGlGraphComposite()->getInputData();

  if (e->type() == QEvent::MouseButtonPress) {
    QMouseEvent * qMouseEv = static_cast<QMouseEvent *>(e);

    if (qMouseEv->buttons() == mButton &&
        (kModifier == Qt::NoModifier ||
         qMouseEv->modifiers() & kModifier)) {
      if (!started) {
        x = qMouseEv->x();
        y =  glw->height() - qMouseEv->y();
        w = 0;
        h = 0;
        started = true;
        graph = inputData->getGraph();
      }
      else {
        if (inputData->getGraph() != graph) {
          graph = NULL;
          started = false;
        }
      }

      return true;
    }

    if (qMouseEv->buttons()==Qt::MidButton) {
      started = false;
      glw->redraw();
      return true;
    }

    return false;
  }

  if (e->type() == QEvent::MouseMove) {
    QMouseEvent * qMouseEv = static_cast<QMouseEvent *>(e);

    if ((qMouseEv->buttons() & mButton) &&
        (kModifier == Qt::NoModifier ||
         qMouseEv->modifiers() & kModifier)) {
      if (inputData->getGraph() != graph) {
        graph = NULL;
        started = false;
      }

      if (started) {
        if ((qMouseEv->x() > 0) && (qMouseEv->x() < glw->width()))
          w = qMouseEv->x() - x;

        if ((qMouseEv->y() > 0) && (qMouseEv->y() < glw->height()))
          h = y - (glw->height() - qMouseEv->y());

        glw->redraw();
        return true;
      }
    }
  }

  if (e->type() == QEvent::MouseButtonDblClick) {
    GlBoundingBoxSceneVisitor bbVisitor(inputData);
    glw->getScene()->getLayer("Main")->acceptVisitor(&bbVisitor);
    QtGlSceneZoomAndPanAnimator zoomAnPan(glw, bbVisitor.getBoundingBox());
    zoomAnPan.animateZoomAndPan();
    return true;
  }

  if (e->type() == QEvent::MouseButtonRelease) {

    QMouseEvent * qMouseEv = static_cast<QMouseEvent *>(e);

    if ((qMouseEv->button() == mButton &&
         (kModifier == Qt::NoModifier ||
          qMouseEv->modifiers() & kModifier))) {
      if (inputData->getGraph() != graph) {
        graph = NULL;
        started = false;
      }

      if (started) {
        started = false;

        if(!(w==0 && h==0)) {
          int width = glw->width();
          int height = glw->height();

          Coord bbMin(width-x, height - y+h);
          Coord bbMax(width-(x+w), height - y);

          if (abs(bbMax[0] - bbMin[0]) > 1 && abs(bbMax[1] - bbMin[1]) > 1) {

            BoundingBox sceneBB;
            sceneBB.expand(glw->getScene()->getGraphCamera().viewportTo3DWorld(glw->screenToViewport(bbMin)));
            sceneBB.expand(glw->getScene()->getGraphCamera().viewportTo3DWorld(glw->screenToViewport(bbMax)));

            QtGlSceneZoomAndPanAnimator zoomAnPan(glw, sceneBB);
            zoomAnPan.animateZoomAndPan();
//.........这里部分代码省略.........

void BoundingSphere::merge(const BoundingBox& box)
{
    if (box.isEmpty())
        return;

    const Vector3& min = box.min;
    const Vector3& max = box.max;

    // Find the corner of the bounding box that is farthest away from this sphere's center.
    float v1x = min.x - center.x;
    float v1y = min.y - center.y;
    float v1z = min.z - center.z;
    float v2x = max.x - center.x;
    float v2y = max.y - center.y;
    float v2z = max.z - center.z;
    float fx = min.x;
    float fy = min.y;
    float fz = min.z;

    if (v2x > v1x)
    {
        fx = max.x;
    }
    if (v2y > v1y)
    {
        fy = max.y;
    }
    if (v2z > v1z)
    {
        fz = max.z;
    }

    // Calculate the unit vector and the distance between the center and the farthest point.
    v1x = center.x - fx;
    v1y = center.y - fy;
    v1z = center.z - fz;
    float distance = sqrt(v1x * v1x + v1y * v1y + v1z * v1z);

    // If the box is inside the sphere, we are done.
    if (distance <= radius)
    {
        return;
    }

    // Calculate the unit vector between the center and the farthest point.
    GP_ASSERT(distance != 0.0f);
    float dI = 1.0f / distance;
    v1x *= dI;
    v1y *= dI;
    v1z *= dI;

    // Calculate the new radius.
    float r = (radius + distance) * 0.5f;

    // Calculate the new center.
    v1x = v1x * r + fx;
    v1y = v1y * r + fy;
    v1z = v1z * r + fz;

    // Set the new center and radius.
    center.x = v1x;
    center.y = v1y;
    center.z = v1z;
    radius = r;
}

void WriteVertex(float*& dest, aiMesh* mesh, unsigned index, unsigned elementMask, BoundingBox& box,
    const Matrix3x4& vertexTransform, const Matrix3& normalTransform, Vector<PODVector<unsigned char> >& blendIndices,
    Vector<PODVector<float> >& blendWeights)
{
    Vector3 vertex = vertexTransform * ToVector3(mesh->mVertices[index]);
    box.Merge(vertex);
    *dest++ = vertex.x_;
    *dest++ = vertex.y_;
    *dest++ = vertex.z_;
    if (elementMask & MASK_NORMAL)
    {
        Vector3 normal = normalTransform * ToVector3(mesh->mNormals[index]);
        *dest++ = normal.x_;
        *dest++ = normal.y_;
        *dest++ = normal.z_;
    }
    if (elementMask & MASK_COLOR)
    {
        *((unsigned*)dest) = Color(mesh->mColors[0][index].r, mesh->mColors[0][index].g, mesh->mColors[0][index].b,
            mesh->mColors[0][index].a).ToUInt();
        ++dest;
    }
    if (elementMask & MASK_TEXCOORD1)
    {
        Vector3 texCoord = ToVector3(mesh->mTextureCoords[0][index]);
        *dest++ = texCoord.x_;
        *dest++ = texCoord.y_;
    }
    if (elementMask & MASK_TEXCOORD2)
    {
        Vector3 texCoord = ToVector3(mesh->mTextureCoords[1][index]);
        *dest++ = texCoord.x_;
        *dest++ = texCoord.y_;
    }
    if (elementMask & MASK_TANGENT)
    {
        Vector3 tangent = normalTransform * ToVector3(mesh->mTangents[index]);
        Vector3 normal = normalTransform * ToVector3(mesh->mNormals[index]);
        Vector3 bitangent = normalTransform * ToVector3(mesh->mBitangents[index]);
        // Check handedness
        float w = 1.0f;
        if ((tangent.CrossProduct(normal)).DotProduct(bitangent) < 0.5f)
            w = -1.0f;

        *dest++ = tangent.x_;
        *dest++ = tangent.y_;
        *dest++ = tangent.z_;
        *dest++ = w;
    }
    if (elementMask & MASK_BLENDWEIGHTS)
    {
        for (unsigned i = 0; i < 4; ++i)
        {
            if (i < blendWeights[index].Size())
                *dest++ = blendWeights[index][i];
            else
                *dest++ = 0.0f;
        }
    }
    if (elementMask & MASK_BLENDINDICES)
    {
        unsigned char* destBytes = (unsigned char*)dest;
        ++dest;
        for (unsigned i = 0; i < 4; ++i)
        {
            if (i < blendIndices[index].Size())
                *destBytes++ = blendIndices[index][i];
            else
                *destBytes++ = 0;
        }
    }
}

BoundingBox MinimumBoundingBox::getMinimumBoundingBox(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
    BoundingBox bb;
    std::vector< pcl::Vertices > polygons;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::ConvexHull<pcl::PointXYZRGB> chull;
    chull.setInputCloud (cloud);
    chull.setDimension(3);
    chull.reconstruct (*cloud_hull,polygons);

    yarp::sig::Vector x1;
    yarp::sig::Vector x2;
    yarp::sig::Vector x3;
    yarp::sig::Vector y1;
    yarp::sig::Vector y2;
    yarp::sig::Vector y3;
    yarp::sig::Vector z1;
    yarp::sig::Vector z2;
    yarp::sig::Vector z3;

    for (int i=0; i<polygons.size(); i++)
    {
        pcl::Vertices vertex=polygons.at(i);
        x1.push_back(cloud_hull->at(vertex.vertices[0]).x);
        x2.push_back(cloud_hull->at(vertex.vertices[1]).x);
        x3.push_back(cloud_hull->at(vertex.vertices[2]).x);
        y1.push_back(cloud_hull->at(vertex.vertices[0]).y);
        y2.push_back(cloud_hull->at(vertex.vertices[1]).y);
        y3.push_back(cloud_hull->at(vertex.vertices[2]).y);
        z1.push_back(cloud_hull->at(vertex.vertices[0]).z);
        z2.push_back(cloud_hull->at(vertex.vertices[1]).z);
        z3.push_back(cloud_hull->at(vertex.vertices[2]).z);
    }

    Matrix pointCloud(cloud_hull->size(),3);
    for (int i=0; i<cloud_hull->size(); i++)
    {
        pointCloud(i,0)=cloud_hull->at(i).x;
        pointCloud(i,1)=cloud_hull->at(i).y;
        pointCloud(i,2)=cloud_hull->at(i).z;
    }

    yarp::sig::Vector v1x=x2-x1;
    yarp::sig::Vector v1y=y2-y1;
    yarp::sig::Vector v1z=z2-z1;
    std::vector<yarp::sig::Vector> edges1;

    retrieveEdges(v1x,v1y,v1z,edges1);

    yarp::sig::Vector v2x=x3-x1;
    yarp::sig::Vector v2y=y3-y1;
    yarp::sig::Vector v2z=z3-z1;
    std::vector<yarp::sig::Vector> edges2;

    retrieveEdges2(v2x,v2y,v2z,edges1,edges2);

    std::vector<yarp::sig::Vector> crossProduct;
    for (int i=0; i<edges1.size(); i++)
    {
        yarp::sig::Vector vect=cross(edges1.at(i),edges2.at(i));
        crossProduct.push_back(vect);
    }

    yarp::sig::Vector alpha;
    yarp::sig::Vector beta;
    yarp::sig::Vector gamma;
    eulerAngles(edges1, edges2, crossProduct, alpha, beta, gamma);

    Matrix minmax(2,3);
    double minVol=100000;
    Matrix rot2;

    for (int i=0; i<alpha.size(); i++)
    {
        Matrix rot;
        buildRotMat3(alpha[i],beta[i],gamma[i],rot);
        Matrix xyz_i=pointCloud*rot;
        findRotation(xyz_i,rot2);

        Matrix rot3dim=eye(3,3);
        rot3dim.setSubmatrix(rot2,0,0);
        Matrix rotation=rot*rot3dim;

        xyz_i=pointCloud*rotation;
        yarp::sig::Vector minimum;
        Helpers::min(xyz_i,minimum);
        yarp::sig::Vector maximum;
        Helpers::max(xyz_i,maximum);
        yarp::sig::Vector h=maximum-minimum;

        double prod=h[0]*h[1]*h[2];
        if (prod<minVol)
        {
            minVol=prod;
            bb.setOrientation(rotation);
            minmax.setRow(0,minimum);
            minmax.setRow(1,maximum);
        }
    }
    Matrix cornerpointsTmp(8,3);
//.........这里部分代码省略.........

    bool Collider::Check(const BoundingBox &BB,  const Sphere &SP)
    {

        real dmin = 0.0;

        if(SP.position.x < BB.Mininmum(X))
        {
            dmin += ((SP.position.x - BB.Mininmum(X))*(SP.position.x - BB.Mininmum(X))) ;
        }
        else if (SP.position.x > BB.Mininmum(X))
        {
            dmin += ((SP.position.x - BB.Mininmum(X))*(SP.position.x - BB.Maximum(X))) ;
        }

        if(SP.position.x < BB.Mininmum(X))
        {
            dmin += ((SP.position.x - BB.Mininmum(X))*(SP.position.x - BB.Mininmum(X))) ;
        }
        else if (SP.position.x > BB.Mininmum(X))
        {
            dmin += ((SP.position.x - BB.Mininmum(X))*(SP.position.x - BB.Maximum(X))) ;
        }

        if(SP.position.y < BB.Mininmum(Y))
        {
            dmin += ((SP.position.y - BB.Mininmum(Y))*(SP.position.y - BB.Mininmum(Y))) ;
        }
        else if (SP.position.y > BB.Mininmum(Y))
        {
            dmin += ((SP.position.y - BB.Mininmum(Y))*(SP.position.y - BB.Maximum(Y))) ;
        }

        if(SP.position.z < BB.Mininmum(Z))
        {
            dmin += ((SP.position.z - BB.Mininmum(Z))*(SP.position.z - BB.Mininmum(Z))) ;
        }
        else if (SP.position.z > BB.Mininmum(Z))
        {
            dmin += ((SP.position.z - BB.Mininmum(Z))*(SP.position.z - BB.Maximum(Z))) ;
        }


          return dmin <= (SP.radious*SP.radious);

    }

//--------------------------------------------------------------------------------------------------
/// 
//--------------------------------------------------------------------------------------------------
bool ShaderUniforms::onInitialize()
{
    m_propGlobalUniformSet = new PropertyBool("Use global uniform set", true);
    m_propertyPublisher->publishProperty(m_propGlobalUniformSet.p());

    // Create the single shader program with default uniforms
    ref<ShaderProgram> shaderProg;
    {
        ShaderProgramGenerator shaderGen("ShaderUniforms", ShaderSourceProvider::instance());
        shaderGen.addVertexCodeFromFile("ShaderUniforms_Vert");
        shaderGen.addFragmentCode(ShaderSourceRepository::light_SimpleHeadlight);
        shaderGen.addFragmentCodeFromFile("ShaderUniforms_Frag");
        shaderProg = shaderGen.generate();
        shaderProg->setDefaultUniform(new UniformFloat("u_color0", Color3f(Color3::WHITE)));
        shaderProg->setDefaultUniform(new UniformFloat("u_color1", Color3f(Color3::MAGENTA)));
        shaderProg->setDefaultUniform(new UniformFloat("u_colorMixFactor", 1.0f));
    }

    // No uniforms, relies only on the shader program defaults
    m_effNoUniforms = new Effect;
    m_effNoUniforms->setShaderProgram(shaderProg.p());

    // Effect with only mix factor set (uses colors from default)
    m_effWithSomeUniforms = new Effect;
    m_effWithSomeUniforms->setShaderProgram(shaderProg.p());
    m_effWithSomeUniforms->setUniform(new UniformFloat("u_colorMixFactor", 0));

    // Fully specified uniforms
    m_effWithAllUniforms = new Effect;
    m_effWithAllUniforms->setShaderProgram(shaderProg.p());
    m_effWithAllUniforms->setUniform(new UniformFloat("u_color0", Color3f(Color3::BLACK)));
    m_effWithAllUniforms->setUniform(new UniformFloat("u_color1", Color3f(Color3::GREEN)));
    m_effWithAllUniforms->setUniform(new UniformFloat("u_colorMixFactor", 1.0f));


    // Box part
    ref<Part> boxPart = new Part;
    {
        BoxGenerator gen;
        gen.setCenterAndExtent(Vec3d(-2, 0, 0), Vec3d(2, 2, 2));
        GeometryBuilderDrawableGeo builder;
        gen.generate(&builder);
        boxPart->setDrawable(builder.drawableGeo().p());
    }

    // Sphere part
    ref<Part> spherePart = new Part;
    {
        GeometryBuilderDrawableGeo builder;
        GeometryUtils::createSphere(1, 10, 10, &builder);
        spherePart->setDrawable(builder.drawableGeo().p());
    }

    // Cylinder part
    ref<Part> cylinderPart = new Part;
    {
        GeometryBuilderDrawableGeo builder;
        GeometryUtils::createObliqueCylinder(1, 1, 2, 0, 0, 10, true, true, true, 1, &builder);
        ref<DrawableGeo> geo = builder.drawableGeo();
        geo->transform(Mat4d::fromTranslation(Vec3d(2, 0, -1)));
        cylinderPart->setDrawable(geo.p());
    }

    ref<ModelBasicList> myModel = new ModelBasicList;
    boxPart->setEffect(m_effNoUniforms.p());
    spherePart->setEffect(m_effWithSomeUniforms.p());
    cylinderPart->setEffect(m_effWithAllUniforms.p());

    myModel->addPart(boxPart.p());
    myModel->addPart(spherePart.p());
    myModel->addPart(cylinderPart.p());
    myModel->updateBoundingBoxesRecursive();


    m_renderSequence->firstRendering()->scene()->addModel(myModel.p());

    BoundingBox bb = myModel->boundingBox();
    if (bb.isValid())
    {
        m_camera->fitView(bb, Vec3d::Y_AXIS, Vec3d::Z_AXIS);
    }

    applyCurrentProperties();

    return true;
}

/***************************************************************
* Function: setSurfaceScalingHint()
***************************************************************/
void CAVEGroupIconSurface::setSurfaceScalingHint(const BoundingBox& boundingbox)
{
    gScaleVal = (CAVEGeodeIcon::gSphereBoundRadius) / (boundingbox.radius());
}

//.........这里部分代码省略.........
        }

        ref<Part> part = new Part;
        part->setDrawable(geo.p());
        part->setEffect(eff.p());

        model->addPart(part.p());
    }

    {
        ref<Vec3fArray> vertices = new Vec3fArray;
        vertices->reserve(10);
        vertices->add(Vec3f(0, 0, 0));
        vertices->add(Vec3f(-3, 0, 0));
        vertices->add(Vec3f(3, 0, 0));

        ref<UIntArray> indices = new UIntArray(vertices->size());
        indices->setConsecutive(0);

        ref<PrimitiveSetIndexedUInt> primSet = new PrimitiveSetIndexedUInt(PT_POINTS);
        primSet->setIndices(indices.p());

        ref<DrawableGeo> geo = new DrawableGeo;
        geo->setVertexArray(vertices.p());
        geo->addPrimitiveSet(primSet.p());

        ref<Effect> eff = new Effect;

        if (useShaders)
        {
            bool useTextureSprite = true;

            cvf::ShaderProgramGenerator gen("PointSprites", cvf::ShaderSourceProvider::instance());
            gen.addVertexCode(ShaderSourceRepository::vs_DistanceScaledPoints);
            
            if (useTextureSprite)  gen.addFragmentCode(ShaderSourceRepository::src_TextureFromPointCoord);
            else                   gen.addFragmentCode(ShaderSourceRepository::src_Color);
            gen.addFragmentCode(ShaderSourceRepository::fs_CenterLitSpherePoints);

            ref<cvf::ShaderProgram> prog = gen.generate();
            eff->setShaderProgram(prog.p());
            eff->setUniform(new UniformFloat("u_pointRadius", 1.0f));

            if (useTextureSprite)
            {
                ref<Texture> tex = createTexture();
                ref<Sampler> sampler = new Sampler;
                sampler->setMinFilter(Sampler::LINEAR);
                sampler->setMagFilter(Sampler::NEAREST);
                sampler->setWrapModeS(Sampler::REPEAT);
                sampler->setWrapModeT(Sampler::REPEAT);
                ref<RenderStateTextureBindings> texBind = new RenderStateTextureBindings(tex.p(), sampler.p(), "u_texture2D");
                eff->setRenderState(texBind.p());
            }
            else
            {
                eff->setUniform(new UniformFloat("u_color", Color4f(Color3::RED)));
            }

            ref<RenderStatePoint> point = new RenderStatePoint(RenderStatePoint::PROGRAM_SIZE);
            point->enablePointSprite(true);
            eff->setRenderState(point.p());
        }
        else
        {
            eff->setRenderState(new RenderStateLighting_FF(false));
            eff->setRenderState(new RenderStateMaterial_FF(RenderStateMaterial_FF::PURE_MAGENTA));

            ref<RenderStatePoint> point = new RenderStatePoint(RenderStatePoint::FIXED_SIZE);
            point->enablePointSprite(true);
            point->setSize(600.0f);
            eff->setRenderState(point.p());
        }

        ref<Part> part = new Part;
        part->setDrawable(geo.p());
        part->setEffect(eff.p());

        model->addPart(part.p());
    }

    model->updateBoundingBoxesRecursive();
    m_renderSequence->firstRendering()->scene()->addModel(model.p());

    BoundingBox bb = model->boundingBox();
    if (bb.isValid())
    {
        m_camera->fitView(bb, -Vec3d::Z_AXIS, Vec3d::Y_AXIS);
        if (m_usePerspective)
        {
            m_camera->setProjectionAsPerspective(m_fovScale*40.0, m_nearPlane, m_camera->farPlane());
        }
        else
        {
            m_camera->setProjectionAsOrtho(m_fovScale*bb.extent().length(), m_nearPlane, m_camera->farPlane());
        }
    }

    return true;
}

const bool BoundingBox::intersects(const BoundingBox &box) const
{
    return(this->minCorner() < box.maxCorner() &&
           this->maxCorner() > box.minCorner());
}

//.........这里部分代码省略.........
    mColorTexPtr->Load(image);

    // build the color rt
    mColorRTPtr = GNEW(Target(mColorTexPtr.Ptr()));
    CHECK(mColorRTPtr);

    // build the primitive
    mQuadPtr = GNEW(Primitive);
    CHECK(mQuadPtr);
    mQuadPtr->SetType(Primitive::PT_TRIANGLES);

    // set the wvp
    Constant* wvp_constant_ptr = GNEW(Constant);
    CHECK(wvp_constant_ptr);
    wvp_constant_ptr->SetMatrix(Matrix());
    mQuadPtr->SetConstant("gWVP",wvp_constant_ptr);

    // set the color texture
    Constant* texture_constant_ptr = GNEW(Constant);
    CHECK(texture_constant_ptr);
    texture_constant_ptr->SetTexture(mColorTexPtr.Ptr());
    mQuadPtr->SetConstant("gBaseTex",texture_constant_ptr);

    // set the shader
    Str shader_key_name = "shader/default.xml";
    KeyPtr shader_key_ptr = Key::Find(shader_key_name.c_str());
    if(shader_key_ptr == NULL)
    {
        Shader*shader = GNEW(Shader);
        CHECK(shader);
        shader->Load(GLoad(shader_key_name.c_str()));
        shader_key_ptr = GNEW(Key(shader_key_name.c_str(), shader));
        CHECK(shader_key_ptr);
    }
    mKeys.push_back(shader_key_ptr);
    mQuadPtr->SetShader(dynamic_cast<Shader*>(shader_key_ptr->Ptr()),"p0");

    // build the vertex buffer
    F32 x = 0.0f, y = 0.0f, w = 256, h = 256;
    DVT vertexes[] =
    {
        {x,		y,		0,		0,		0},
        {x+w,	y,		0,		1,		0},
        {x+w,	y+h,	0,		1,		1},
        {x,		y+h,	0,		0,		1},
    };
    VertexBufferPtr vb_ptr = GNEW(VertexBuffer);
    CHECK(vb_ptr);
    {
        GDataPtr vd_ptr = GNEW(GData);
        CHECK(vd_ptr);
        vd_ptr->Size(3*sizeof(U32) + 3*sizeof(U8) + sizeof(vertexes));
        U8*data_ptr = (U8*)vd_ptr->Ptr();
        *(U32*)data_ptr = MAKEFOURCC('G','V','B','O');
        data_ptr += sizeof(U32);
        *(U32*)data_ptr = sizeof(vertexes)/sizeof(DVT);
        data_ptr += sizeof(U32);
        *(U32*)data_ptr = sizeof(DVT);
        data_ptr += sizeof(U32);
        *(U8*)data_ptr = 2;
        data_ptr += sizeof(U8);
        *(U8*)data_ptr = VertexBuffer::VT_3F;
        data_ptr += sizeof(U8);
        *(U8*)data_ptr = VertexBuffer::VT_2F;
        data_ptr += sizeof(U8);
        ::memcpy(data_ptr, vertexes, sizeof(vertexes));
        data_ptr += sizeof(vertexes);
        vb_ptr->Load(vd_ptr.Ptr());
    }
    mQuadPtr->SetVertexBuffer(vb_ptr.Ptr());

    // build the index
    const U16 indexes[] = { 3, 0, 2, 2, 0, 1 };
    IndexBufferPtr ib_ptr = GNEW(IndexBuffer);
    CHECK(ib_ptr);
    {
        GDataPtr id_ptr = GNEW(GData);
        CHECK(id_ptr);
        id_ptr->Size(3*sizeof(U32) + sizeof(indexes));
        U8*data_ptr = (U8*)id_ptr->Ptr();
        *(U32*)data_ptr = MAKEFOURCC('G','I','B','O');
        data_ptr += sizeof(U32);
        *(U32*)data_ptr = sizeof(indexes)/sizeof(U16);
        data_ptr += sizeof(U32);
        *(U32*)data_ptr = sizeof(U16);
        data_ptr += sizeof(U32);
        ::memcpy(data_ptr, &indexes[0], sizeof(indexes));
        data_ptr += sizeof(indexes);
        ib_ptr->Load(id_ptr.Ptr());
    }
    mQuadPtr->SetIndexBuffer(ib_ptr.Ptr());

    // build the bounding box
    BoundingBox box;
    box.set(MAX_F32,MAX_F32,MAX_F32,MIN_F32,MIN_F32,MIN_F32);
    for(U32 i = 0; i < sizeof(vertexes)/sizeof(DVTN); i++)box.expand(vertexes[i].point);
    mQuadPtr->SetBox(box);

    UNGUARD;
}

const bool BoundingBox::contains(const BoundingBox &box) const
{
    return(box.minCorner() >= this->minCorner() &&
           box.maxCorner() <= this->maxCorner());
}

bool
BridgeDetector::detect_angle()
{
    if (this->_edges.empty() || this->_anchors.empty()) return false;
    
    /*  Outset the bridge expolygon by half the amount we used for detecting anchors;
        we'll use this one to clip our test lines and be sure that their endpoints
        are inside the anchors and not on their contours leading to false negatives. */
    Polygons clip_area;
    offset(this->expolygon, &clip_area, +this->extrusion_width/2);
    
    /*  we'll now try several directions using a rudimentary visibility check:
        bridge in several directions and then sum the length of lines having both
        endpoints within anchors */
    
    // we test angles according to configured resolution
    std::vector<double> angles;
    for (int i = 0; i <= PI/this->resolution; ++i)
        angles.push_back(i * this->resolution);
    
    // we also test angles of each bridge contour
    {
        Polygons pp = this->expolygon;
        for (Polygons::const_iterator p = pp.begin(); p != pp.end(); ++p) {
            Lines lines = p->lines();
            for (Lines::const_iterator line = lines.begin(); line != lines.end(); ++line)
                angles.push_back(line->direction());
        }
    }
    
    /*  we also test angles of each open supporting edge
        (this finds the optimal angle for C-shaped supports) */
    for (Polylines::const_iterator edge = this->_edges.begin(); edge != this->_edges.end(); ++edge) {
        if (edge->first_point().coincides_with(edge->last_point())) continue;
        angles.push_back(Line(edge->first_point(), edge->last_point()).direction());
    }
    
    // remove duplicates
    double min_resolution = PI/180.0;  // 1 degree
    std::sort(angles.begin(), angles.end());
    for (size_t i = 1; i < angles.size(); ++i) {
        if (Slic3r::Geometry::directions_parallel(angles[i], angles[i-1], min_resolution)) {
            angles.erase(angles.begin() + i);
            --i;
        }
    }
    /*  compare first value with last one and remove the greatest one (PI) 
        in case they are parallel (PI, 0) */
    if (Slic3r::Geometry::directions_parallel(angles.front(), angles.back(), min_resolution))
        angles.pop_back();
    
    BridgeDirectionComparator bdcomp(this->extrusion_width);
    double line_increment = this->extrusion_width;
    bool have_coverage = false;
    for (std::vector<double>::const_iterator angle = angles.begin(); angle != angles.end(); ++angle) {
        Polygons my_clip_area = clip_area;
        ExPolygons my_anchors = this->_anchors;
        
        // rotate everything - the center point doesn't matter
        for (Polygons::iterator it = my_clip_area.begin(); it != my_clip_area.end(); ++it)
            it->rotate(-*angle, Point(0,0));
        for (ExPolygons::iterator it = my_anchors.begin(); it != my_anchors.end(); ++it)
            it->rotate(-*angle, Point(0,0));
    
        // generate lines in this direction
        BoundingBox bb;
        for (ExPolygons::const_iterator it = my_anchors.begin(); it != my_anchors.end(); ++it)
            bb.merge((Points)*it);
        
        Lines lines;
        for (coord_t y = bb.min.y; y <= bb.max.y; y += line_increment)
            lines.push_back(Line(Point(bb.min.x, y), Point(bb.max.x, y)));
        
        Lines clipped_lines;
        intersection(lines, my_clip_area, &clipped_lines);
        
        // remove any line not having both endpoints within anchors
        for (size_t i = 0; i < clipped_lines.size(); ++i) {
            Line &line = clipped_lines[i];
            if (!Slic3r::Geometry::contains(my_anchors, line.a)
                || !Slic3r::Geometry::contains(my_anchors, line.b)) {
                clipped_lines.erase(clipped_lines.begin() + i);
                --i;
            }
        }
        
        std::vector<double> lengths;
        double total_length = 0;
        for (Lines::const_iterator line = clipped_lines.begin(); line != clipped_lines.end(); ++line) {
            double len = line->length();
            lengths.push_back(len);
            total_length += len;
        }
        if (total_length) have_coverage = true;
        
        // sum length of bridged lines
        bdcomp.dir_coverage[*angle] = total_length;
        
        /*  The following produces more correct results in some cases and more broken in others.
            TODO: investigate, as it looks more reliable than line clipping. */
//.........这里部分代码省略.........

//--------------------------------------------------------------------------------------------------
/// 
//--------------------------------------------------------------------------------------------------
TEST(BoxGeneratorTest, GenerateSimple)
{
    // From min/max
    {
        BoxGenerator gen;
        gen.setMinMax(Vec3d(10, 20, 30), Vec3d(20, 40, 60));

        BuilderTrisQuads builder;
        gen.generate(&builder);
        BoundingBox bb = builder.vertexBoundingBox();
        EXPECT_DOUBLE_EQ(10, bb.min().x());
        EXPECT_DOUBLE_EQ(20, bb.min().y());
        EXPECT_DOUBLE_EQ(30, bb.min().z());
        EXPECT_DOUBLE_EQ(20, bb.max().x());
        EXPECT_DOUBLE_EQ(40, bb.max().y());
        EXPECT_DOUBLE_EQ(60, bb.max().z());

        EXPECT_EQ(0, builder.triCount());
        EXPECT_EQ(6, builder.quadCount());
    }

    // From origin and extent
    {
        BoxGenerator gen;
        gen.setOriginAndExtent(Vec3d(10, 20, 30), Vec3d(100, 200, 300));

        BuilderTrisQuads builder;
        gen.generate(&builder);
        BoundingBox bb = builder.vertexBoundingBox();
        EXPECT_DOUBLE_EQ(10,  bb.min().x());
        EXPECT_DOUBLE_EQ(20,  bb.min().y());
        EXPECT_DOUBLE_EQ(30,  bb.min().z());
        EXPECT_DOUBLE_EQ(110, bb.max().x());
        EXPECT_DOUBLE_EQ(220, bb.max().y());
        EXPECT_DOUBLE_EQ(330, bb.max().z());
    }

    // From center and extent
    {
        BoxGenerator gen;
        gen.setCenterAndExtent(Vec3d(100, 200, 300), Vec3d(10, 20, 30));

        BuilderTrisQuads builder;
        gen.generate(&builder);
        BoundingBox bb = builder.vertexBoundingBox();
        EXPECT_DOUBLE_EQ(95,  bb.min().x());
        EXPECT_DOUBLE_EQ(190, bb.min().y());
        EXPECT_DOUBLE_EQ(285,  bb.min().z());
        EXPECT_DOUBLE_EQ(105, bb.max().x());
        EXPECT_DOUBLE_EQ(210, bb.max().y());
        EXPECT_DOUBLE_EQ(315, bb.max().z());
    }
}

void
process_goturn_detection(Mat *img, double time_stamp)
{
	char string1[128];
	CvFont font;
	bool recovery = false;
	static int cont = 0;
	static int first = 0;
	int step = 10;

	//cv::Mat mat_image=cvarrToMat(&img);
	static Mat prev_image;
	prev_image = img->clone();
	if(box.x1_ != -1.0)
	{

		tracker.Track(*img, &regressor, &box);
		if (cont > 30)
		{
			calculate_box_average(box, last_track, img, step, cont);
//			recovery = true;
		}
		//Apenas para publicar-ainda falta definir
		average_box_confidence = 1.0;

		last_track[(cont%30)].prev_image = prev_image;
		last_track[(cont%30)].prev_box = box;
		box.DrawBoundingBox(img);
		cont++;

		tracker.Init(*img, box, &regressor);

	}

	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, .4, .5, 0, 1, 8);
//	cvRectangle(&prev_image, cvPoint(0, 0), cvPoint(img.width, 50), CV_RGB(0, 0, 255), CV_FILLED, 8, 0);
//	cvPutText(&prev_image.data, string1, cvPoint(25, 25), &font, CV_RGB(255, 255, 255));
	// draw the box
	sprintf(string1, "Time:%.2f, FPS:%d", time_stamp, disp_last_fps);
	cv::Size s = img->size();
	cv::Point textOrg(25,25);
	cv::rectangle(*img, cv::Point(0, 0), cv::Point(s.width, 50), Scalar::all(0), -1);
	cv::putText(*img, string1,textOrg,FONT_HERSHEY_SIMPLEX, 0.4,Scalar::all(255),1,8);

	cv::imshow(window_name, *img);

	char c = cv::waitKey(2);

	switch (c)
	{
	case 'r':

		CvRect rect;

		if (getBBFromUser(img, rect, window_name) == 0)
		{
			return;
		}

		box.x1_ = rect.x;
		box.y1_ = rect.y;
		box.x2_ = rect.x+rect.width;
		box.y2_ = rect.y+rect.height;
		last_track[(cont%30)].prev_image = prev_image;
		last_track[(cont%30)].prev_box = box;
		cont++;
		tracker.Init(*img, box, &regressor);
//		first = 1;
//		tracker.Track(*img, &regressor, &box);
		break;

	case 'q':
		exit(0);
	}
		// Track and estimate the bounding box location.

}

bool DynamicNavigationMesh::Build(const BoundingBox& boundingBox)
{
    URHO3D_PROFILE(BuildPartialNavigationMesh);

    if (!node_)
        return false;

    if (!navMesh_)
    {
        URHO3D_LOGERROR("Navigation mesh must first be built fully before it can be partially rebuilt");
        return false;
    }

    if (!node_->GetWorldScale().Equals(Vector3::ONE))
        URHO3D_LOGWARNING("Navigation mesh root node has scaling. Agent parameters may not work as intended");

    BoundingBox localSpaceBox = boundingBox.Transformed(node_->GetWorldTransform().Inverse());

    float tileEdgeLength = (float)tileSize_ * cellSize_;

    Vector<NavigationGeometryInfo> geometryList;
    CollectGeometries(geometryList);

    int sx = Clamp((int)((localSpaceBox.min_.x_ - boundingBox_.min_.x_) / tileEdgeLength), 0, numTilesX_ - 1);
    int sz = Clamp((int)((localSpaceBox.min_.z_ - boundingBox_.min_.z_) / tileEdgeLength), 0, numTilesZ_ - 1);
    int ex = Clamp((int)((localSpaceBox.max_.x_ - boundingBox_.min_.x_) / tileEdgeLength), 0, numTilesX_ - 1);
    int ez = Clamp((int)((localSpaceBox.max_.z_ - boundingBox_.min_.z_) / tileEdgeLength), 0, numTilesZ_ - 1);

    unsigned numTiles = 0;

    for (int z = sz; z <= ez; ++z)
    {
        for (int x = sx; x <= ex; ++x)
        {
            dtCompressedTileRef existing[TILECACHE_MAXLAYERS];
            const int existingCt = tileCache_->getTilesAt(x, z, existing, maxLayers_);
            for (int i = 0; i < existingCt; ++i)
            {
                unsigned char* data = 0x0;
                if (!dtStatusFailed(tileCache_->removeTile(existing[i], &data, 0)) && data != 0x0)
                    dtFree(data);
            }

            TileCacheData tiles[TILECACHE_MAXLAYERS];
            int layerCt = BuildTile(geometryList, x, z, tiles);
            for (int i = 0; i < layerCt; ++i)
            {
                dtCompressedTileRef tileRef;
                int status = tileCache_->addTile(tiles[i].data, tiles[i].dataSize, DT_COMPRESSEDTILE_FREE_DATA, &tileRef);
                if (dtStatusFailed((dtStatus)status))
                {
                    dtFree(tiles[i].data);
                    tiles[i].data = 0x0;
                }
                else
                {
                    tileCache_->buildNavMeshTile(tileRef, navMesh_);
                    ++numTiles;
                }
            }
        }
    }

    URHO3D_LOGDEBUG("Rebuilt " + String(numTiles) + " tiles of the navigation mesh");
    return true;
}