Beispiel #1
0
void Road::getLaneRoadPoints(double s, int i, RoadPoint &pointIn, RoadPoint &pointOut, double &disIn, double &disOut)
{
    LaneSection *section = (--laneSectionMap.upper_bound(s))->second;
    disIn = section->getDistanceToLane(s, i);
    disOut = disIn + section->getLaneWidth(s, i);
    //std::cerr << "s: " << s << ", i: " << i << ", distance: " << disIn << ", width: " << disOut-disIn << std::endl;

    Vector3D xyPoint = ((--xyMap.upper_bound(s))->second)->getPoint(s);
    Vector2D zPoint = ((--zMap.upper_bound(s))->second)->getPoint(s);

    double alpha = ((--lateralProfileMap.upper_bound(s))->second)->getAngle(s, (disOut + disIn) * 0.5);
    double beta = zPoint[1];
    double gamma = xyPoint[2];
    double Tx = (sin(alpha) * sin(beta) * cos(gamma) - cos(alpha) * sin(gamma));
    double Ty = (sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma));
    double Tz = (sin(alpha) * cos(beta));
    //std::cout << "s: " << s << ", i: " << i << ", alpha: " << alpha << ", beta: " << beta << ", gamma: " << gamma << std::endl;

    double nx = sin(alpha) * sin(gamma) + cos(alpha) * sin(beta) * cos(gamma);
    double ny = cos(alpha) * sin(beta) * sin(gamma) - sin(alpha) * cos(gamma);
    double nz = cos(alpha) * cos(beta);

    pointIn = RoadPoint(xyPoint.x() + Tx * disIn, xyPoint.y() + Ty * disIn, zPoint[0] + Tz * disIn, nx, ny, nz);
    pointOut = RoadPoint(xyPoint.x() + Tx * disOut, xyPoint.y() + Ty * disOut, zPoint[0] + Tz * disOut, nx, ny, nz);
    //std::cout << "s: " << s << ", i: " << i << ", inner: x: " << pointIn.x() << ", y: " << pointIn.y() << ", z: " << pointIn.z() << std::endl;
    //std::cout << "s: " << s << ", i: " << i << ", outer: x: " << pointOut.x() << ", y: " << pointOut.y() << ", z: " << pointOut.z() << std::endl << std::endl;
}
Beispiel #2
0
Vector2D Road::getLaneCenter(const int &l, const double &s)
{
    if (l != Lane::NOLANE)
    {
        LaneSection *section = (--laneSectionMap.upper_bound(s))->second;
        //std::cout << "getLaneCenter: Road: " << id << ", section: " << section << std::endl;
        return Vector2D(section->getDistanceToLane(s, l) + 0.5 * section->getLaneWidth(s, l), atan(0.5 * section->getLaneWidthSlope(s, l)));
    }
    else
    {
        return Vector2D(0.0, 0.0);
    }
}
Beispiel #3
0
osg::Geode *RoadObject::createOutlineGeode()
{
    osg::Geode *OutlineGeode = new osg::Geode();
    OutlineGeode->setName(name.c_str());

    double h = 4.5;
    double texlength = 4.5;
    double texwidth = 0.33;
    double railLength = length;
    if (repeatLength > 0)
        railLength = repeatLength;
    if (s + railLength > road->getLength())
    {
        railLength = road->getLength() - s;
    }
    if (repeatDistance > 0)
    {
        h = repeatDistance;
        texlength = repeatDistance;
    }

    osg::Geometry *OutlineGeometry;
    OutlineGeometry = new osg::Geometry();
    OutlineGeode->addDrawable(OutlineGeometry);

    OutlineGeometry->setUseDisplayList(true);
    OutlineGeometry->setUseVertexBufferObjects(false);

    osg::Vec3Array *OutlineVertices;
    OutlineVertices = new osg::Vec3Array;
    OutlineGeometry->setVertexArray(OutlineVertices);

    osg::Vec3Array *OutlineNormals;
    OutlineNormals = new osg::Vec3Array;
    OutlineGeometry->setNormalArray(OutlineNormals);
    OutlineGeometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX);

    osg::Vec2Array *OutlineTexCoords;
    OutlineTexCoords = new osg::Vec2Array;
    OutlineGeometry->setTexCoordArray(3, OutlineTexCoords);

    RoadPoint railPoint;
    RoadPoint railPoint2;
    RoadPoint nextRailPoint;

    osg::Vec3 n;

    int numVert = outline->size();
    int numSegments = numVert - 1;
    osg::Vec2 *profile = new osg::Vec2[numVert];
    float len=0;
    float *tcv = new float[numVert];
    for (int i = 0; i < numVert; i++)
    {
        profile[i] = osg::Vec2(outline->at(i).v, outline->at(i).z);
    }
    tcv[0]=0;
    for (int i = 1; i < numVert; i++)
    {
        len += (profile[i] - profile[i-1]).length();
        tcv[i]=len;
    }
    for (int i = 1; i < numVert; i++)
    {
        tcv[i]/= len;
    }


    LaneSection * startLaneSection = NULL;
    int startLaneId = 0;
    double sSection = s;
    double d = 0.0;
    int lengthSegments;
    double currentT;
    LaneSection * currentLaneSection;
    int currentLaneId;

    for (int pseg = 0; pseg < numSegments; pseg++)
    {

        lengthSegments = 0;
        currentT = t;
        currentLaneSection = startLaneSection;
        currentLaneId = startLaneId;

        for (double currentS = s; currentS <= s + railLength; currentS += h)
        {
            if (currentS > (s + railLength - (h)))
            {
                currentS = (s + railLength);
            }

            if (road->getLaneSection(currentS) != currentLaneSection)
            {
                LaneSection * newLaneSection = road->getLaneSection(currentS);
                while (currentLaneSection && (currentLaneSection != newLaneSection))
                {
                    if (t < 0)
                    {
                        currentT = -currentLaneSection->getLaneSpanWidth(0, currentLaneId, road->getLaneSectionEnd(currentLaneSection->getStart())) + d;
                    }
                    else if (t > 0)
                    {
                        currentT = currentLaneSection->getLaneSpanWidth(0, currentLaneId, road->getLaneSectionEnd(currentLaneSection->getStart())) + d;
                    }

                    currentLaneSection = road->getLaneSectionNext(currentLaneSection->getStart() + 1.0e-3);
                    currentLaneId = road->getLaneNumber(currentLaneSection->getStart(), currentT);
                    sSection = currentLaneSection->getStart();
                }

                currentLaneSection = newLaneSection;
                currentLaneId = road->getLaneNumber(sSection, currentT);
                if (t < 0)
                {
                    if (fabs(currentT) < currentLaneSection->getLaneSpanWidth(0, currentLaneId + 1, currentS) + currentLaneSection->getLaneWidth(currentS, currentLaneId)/2)
                    {
                        currentLaneId++;
                    }
                    d = currentLaneSection->getLaneSpanWidth(0, currentLaneId, currentS) + currentT;
                }
                else if (t >  0)
                {
                    if (currentT < currentLaneSection->getLaneSpanWidth(0, currentLaneId - 1,  currentS) + currentLaneSection->getLaneWidth(currentS, currentLaneId)/2)
                    {
                        currentLaneId--;
                    }
                    d = currentT -  currentLaneSection->getLaneSpanWidth(0, currentLaneId, currentS);
                }
            }



            if (t < 0)
            {
                currentT = -currentLaneSection->getLaneSpanWidth(0, currentLaneId, currentS) + d;
            }
            else if (t > 0)
            {
                currentT = currentLaneSection->getLaneSpanWidth(0, currentLaneId, currentS) + d;
            }



            nextRailPoint = road->getRoadPoint(currentS + h, currentT);
            osg::Vec3 p3(nextRailPoint.x(), nextRailPoint.y(), nextRailPoint.z());

            railPoint = road->getRoadPoint(currentS, currentT);
            if (t > 0)
                railPoint2 = road->getRoadPoint(currentS, currentT + 1.0);
            else
                railPoint2 = road->getRoadPoint(currentS, currentT - 1.0);
            n.set(railPoint2.x() - railPoint.x(), railPoint2.y() - railPoint.y(), railPoint2.z() - railPoint.z());
            n.normalize();
            osg::Vec3 negn = -n;
            osg::Vec3 p1(railPoint.x(), railPoint.y(), railPoint.z());
            p1 += n * profile[pseg].x();
            p1[2] += profile[pseg].y();
            OutlineVertices->push_back(osg::Vec3(p1.x(), p1.y(), p1.z()));
            OutlineTexCoords->push_back(osg::Vec2((currentS/texlength), tcv[pseg]));

            osg::Vec3 p2(railPoint.x(), railPoint.y(), railPoint.z());
            p2 += n * profile[pseg + 1].x();
            p2[2] += profile[pseg + 1].y();
            OutlineVertices->push_back(osg::Vec3(p2.x(), p2.y(), p2.z()));

            OutlineTexCoords->push_back(osg::Vec2((currentS/texlength), tcv[pseg+1]));
            lengthSegments += 2;
            osg::Vec3 up = p2 - p1;
            osg::Vec3 forward = p3 - p1;
            osg::Vec3 norm = up ^ forward;
            /*
            if(p1.z() == p2.z())
            {
            	norm.set(0,0,-1);
            }
            else if(p1.z() < p2.z())
            {
            	osg::Vec3 up = p2-p1;
            	up.normalize();
            	osg::Vec3 right;
            	if(t>=0)
            	{
            		right= up ^ negn;
            	}
            	else
            	{
            		right= up ^ n;
            	}
            	right.normalize();
            	norm = up ^ right;
            	norm.normalize();
            }
            else
            {
            	osg::Vec3 up = p1-p2;
            	up.normalize();
            	osg::Vec3 right;
            	if(t>=0)
            	{
            		right= up ^ negn;
            	}
            	else
            	{
            		right= up ^ n;
            	}
            	right.normalize();
            	norm = up ^ right;
            	norm.normalize();
            }
            */

            OutlineNormals->push_back(norm);
            OutlineNormals->push_back(norm);
        }

        osg::DrawArrays *OutlineBarrier = new osg::DrawArrays(osg::PrimitiveSet::TRIANGLE_STRIP, lengthSegments * pseg, lengthSegments);
        OutlineGeometry->addPrimitiveSet(OutlineBarrier);
    }

    osg::StateSet *OutlineStateSet = OutlineGeode->getOrCreateStateSet();

    OutlineStateSet->setMode(GL_LIGHTING, osg::StateAttribute::ON);
    if(numSegments>2)
    {
        osg::CullFace *cullFace = new osg::CullFace();
        cullFace->setMode(osg::CullFace::BACK);
        OutlineStateSet->setAttributeAndModes(cullFace, osg::StateAttribute::ON);
    }
    if(textureFileName!="")
    {
        const char *fileName = coVRFileManager::instance()->getName(textureFileName.c_str());
        if (fileName)
        {
            osg::Image *OutlineTexImage = osgDB::readImageFile(fileName);
            osg::Texture2D *OutlineTex = new osg::Texture2D;
            OutlineTex->setWrap(osg::Texture2D::WRAP_S, osg::Texture::REPEAT);
            OutlineTex->setWrap(osg::Texture2D::WRAP_T, osg::Texture::CLAMP);
            if (OutlineTexImage)
                OutlineTex->setImage(OutlineTexImage);
            OutlineStateSet->setTextureAttributeAndModes(3, OutlineTex, osg::StateAttribute::ON);
            osg::AlphaFunc *alphaFunc = new osg::AlphaFunc();
            alphaFunc->setFunction(osg::AlphaFunc::GEQUAL, 0.1f);

            OutlineStateSet->setAttributeAndModes(alphaFunc, osg::StateAttribute::ON);
        }
        else
        {
            std::cerr << "ERROR: no texture found named: "<< textureFileName<< std::endl;
        }
    }

    return OutlineGeode;
}
Beispiel #4
0
osg::Geode *RoadObject::createGuardRailGeode()
{
    osg::Geode *guardRailGeode = new osg::Geode();
    guardRailGeode->setName(name.c_str());

    double h = 4.5;
    double texlength = 4.5;
    double texwidth = 0.33;
    double railLength = length;
    if (repeatLength > 0)
        railLength = repeatLength;
    if (s + railLength > road->getLength())
    {
        railLength = road->getLength() - s;
    }
    if (repeatDistance > 0)
    {
        h = repeatDistance;
        texlength = repeatDistance;
    }

    osg::Geometry *guardRailGeometry;
    guardRailGeometry = new osg::Geometry();
    guardRailGeode->addDrawable(guardRailGeometry);

    guardRailGeometry->setUseDisplayList(true);
    guardRailGeometry->setUseVertexBufferObjects(false);

    osg::Vec3Array *guardRailVertices;
    guardRailVertices = new osg::Vec3Array;
    guardRailGeometry->setVertexArray(guardRailVertices);

    osg::Vec3Array *guardRailNormals;
    guardRailNormals = new osg::Vec3Array;
    guardRailGeometry->setNormalArray(guardRailNormals);
    guardRailGeometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX);

    osg::Vec2Array *guardRailTexCoords;
    guardRailTexCoords = new osg::Vec2Array;
    guardRailGeometry->setTexCoordArray(3, guardRailTexCoords);

    osg::Geometry *guardRailPostGeometry;
    guardRailPostGeometry = new osg::Geometry();

    guardRailGeode->addDrawable(guardRailPostGeometry);

    osg::Vec3Array *guardRailPostVertices;
    guardRailPostVertices = new osg::Vec3Array;
    guardRailPostGeometry->setVertexArray(guardRailPostVertices);

    osg::Vec2Array *guardRailPostTexCoords;
    guardRailPostTexCoords = new osg::Vec2Array;
    guardRailPostGeometry->setTexCoordArray(3, guardRailPostTexCoords);

    osg::Vec3Array *guardRailPostNormals;
    guardRailPostNormals = new osg::Vec3Array;
    guardRailPostGeometry->setNormalArray(guardRailPostNormals);
    guardRailPostGeometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX);

    bool up = true;
    if (s ==  0)
        up = false; // do not go up if we start right at the bginning of a road, then we continue from the last road
    bool down = false;

    RoadPoint railPoint;
    RoadPoint railPoint2;

    osg::Vec3 n;

    LaneSection * currentLaneSection = NULL;
    int currentLaneId = 0;
    double sSection = s;
    double d = 0.0;
    double currentT = t;

    for (double currentS = s; currentS <= s + railLength; currentS += h)
    {
        if (currentS > (s + railLength - (h)))
        {
            currentS = (s + railLength);
            if (fabs(s + railLength - road->getLength()) > 1.0e-6) // only go down if the rail does not extend to the end of the road.
            {
                down = true;
            }
        }

        if (road->getLaneSection(currentS) != currentLaneSection)
        {
            LaneSection * newLaneSection = road->getLaneSection(currentS);
            while (currentLaneSection && (currentLaneSection != newLaneSection))
            {
                if (t < 0)
                {
                    currentT = -currentLaneSection->getLaneSpanWidth(0, currentLaneId, road->getLaneSectionEnd(currentLaneSection->getStart())) + d;
                }
                else if (t > 0)
                {
                    currentT = currentLaneSection->getLaneSpanWidth(0, currentLaneId, road->getLaneSectionEnd(currentLaneSection->getStart())) + d;
                }

                currentLaneSection = road->getLaneSectionNext(currentLaneSection->getStart() + 1.0e-3);
                currentLaneId = road->getLaneNumber(currentLaneSection->getStart(), currentT);
                sSection = currentLaneSection->getStart();
            }

            currentLaneSection = newLaneSection;
            currentLaneId = road->getLaneNumber(sSection, currentT);

            if (t < 0)
            {
                if (fabs(currentT) < currentLaneSection->getLaneSpanWidth(0, currentLaneId + 1, currentS) + currentLaneSection->getLaneWidth(currentS, currentLaneId)/2)
                {
                    currentLaneId++;
                }
                d = currentLaneSection->getLaneSpanWidth(0, currentLaneId, currentS) + currentT;
            }
            else if (t >  0)
            {
                if (currentT < currentLaneSection->getLaneSpanWidth(0, currentLaneId - 1,  currentS) + currentLaneSection->getLaneWidth(currentS, currentLaneId)/2)
                {
                    currentLaneId--;
                }
                d = currentT -  currentLaneSection->getLaneSpanWidth(0, currentLaneId, currentS);
            }
        }


        if (t < 0)
        {
            currentT = -currentLaneSection->getLaneSpanWidth(0, currentLaneId, currentS) + d;
        }
        else if (t > 0)
        {
            currentT = currentLaneSection->getLaneSpanWidth(0, currentLaneId, currentS) + d;
        }

        railPoint = road->getRoadPoint(currentS, currentT);
        if (t > 0)
            railPoint2 = road->getRoadPoint(currentS, currentT + 1.0);
        else
            railPoint2 = road->getRoadPoint(currentS, currentT - 1.0);
        n.set(railPoint2.x() - railPoint.x(), railPoint2.y() - railPoint.y(), railPoint2.z() - railPoint.z());
        n.normalize();
        osg::Vec3 negn = -n;

        if (up || down)
        {

            guardRailVertices->push_back(osg::Vec3(railPoint.x(), railPoint.y(), railPoint.z() - 0.31));
            guardRailTexCoords->push_back(osg::Vec2((currentS / texlength), 0));

            guardRailVertices->push_back(osg::Vec3(railPoint.x(), railPoint.y(), railPoint.z()));
            guardRailTexCoords->push_back(osg::Vec2((currentS / texlength), 0.33 / texwidth));
        }
        else
        {
            guardRailVertices->push_back(osg::Vec3(railPoint.x(), railPoint.y(), railPoint.z() + 0.44));
            guardRailTexCoords->push_back(osg::Vec2((currentS / texlength), 0));

            guardRailVertices->push_back(osg::Vec3(railPoint.x(), railPoint.y(), railPoint.z() + 0.75));
            guardRailTexCoords->push_back(osg::Vec2((currentS / texlength), 0.33 / texwidth));

            // post

            osg::Vec3 p1, p2, p3, p4;
            osg::Vec3 n2;
            n2.set(railPoint.nx(), railPoint.ny(), railPoint.nz());
            n2.normalize();
            osg::Vec3 n3 = n2 ^ n;
            n3.normalize();

            p1.set(railPoint.x(), railPoint.y(), railPoint.z());
            p1 += n * 0.01;
            p2 = p1 + (n3 * 0.08);
            p3 = p2 + (n * 0.06);
            p4 = p1 + (n * 0.06);

            osg::Vec3 p5, p6, p7, p8;
            p5.set(p1.x(), p1.y(), p1.z() + 0.44);
            p6.set(p2.x(), p2.y(), p2.z() + 0.44);

            guardRailPostVertices->push_back(p1);
            guardRailPostTexCoords->push_back(osg::Vec2(0.02, 0.02));
            guardRailPostNormals->push_back(n);
            guardRailPostVertices->push_back(p2);
            guardRailPostTexCoords->push_back(osg::Vec2(0.02, 0.1));
            guardRailPostNormals->push_back(n);
            guardRailPostVertices->push_back(p6);
            guardRailPostTexCoords->push_back(osg::Vec2(0.1, 0.1));
            guardRailPostNormals->push_back(n);
            guardRailPostVertices->push_back(p5);
            guardRailPostTexCoords->push_back(osg::Vec2(0.1, 0.02));
            guardRailPostNormals->push_back(n);

            p5.set(p1.x(), p1.y(), p1.z() + 0.6);
            p6.set(p2.x(), p2.y(), p2.z() + 0.6);
            p7.set(p3.x(), p3.y(), p3.z() + 0.6);
            p8.set(p4.x(), p4.y(), p4.z() + 0.6);

            osg::Vec3 negn3 = -n3;

            guardRailPostVertices->push_back(p2);
            guardRailPostTexCoords->push_back(osg::Vec2(0.02, 0.02));
            guardRailPostNormals->push_back(negn3);
            guardRailPostVertices->push_back(p3);
            guardRailPostTexCoords->push_back(osg::Vec2(0.02, 0.1));
            guardRailPostNormals->push_back(negn3);
            guardRailPostVertices->push_back(p7);
            guardRailPostTexCoords->push_back(osg::Vec2(0.1, 0.1));
            guardRailPostNormals->push_back(negn3);
            guardRailPostVertices->push_back(p6);
            guardRailPostTexCoords->push_back(osg::Vec2(0.1, 0.02));
            guardRailPostNormals->push_back(negn3);

            guardRailPostVertices->push_back(p1);
            guardRailPostTexCoords->push_back(osg::Vec2(0.02, 0.02));
            guardRailPostNormals->push_back(negn3);
            guardRailPostVertices->push_back(p4);
            guardRailPostTexCoords->push_back(osg::Vec2(0.02, 0.1));
            guardRailPostNormals->push_back(negn3);
            guardRailPostVertices->push_back(p8);
            guardRailPostTexCoords->push_back(osg::Vec2(0.1, 0.1));
            guardRailPostNormals->push_back(negn3);
            guardRailPostVertices->push_back(p5);
            guardRailPostTexCoords->push_back(osg::Vec2(0.1, 0.02));
            guardRailPostNormals->push_back(negn3);
        }

        if (t > 0)
        {
            guardRailNormals->push_back(n);
            guardRailNormals->push_back(n);
        }
        else
        {
            guardRailNormals->push_back(negn);
            guardRailNormals->push_back(negn);
        }

        up = false;
    }

    osg::DrawArrays *guardRailPosts = new osg::DrawArrays(osg::PrimitiveSet::QUADS, 0, guardRailPostVertices->size());
    guardRailPostGeometry->addPrimitiveSet(guardRailPosts);

    osg::DrawArrays *guardRail = new osg::DrawArrays(osg::PrimitiveSet::TRIANGLE_STRIP, 0, guardRailVertices->size());
    guardRailGeometry->addPrimitiveSet(guardRail);

    osg::StateSet *guardRailStateSet = guardRailGeode->getOrCreateStateSet();

    guardRailStateSet->setMode(GL_LIGHTING, osg::StateAttribute::ON);
    //guardRailStateSet->setMode ( GL_LIGHT0, osg::StateAttribute::ON );
    //guardRailStateSet->setMode ( GL_LIGHT1, osg::StateAttribute::ON);

    const char *fileName = coVRFileManager::instance()->getName("share/covise/materials/guardRailTex.jpg");
    if (fileName)
    {
        osg::Image *guardRailTexImage = osgDB::readImageFile(fileName);
        osg::Texture2D *guardRailTex = new osg::Texture2D;
        guardRailTex->setWrap(osg::Texture2D::WRAP_S, osg::Texture::REPEAT);
        guardRailTex->setWrap(osg::Texture2D::WRAP_T, osg::Texture::REPEAT);
        if (guardRailTexImage)
            guardRailTex->setImage(guardRailTexImage);
        guardRailStateSet->setTextureAttributeAndModes(3, guardRailTex, osg::StateAttribute::ON);
    }
    else
    {
        std::cerr << "ERROR: no texture found named: share/covise/materials/guardRailTex.jpg";
    }

    return guardRailGeode;
}