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