rosimporter.cpp

rcssserver3d Robocup 3D比赛官方指定平台

    if (
        (! ReadAttribute(element, RA_NAME, name, true)) ||
        (! ReadTrans(element, trans)) ||
        (! ReadAppearance(element, appear)) ||
        (! ReadPhysical(element, physical))
        )
        {
            return false;
        }

    // look for an inlined vertex list and parse it
    TiXmlElement* vertListElem =
        GetFirstChild(element, RosElements::RE_VERTEXLIST);

    if (
        (vertListElem != 0) &&
        (! ReadVertexList(vertListElem))
        )
        {
            return false;
        }

    shared_ptr<Transform> contextTransform = GetContextTransform(parent, Trans());
    shared_ptr<Transform> transform = CreateTransform(contextTransform, trans);
    transform->SetName(name);

    if (! ReadPhysicalRep(parent,element))
        {
            return false;
        }

    shared_ptr<TriMesh> mesh(new TriMesh);
    mesh->SetName(name);

    if (! ReadGraphicalRep(element, mesh, appear))
        {
            return false;
        }

    geometryServer->RegisterMesh(mesh);
    shared_ptr<StaticMesh> staticMesh = shared_dynamic_cast<StaticMesh>
        (GetCore()->New("/kerosin/StaticMesh"));

    transform->AddChildReference(staticMesh);
    staticMesh->Load(name);

    GetLog()->Debug() << "(RosImporter) read complex shape " << name << "\n";
    return ReadChildElements(transform, element);
}

bool RosImporter::ReadGraphicalRep(TiXmlElement* element, shared_ptr<TriMesh> mesh, const Appearance& appear)
{
    TiXmlElement* graphRepElem = GetFirstChild(element, RosElements::RE_GRAPHICALREPRESENTATION);
    if (graphRepElem == 0)
        {
            string name = S_UNNAMED;
            ReadAttribute(element, RA_NAME, name, true);

            GetLog()->Error() << "(RosImporter) ERROR: missing graphical representation in "
                              << GetXMLPath(element) << " name " << name << " \n";

            return false;
        }

    string vertexListName;
    if (! ReadAttribute(graphRepElem, RA_VERTEXLIST, vertexListName))
        {
            return false;
        }

    TVertexListMap::iterator iter = mVertexListMap.find(vertexListName);
    if (iter == mVertexListMap.end())
        {
            string name;
            ReadAttribute(element, RA_NAME, name, true);

            GetLog()->Error() << "(RosImporter) ERROR: undefined vertex list " << vertexListName << " in "
                              << GetXMLPath(element) << " name " << name << " \n";

            return false;
        }

    TComplexGeomList geomList;
    if (! ReadComplexElements(graphRepElem, geomList))
        {
            return false;
        }

    TVertexList& vertexList = (*iter).second;
    BuildTriMesh(mesh, vertexList, geomList, appear);

    GetLog()->Debug() << "(RosImporter) read graphical representation\n";

    return true;
}

bool RosImporter::ReadComplexElements(TiXmlElement* element, TComplexGeomList& geomList)
{
    for (
         TiXmlNode* node = element->FirstChild();
         node != 0;
         node = element->IterateChildren(node)
         )
        {
            if (IgnoreNode(node))
                {
                    continue;
                }

            TiXmlElement* element = static_cast<TiXmlElement*>(node);

            RosElements::ERosElement type = GetType(element);
            switch (type)
                {
                default:
                    GetLog()->Error() << "(RosImporter::ReadComplexElements) ERROR: skipping unknown element "
                                      << GetXMLPath(element) << "\n";
                    break;

                case RosElements::RE_POLYGON:
                    {
                        ComplexGeom geom(CG_Polygon);
                        if (! ReadComplexGeom(element, geom))
                            {
                                return false;
                            }

                        geomList.push_back(geom);
                        break;
                    }

                case RosElements::RE_TRIANGLESTRIP:
                    {
                        ComplexGeom geom(CG_TriangleStrip);
                        if (! ReadComplexGeom(element, geom))
                            {
                                return false;
                            }

                        geomList.push_back(geom);
                        break;
                    }
                }
        }

    return true;
}

