407 lines
11 KiB
C++
407 lines
11 KiB
C++
/*
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*
|
|
*/
|
|
|
|
#ifdef _WIN32
|
|
// Ensure that Winsock2.h is included before Windows.h, which can get
|
|
// pulled in by anybody (e.g., Boost).
|
|
#include <Winsock2.h>
|
|
#endif
|
|
|
|
#include <boost/bind.hpp>
|
|
#include <algorithm>
|
|
|
|
#include "gazebo/transport/transport.hh"
|
|
#include "gazebo/rendering/Conversions.hh"
|
|
#include "gazebo/rendering/Road2d.hh"
|
|
#include "gazebo/rendering/RenderEngine.hh"
|
|
|
|
using namespace gazebo;
|
|
using namespace rendering;
|
|
|
|
/////////////////////////////////////////////////
|
|
Road2d::Road2d()
|
|
{
|
|
this->node = transport::NodePtr(new transport::Node());
|
|
this->node->Init();
|
|
this->sub = this->node->Subscribe("~/roads", &Road2d::OnRoadMsg, this, true);
|
|
|
|
this->connections.push_back(
|
|
event::Events::ConnectPreRender(boost::bind(&Road2d::PreRender, this)));
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
Road2d::~Road2d()
|
|
{
|
|
this->sub.reset();
|
|
this->node.reset();
|
|
this->parent.reset();
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Road2d::Load(VisualPtr _parent)
|
|
{
|
|
this->parent = _parent;
|
|
|
|
/* this->points.push_back(math::Vector3(0, 0, 0.01));
|
|
this->points.push_back(math::Vector3(4, 4, 0.01));
|
|
this->points.push_back(math::Vector3(4, 8, 0.01));
|
|
this->points.push_back(math::Vector3(8, 8, 0.01));
|
|
this->points.push_back(math::Vector3(20, 0, 0.01));
|
|
this->points.push_back(math::Vector3(10, -20, 0.01));
|
|
|
|
this->mRenderOp.vertexData = new Ogre::VertexData;
|
|
this->mRenderOp.operationType = Ogre::RenderOperation::OT_TRIANGLE_STRIP;
|
|
this->mRenderOp.indexData = 0;
|
|
this->mRenderOp.useIndexes = false;
|
|
|
|
Ogre::VertexDeclaration *vertexDecl =
|
|
this->mRenderOp.vertexData->vertexDeclaration;
|
|
|
|
size_t currOffset = 0;
|
|
|
|
// Positions
|
|
vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
|
|
currOffset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
|
|
|
|
// Normals
|
|
vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT3, Ogre::VES_NORMAL);
|
|
currOffset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
|
|
|
|
// two dimensional ->roadObjtexture coordinates
|
|
vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT2,
|
|
Ogre::VES_TEXTURE_COORDINATES, 0);
|
|
|
|
// allocate the vertex buffer
|
|
this->mRenderOp.vertexData->vertexCount = this->points.size() * 2;
|
|
Ogre::HardwareVertexBufferSharedPtr vBuf =
|
|
Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(
|
|
vertexDecl->getVertexSize(0), this->mRenderOp.vertexData->vertexCount,
|
|
Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY, false);
|
|
|
|
Ogre::VertexBufferBinding *binding =
|
|
this->mRenderOp.vertexData->vertexBufferBinding;
|
|
binding->setBinding(0, vBuf);
|
|
float *vertices = static_cast<float*>(
|
|
vBuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));
|
|
|
|
math::Vector3 pA, pB, tangent;
|
|
double factor = 1.0;
|
|
double theta = 0.0;
|
|
|
|
math::Box bounds;
|
|
bounds.min.Set(GZ_DBL_MAX, GZ_DBL_MAX, GZ_DBL_MAX);
|
|
bounds.max.Set(GZ_DBL_MIN, GZ_DBL_MIN, GZ_DBL_MIN);
|
|
|
|
// Generate the triangles for the road
|
|
for (unsigned int i = 0; i < this->points.size(); ++i)
|
|
{
|
|
factor = 1.0;
|
|
|
|
if (i == 0)
|
|
{
|
|
tangent = (this->points[i+1] - this->points[i]).Normalize();
|
|
}
|
|
else if (i == this->points.size() - 1)
|
|
{
|
|
tangent = (this->points[i] - this->points[i-1]).Normalize();
|
|
}
|
|
else
|
|
{
|
|
math::Vector3 v1 = (this->points[i+1] - this->points[i]).Normalize();
|
|
math::Vector3 v0 = (this->points[i] - this->points[i-1]).Normalize();
|
|
double dot = v0.Dot(v1 * -1);
|
|
tangent = (this->points[i+1] - this->points[i-1]).Normalize();
|
|
|
|
// Check to see if the points are not colinear
|
|
if (dot > -.97 && dot < 0.97)
|
|
{
|
|
factor = 1.0 / sin(acos(dot) * 0.5);
|
|
}
|
|
}
|
|
|
|
theta = atan2(tangent.x, -tangent.y);
|
|
|
|
pA = pB = this->points[i];
|
|
double w = (this->width * factor) * 0.5;
|
|
|
|
pA.x += cos(theta) * w;
|
|
pA.y += sin(theta) * w;
|
|
|
|
pB.x -= cos(theta) * w;
|
|
pB.y -= sin(theta) * w;
|
|
|
|
bounds.min.SetToMin(pA);
|
|
bounds.min.SetToMin(pB);
|
|
|
|
bounds.max.SetToMax(pA);
|
|
bounds.max.SetToMax(pB);
|
|
|
|
// Position
|
|
*vertices++ = pA.x;
|
|
*vertices++ = pA.y;
|
|
*vertices++ = pA.z;
|
|
|
|
// Normal
|
|
*vertices++ = 0;
|
|
*vertices++ = 0;
|
|
*vertices++ = 1;
|
|
|
|
// UV Coord
|
|
*vertices++ = 0;
|
|
*vertices++ = 1;
|
|
|
|
// Position
|
|
*vertices++ = pB.x;
|
|
*vertices++ = pB.y;
|
|
*vertices++ = pB.z;
|
|
|
|
// Normal
|
|
*vertices++ = 0;
|
|
*vertices++ = 0;
|
|
*vertices++ = 1;
|
|
|
|
// UV Coord
|
|
*vertices++ = 1;
|
|
*vertices++ = 1;
|
|
}
|
|
|
|
this->mBox.setExtents(Conversions::Convert(bounds.min),
|
|
Conversions::Convert(bounds.max));
|
|
|
|
vBuf->unlock();
|
|
this->parent->AttachObject(this);
|
|
|
|
this->setMaterial("Gazebo/Road");
|
|
*/
|
|
}
|
|
|
|
|
|
//////////////////////////////////////////////////
|
|
void Road2d::PreRender()
|
|
{
|
|
for (RoadMsgs_L::iterator iter = this->msgs.begin();
|
|
iter != this->msgs.end(); ++iter)
|
|
{
|
|
Road2d::Segment *segment = new Road2d::Segment;
|
|
segment->Load(**iter);
|
|
this->parent->AttachObject(segment);
|
|
this->segments.push_back(segment);
|
|
}
|
|
this->msgs.clear();
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Road2d::OnRoadMsg(ConstRoadPtr &_msg)
|
|
{
|
|
this->msgs.push_back(_msg);
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Road2d::Segment::Load(msgs::Road _msg)
|
|
{
|
|
this->width = _msg.width();
|
|
|
|
for (int i = 0; i < _msg.point_size(); ++i)
|
|
{
|
|
this->points.push_back(msgs::ConvertIgn(_msg.point(i)));
|
|
}
|
|
|
|
this->mRenderOp.vertexData = new Ogre::VertexData;
|
|
this->mRenderOp.operationType = Ogre::RenderOperation::OT_TRIANGLE_STRIP;
|
|
this->mRenderOp.indexData = 0;
|
|
this->mRenderOp.useIndexes = false;
|
|
|
|
Ogre::VertexDeclaration *vertexDecl =
|
|
this->mRenderOp.vertexData->vertexDeclaration;
|
|
|
|
size_t currOffset = 0;
|
|
|
|
// Positions
|
|
vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
|
|
currOffset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
|
|
|
|
// Normals
|
|
vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT3, Ogre::VES_NORMAL);
|
|
currOffset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
|
|
|
|
// two dimensional ->roadObjtexture coordinates
|
|
vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT2,
|
|
Ogre::VES_TEXTURE_COORDINATES, 0);
|
|
|
|
// allocate the vertex buffer
|
|
this->mRenderOp.vertexData->vertexCount = this->points.size() * 2;
|
|
Ogre::HardwareVertexBufferSharedPtr vBuf =
|
|
Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(
|
|
vertexDecl->getVertexSize(0), this->mRenderOp.vertexData->vertexCount,
|
|
Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY, false);
|
|
|
|
Ogre::VertexBufferBinding *binding =
|
|
this->mRenderOp.vertexData->vertexBufferBinding;
|
|
binding->setBinding(0, vBuf);
|
|
float *vertices = static_cast<float*>(
|
|
vBuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));
|
|
|
|
ignition::math::Vector3d pA, pB, tangent;
|
|
|
|
math::Box bounds;
|
|
bounds.min.Set(GZ_DBL_MAX, GZ_DBL_MAX, GZ_DBL_MAX);
|
|
bounds.max.Set(GZ_DBL_MIN, GZ_DBL_MIN, GZ_DBL_MIN);
|
|
|
|
// length for each texture tile, same as road width as texture is square
|
|
// (if texture size should change or made custom in a future version
|
|
// there needs to be code to handle this)
|
|
double texMaxLen = this->width;
|
|
|
|
// current road length
|
|
double curLen = 0.0;
|
|
|
|
// Generate the triangles for the road
|
|
for (unsigned int i = 0; i < this->points.size(); ++i)
|
|
{
|
|
double factor = 1.0;
|
|
|
|
// update current road length
|
|
if (i > 0)
|
|
{
|
|
curLen += this->points[i].Distance(this->points[i-1]);
|
|
}
|
|
|
|
// assign texture coordinate as percentage of texture tile size
|
|
// and let ogre/opengl handle the texture wrapping
|
|
float texCoord = curLen/texMaxLen;
|
|
|
|
// Start point is a special case
|
|
if (i == 0)
|
|
{
|
|
tangent = (this->points[i+1] - this->points[i]).Normalize();
|
|
}
|
|
// End point is a special case
|
|
else if (i == this->points.size() - 1)
|
|
{
|
|
tangent = (this->points[i] - this->points[i-1]).Normalize();
|
|
}
|
|
// Every other point in the road
|
|
else
|
|
{
|
|
math::Vector3 v1 = (this->points[i+1] - this->points[i]).Normalize();
|
|
math::Vector3 v0 = (this->points[i] - this->points[i-1]).Normalize();
|
|
double dot = v0.Dot(v1 * -1);
|
|
tangent = (this->points[i+1] - this->points[i-1]).Normalize();
|
|
|
|
// Check to see if the points are not colinear
|
|
// If not colinear, then the road needs to be widended for the turns
|
|
if (dot > -.97 && dot < 0.97)
|
|
{
|
|
factor = 1.0 / sin(acos(dot) * 0.5);
|
|
}
|
|
}
|
|
|
|
// The tangent is used to calculate the two verteces to either side of
|
|
// the point. The vertices define the triangle mesh of the road
|
|
double theta = atan2(tangent.X(), -tangent.Y());
|
|
|
|
pA = pB = this->points[i];
|
|
double w = (this->width * factor) * 0.5;
|
|
|
|
pA.X() += cos(theta) * w;
|
|
pA.Y() += sin(theta) * w;
|
|
|
|
pB.X() -= cos(theta) * w;
|
|
pB.Y() -= sin(theta) * w;
|
|
|
|
bounds.min.SetToMin(pA);
|
|
bounds.min.SetToMin(pB);
|
|
|
|
bounds.max.SetToMax(pA);
|
|
bounds.max.SetToMax(pB);
|
|
|
|
// Position
|
|
*vertices++ = pA.X();
|
|
*vertices++ = pA.Y();
|
|
*vertices++ = pA.Z();
|
|
|
|
// Normal
|
|
*vertices++ = 0;
|
|
*vertices++ = 0;
|
|
*vertices++ = 1;
|
|
|
|
// UV Coord
|
|
*vertices++ = 0;
|
|
*vertices++ = texCoord;
|
|
|
|
// Position
|
|
*vertices++ = pB.X();
|
|
*vertices++ = pB.Y();
|
|
*vertices++ = pB.Z();
|
|
|
|
// Normal
|
|
*vertices++ = 0;
|
|
*vertices++ = 0;
|
|
*vertices++ = 1;
|
|
|
|
// UV Coord
|
|
*vertices++ = 1;
|
|
*vertices++ = texCoord;
|
|
}
|
|
|
|
this->mBox.setExtents(Conversions::Convert(bounds.min),
|
|
Conversions::Convert(bounds.max));
|
|
|
|
vBuf->unlock();
|
|
|
|
if (_msg.has_material())
|
|
{
|
|
if (_msg.material().has_script())
|
|
{
|
|
for (int i = 0; i < _msg.material().script().uri_size(); ++i)
|
|
{
|
|
std::string matUri = _msg.material().script().uri(i);
|
|
if (!matUri.empty())
|
|
RenderEngine::Instance()->AddResourcePath(matUri);
|
|
}
|
|
|
|
std::string matName = _msg.material().script().name();
|
|
|
|
if (!matName.empty())
|
|
this->setMaterial(matName);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
this->setMaterial("Gazebo/Road");
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
Ogre::Real Road2d::Segment::getBoundingRadius() const
|
|
{
|
|
return Ogre::Math::Sqrt(std::max(this->mBox.getMaximum().squaredLength(),
|
|
this->mBox.getMinimum().squaredLength()));
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
Ogre::Real Road2d::Segment::getSquaredViewDepth(
|
|
const Ogre::Camera *_cam) const
|
|
{
|
|
Ogre::Vector3 vMin, vMax, vMid, vDist;
|
|
vMin = this->mBox.getMinimum();
|
|
vMax = this->mBox.getMaximum();
|
|
vMid = ((vMax - vMin) * 0.5) + vMin;
|
|
vDist = _cam->getDerivedPosition() - vMid;
|
|
return vDist.squaredLength();
|
|
}
|