147 lines
4.7 KiB
C++
147 lines
4.7 KiB
C++
/*
|
|
* Copyright (C) 2014 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.
|
|
*
|
|
*/
|
|
|
|
#include "gazebo/common/MeshManager.hh"
|
|
#include "gazebo/rendering/Visual.hh"
|
|
|
|
#include "gazebo/rendering/Conversions.hh"
|
|
#include "gazebo/rendering/Scene.hh"
|
|
#include "gazebo/rendering/Camera.hh"
|
|
#include "gazebo/rendering/RayQueryPrivate.hh"
|
|
#include "gazebo/rendering/RayQuery.hh"
|
|
|
|
using namespace gazebo;
|
|
using namespace rendering;
|
|
|
|
/////////////////////////////////////////////////
|
|
RayQuery::RayQuery(CameraPtr _camera)
|
|
: dataPtr(new RayQueryPrivate)
|
|
{
|
|
this->dataPtr->camera = _camera;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
RayQuery::~RayQuery()
|
|
{
|
|
delete this->dataPtr;
|
|
this->dataPtr = NULL;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
bool RayQuery::SelectMeshTriangle(int _x, int _y, VisualPtr _visual,
|
|
math::Vector3 &_intersect, std::vector<math::Vector3> &_vertices)
|
|
{
|
|
// create the ray to test
|
|
Ogre::Ray ray =
|
|
this->dataPtr->camera->OgreCamera()->getCameraToViewportRay(
|
|
static_cast<float>(_x) / this->dataPtr->camera->ViewportWidth(),
|
|
static_cast<float>(_y) / this->dataPtr->camera->ViewportHeight());
|
|
|
|
std::vector<rendering::VisualPtr> visuals;
|
|
this->GetMeshVisuals(_visual, visuals);
|
|
|
|
|
|
Ogre::Real closestDistance = -1.0f;
|
|
Ogre::Vector3 closestResult;
|
|
bool newClosestFound = false;
|
|
std::vector<Ogre::Vector3> vertices;
|
|
|
|
for (unsigned int i = 0; i < visuals.size(); ++i)
|
|
{
|
|
const common::Mesh *mesh =
|
|
common::MeshManager::Instance()->GetMesh(visuals[i]->GetMeshName());
|
|
|
|
if (!mesh)
|
|
continue;
|
|
|
|
Ogre::Matrix4 transform = visuals[i]->GetSceneNode()->_getFullTransform();
|
|
// test for hitting individual triangles on the mesh
|
|
for (unsigned int j = 0; j < mesh->GetSubMeshCount(); ++j)
|
|
{
|
|
const common::SubMesh *submesh = mesh->GetSubMesh(j);
|
|
if (submesh->GetVertexCount() < 3u)
|
|
continue;
|
|
unsigned int indexCount = submesh->GetIndexCount();
|
|
for (unsigned int k = 0; k < indexCount; k += 3)
|
|
{
|
|
if (indexCount <= k+2)
|
|
continue;
|
|
|
|
ignition::math::Vector3d vertexA =
|
|
submesh->Vertex(submesh->GetIndex(k));
|
|
ignition::math::Vector3d vertexB =
|
|
submesh->Vertex(submesh->GetIndex(k+1));
|
|
ignition::math::Vector3d vertexC =
|
|
submesh->Vertex(submesh->GetIndex(k+2));
|
|
|
|
Ogre::Vector3 worldVertexA = transform * Conversions::Convert(vertexA);
|
|
Ogre::Vector3 worldVertexB = transform * Conversions::Convert(vertexB);
|
|
Ogre::Vector3 worldVertexC = transform * Conversions::Convert(vertexC);
|
|
|
|
// check for a hit against this triangle
|
|
std::pair<bool, Ogre::Real> hit = Ogre::Math::intersects(ray,
|
|
worldVertexA, worldVertexB, worldVertexC,
|
|
(worldVertexB - worldVertexA).crossProduct(
|
|
worldVertexC - worldVertexA));
|
|
|
|
// if it was a hit check if its the closest
|
|
if (hit.first &&
|
|
(closestDistance < 0.0f || hit.second < closestDistance))
|
|
{
|
|
// this is the closest so far, save it off
|
|
closestDistance = hit.second;
|
|
vertices.clear();
|
|
vertices.push_back(worldVertexA);
|
|
vertices.push_back(worldVertexB);
|
|
vertices.push_back(worldVertexC);
|
|
newClosestFound = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// if we found a new closest raycast for this object, update the
|
|
// closestResult before moving on to the next object.
|
|
if (newClosestFound)
|
|
closestResult = ray.getPoint(closestDistance);
|
|
|
|
// return the result
|
|
if (closestDistance >= 0.0f && vertices.size() == 3u)
|
|
{
|
|
// raycast success
|
|
_intersect = Conversions::Convert(closestResult);
|
|
_vertices.clear();
|
|
_vertices.push_back(Conversions::Convert(vertices[0]));
|
|
_vertices.push_back(Conversions::Convert(vertices[1]));
|
|
_vertices.push_back(Conversions::Convert(vertices[2]));
|
|
return true;
|
|
}
|
|
// raycast failed
|
|
return false;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void RayQuery::GetMeshVisuals(rendering::VisualPtr _visual,
|
|
std::vector<rendering::VisualPtr> &_visuals)
|
|
{
|
|
if (_visual->GetMeshName() != "")
|
|
_visuals.push_back(_visual);
|
|
|
|
for (unsigned int i = 0; i < _visual->GetChildCount(); ++i)
|
|
this->GetMeshVisuals(_visual->GetChild(i), _visuals);
|
|
}
|