bool RosImporter::ReadComplexGeom(TiXmlElement* element, ComplexGeom& geom)
{
    for (
         TiXmlNode* node = GetFirstChild(element, RosElements::RE_VERTEX);
         node != 0;
         node = element->IterateChildren(node)
         )
        {
            TiXmlElement* element = static_cast<TiXmlElement*>(node);

            RosElements::ERosElement type = GetType(element);
            switch (type)
                {
                default:
                    GetLog()->Error() << "(RosImporter::ReadComplexGeom) ERROR: skipping unknown element "
                                      << GetXMLPath(element) << "\n";
                    break;

                case RosElements::RE_VERTEX:
                    {
                        string ref;
                        if (! ReadAttribute(element, RA_REF, ref))
                            {
                                return false;
                            }

                        geom.vertices.push_back(ref);

                        break;
                    }
                }
        }

    //    std::reverse(geom.vertices.begin(), geom.vertices.end());
    return true;
}

void RosImporter::BuildTriMesh(shared_ptr<TriMesh> mesh, TVertexList& vertexList,
                               const TComplexGeomList& geomList, const Appearance& appear)
{
    GetLog()->Debug() << "(RosImporter) building trimesh for " << mesh->GetName() << "\n";
    mesh->SetPos(vertexList.GetPos(), vertexList.GetPosCount());

    shared_ptr<IndexBuffer> ibuffer(new IndexBuffer);
    IndexBuffer& ibufferRef = (*ibuffer);

    // build triangles
    for (
         TComplexGeomList::const_iterator iter = geomList.begin();
         iter != geomList.end();
         ++iter
         )
        {
            const ComplexGeom& geom = (*iter);

            switch (geom.type)
                {
                default:
                    continue;

                case CG_Polygon:
                    BuildPolygon(ibufferRef, vertexList, geom);
                    break;
                }
        }

    mesh->AddFace(ibuffer,appear.ref);
}

void RosImporter::BuildPolygon(IndexBuffer& ibuffer, TVertexList& vertexList, const ComplexGeom& geom)
{
    int nVerts = static_cast<int>(geom.vertices.size());
    int j = (nVerts - 2);

    for (int i = 0;i<j;++i)
        {
            ibuffer.Cache(vertexList.GetIndex(geom.vertices[0]));
            ibuffer.Cache(vertexList.GetIndex(geom.vertices[i+1]));
            ibuffer.Cache(vertexList.GetIndex(geom.vertices[i+2]));
        }
}

bool RosImporter::ReadPhysicalRep(shared_ptr<BaseNode> parent, TiXmlElement* element)
{
    string name = S_UNNAMED;
    ReadAttribute(element, RA_NAME, name, true);

    TiXmlElement* physicalRep
        = GetFirstChild(element, RosElements::RE_PHYSICALREPRESENTATION);

    if (physicalRep == 0)
        {
            GetLog()->Error() << "(RosImporter) ERROR: missing physical representation in "
                              << GetXMLPath(element) << " name " << name << " \n";

            return false;
        }

    for (
         TiXmlNode* node = physicalRep->FirstChild();
         node != 0;
         node = physicalRep->IterateChildren(node)
         )
        {
            if (IgnoreNode(node))
                {
                    continue;
                }

            TiXmlElement* element = static_cast<TiXmlElement*>(node);

            RosElements::ERosElement type = GetType(element);
            switch (type)
                {
                default:
                    GetLog()->Error() << "(RosImporter::ReadPhysicalRep) ERROR: skipping unknown element "
                                      << GetXMLPath(element) << "\n";
                    break;

                case RosElements::RE_SIMPLEBOX:
                    if (! ReadSimpleBox(parent, element))
                        {
                            return false;
                        }
                    break;

                case RosElements::RE_SIMPLESPHERE:
                    if (!  ReadSimpleSphere(parent, element))
                        {
                            return false;
                        }
                    break;

                case RosElements::RE_SIMPLECYLINDER:
                    //! simulate cylinder with a capped cylinder
                case RosElements::RE_SIMPLECAPPEDCYLINDER:
                    if (! ReadSimpleCappedCylinder(parent, element))
                        {
                            return false;
                        }
                    break;
                }
        }

    GetLog()->Debug() << "(RosImporter) read physical representation\n";
    return true;
}

bool RosImporter::ReadSimpleBox(shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element)
{
    string name;
    double length;
    double width;
    double height;
    Trans trans;
    Physical physical;

    if (
        (! ReadAttribute(element, RA_NAME, name, true)) ||
        (! ReadAttribute(element, RA_LENGTH, length)) ||
        (! ReadAttribute(element, RA_WIDTH, width)) ||
        (! ReadAttribute(element, RA_HEIGHT, height)) ||
        (! ReadTrans(element, trans)) ||
        (! ReadPhysical(element, physical))
        )
        {
            return false;
        }

    // transform
    shared_ptr<Transform> contextTransform = GetContextTransform(parent, trans);
    Vector3f boxDim = Vector3f(length, width, height);

    shared_ptr<Body> body = GetContextBody(contextTransform);
    if (body.get() != 0)
        {
            body->AddBoxTotal(physical.mass, boxDim, trans.matrix);
            GetContext().AddMass(physical.mass, trans);
        }

    if (physical.canCollide)
        {
            // geometry
            shared_ptr<TransformCollider> transCollider
                = CreateTransformCollider(contextTransform,trans);
            transCollider->SetName(S_GEOMTRANS+name);

            shared_ptr<BoxCollider> collider = shared_dynamic_cast<BoxCollider>
                (GetCore()->New("/oxygen/BoxCollider"));

            transCollider->AddChildReference(collider);
            collider->SetName(S_GEOM+name);
            collider->SetBoxLengths(boxDim);

            // collision handler
            shared_ptr<ContactJointHandler> handler = CreateContactJointHandler();
            collider->AddChildReference(handler);
        }

    GetLog()->Debug() << "(RosImporter) created simple box " << name << "\n";
    return true;
}

bool RosImporter::ReadSimpleSphere(shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element)
{
    string name;
    double radius;
    Trans trans;
    Physical physical;

    if (
        (! ReadAttribute(element, RA_NAME, name, true)) ||
        (! ReadAttribute(element, RA_RADIUS, radius)) ||
        (! ReadTrans(element, trans)) ||
        (! ReadPhysical(element, physical))
        )
        {
            return false;
        }

    // transform
    shared_ptr<Transform> contextTransform = GetContextTransform(parent, trans);

    shared_ptr<Body> body = GetContextBody(contextTransform);
    if (body.get() != 0)
        {
            body->AddSphereTotal(physical.mass, radius, trans.matrix);
            GetContext().AddMass(physical.mass, trans);
        }

    if (physical.canCollide)
        {
            // geometry
            shared_ptr<TransformCollider> transCollider
                = CreateTransformCollider(body,trans);
            transCollider->SetName(S_GEOMTRANS+name);

            shared_ptr<SphereCollider> collider = shared_dynamic_cast<SphereCollider>
                (GetCore()->New("/oxygen/SphereCollider"));

            transCollider->AddChildReference(collider);
            collider->SetRadius(radius);

            // collision handler
            shared_ptr<ContactJointHandler> handler = CreateContactJointHandler();
            collider->AddChildReference(handler);
        }

    GetLog()->Debug() << "(RosImporter) created simple sphere " << name << "\n";
    return true;
}

bool RosImporter::ReadSimpleCappedCylinder(shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element)
{
    string name;
    double radius;
    double height;
    Trans trans;
    Physical physical;

    if (
        (! ReadAttribute(element, RA_NAME, name, true)) ||
        (! ReadAttribute(element, RA_RADIUS, radius)) ||
        (! ReadAttribute(element, RA_HEIGHT, height)) ||
        (! ReadTrans(element, trans)) ||
        (! ReadPhysical(element, physical))
        )
        {
            return false;
        }

    // transform
    shared_ptr<Transform> contextTransform = GetContextTransform(parent, trans);

    shared_ptr<Body> body = GetContextBody(contextTransform);
    if (body.get() != 0)
        {
            body->AddCappedCylinderTotal(physical.mass, radius, height, trans.matrix);
            GetContext().AddMass(physical.mass, trans);
        }

    if (physical.canCollide)
        {
            // geometry
            shared_ptr<TransformCollider> transCollider
                = CreateTransformCollider(body,trans);
            transCollider->SetName(S_GEOMTRANS+name);

            shared_ptr<CCylinderCollider> collider = shared_dynamic_cast<CCylinderCollider>
                (GetCore()->New("/oxygen/CCylinderCollider"));

            transCollider->AddChildReference(collider);
            collider->SetName(S_GEOM+name);
            collider->SetParams(radius, height);

            // collision handler
            shared_ptr<ContactJointHandler> handler = CreateContactJointHandler();
            collider->AddChildReference(handler);
        }

    GetLog()->Debug() << "(RosImporter) created simple capped cylinder " << name << "\n";
    return true;
}

void RosImporter::Attach(shared_ptr<Joint> joint, shared_ptr<Body> body1, shared_ptr<Body> body2,
                         const JointAxis& axis1, const JointAxis& axis2)
{
    if (joint.get() == 0)
        {
            assert(false);
            return;
        }

    JointAttach ja;
    ja.joint = joint;
    ja.body1 = body1;
    ja.body2 = body2;
    ja.axis1 = axis1;
    ja.axis2 = axis2;

    AttachJoint(ja);
}

void RosImporter::AttachJoint(const JointAttach& ja)
{
    ja.joint->Attach(ja.body1, ja.body2);

    shared_ptr<HingeJoint> hinge = shared_dynamic_cast<HingeJoint>(ja.joint);
    if (hinge.get() != 0)
        {
            hinge->SetAxis(ja.axis1.dir);
            hinge->SetAnchor(Vector3f(0.0,0.0,0.0));

            if (ja.axis1.setDeflection)
                {
                    hinge->SetParameter(dParamLoStop, ja.axis1.loStop);
                    hinge->SetParameter(dParamHiStop, ja.axis1.hiStop);
                    hinge->SetParameter(dParamLoStop, ja.axis1.loStop);
                }

            return;
        }

    shared_ptr<UniversalJoint> universal = shared_dynamic_cast<UniversalJoint>(ja.joint);
    if (universal.get() != 0)
        {
            universal->SetAxis1(ja.axis1.dir);
            universal->SetAxis2(ja.axis2.dir);
            universal->SetAnchor(Vector3f(0.0,0.0,0.0));

            if (ja.axis1.setDeflection)
                {
                    universal->SetParameter(dParamLoStop, ja.axis1.loStop);
                    universal->SetParameter(dParamHiStop, ja.axis1.hiStop);
                    universal->SetParameter(dParamLoStop, ja.axis1.loStop);
                }

            if (ja.axis2.setDeflection)
                {
                    universal->SetParameter(dParamLoStop2, ja.axis2.loStop);
                    universal->SetParameter(dParamHiStop2, ja.axis2.hiStop);
                    universal->SetParameter(dParamLoStop2, ja.axis2.loStop);
                }

            return;
        }

    shared_ptr<SliderJoint> slider = shared_dynamic_cast<SliderJoint>(ja.joint);
    if (slider.get() != 0)
        {
            // slider axis is set via parent transform node
            // slider->SetAxis(ja.axis1);
            return;
        }

    assert(false);
}