Gazebo Rendering

API Reference

6.6.3
gz/rendering/base/BaseRayQuery.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16  */
17 #ifndef GZ_RENDERING_BASE_BASERAYQUERY_HH_
18 #define GZ_RENDERING_BASE_BASERAYQUERY_HH_
19 
20 #include <gz/math/Matrix4.hh>
21 #include <gz/math/Vector3.hh>
22 
23 #include "gz/rendering/RayQuery.hh"
24 #include "gz/rendering/Scene.hh"
25 
26 namespace ignition
27 {
28  namespace rendering
29  {
30  inline namespace IGNITION_RENDERING_VERSION_NAMESPACE {
31  //
35  template <class T>
36  class BaseRayQuery :
37  public virtual RayQuery,
38  public T
39  {
41  protected: BaseRayQuery();
42 
44  public: virtual ~BaseRayQuery() override;
45 
46  // Documentation inherited
47  public: virtual void SetOrigin(const math::Vector3d &_origin) override;
48 
49  // Documentation inherited
50  public: virtual math::Vector3d Origin() const override;
51 
52  // Documentation inherited
53  public: virtual void SetDirection(const math::Vector3d &_dir) override;
54 
55  // Documentation inherited
56  public: virtual math::Vector3d Direction() const override;
57 
58  // Documentation inherited
59  public: virtual void SetFromCamera(const CameraPtr &_camera,
60  const math::Vector2d &_coord) override;
61 
62  // Documentation inherited
63  public: virtual RayQueryResult ClosestPoint() override;
64 
66  protected: math::Vector3d origin;
67 
70  };
71 
73  template <class T>
75  {
76  }
77 
79  template <class T>
81  {
82  }
83 
85  template <class T>
87  {
88  this->origin = _origin;
89  }
90 
92  template <class T>
93  gz::math::Vector3d BaseRayQuery<T>::Origin() const
94  {
95  return this->origin;
96  }
97 
99  template <class T>
101  {
102  this->direction = _dir;
103  }
104 
106  template <class T>
107  gz::math::Vector3d BaseRayQuery<T>::Direction() const
108  {
109  return this->direction;
110  }
111 
113  template <class T>
115  const gz::math::Vector2d &_coord)
116  {
117  math::Matrix4d projectionMatrix = _camera->ProjectionMatrix();
118  math::Matrix4d viewMatrix = _camera->ViewMatrix();
119  math::Vector3d start(_coord.X(), _coord.Y(), -1.0);
120  math::Vector3d end(_coord.X(), _coord.Y(), 0.0);
121  math::Matrix4d viewProjInv = (projectionMatrix * viewMatrix).Inverse();
122 
123  // rotate start and end
124  // ign math does not support matrix4 * vec4
125  // so calc homogeneous coordinate w ourselves
126  double startw = viewProjInv(3, 0) * start[0] +
127  viewProjInv(3, 1) * start[1] +
128  viewProjInv(3, 2) * start[2] + viewProjInv(3, 3);
129  double endw = viewProjInv(3, 0) * end[0] +
130  viewProjInv(3, 1) * end[1] +
131  viewProjInv(3, 2) * end[2] + viewProjInv(3, 3);
132  start = viewProjInv * start;
133  end = viewProjInv * end;
134  // normalize
135  start = start / startw;
136  end = end / endw;
137  math::Vector3d dir = (end - start).Normalize();
138 
139  this->origin = start;
140  this->direction = dir;
141  }
142 
144  template <class T>
146  {
147  // TODO(anyone): implement a generic ray query here?
148  RayQueryResult result;
149  result.distance = -1;
150  return result;
151  }
152  }
153  }
154 }
155 #endif
A Ray Query class used for computing ray object intersections.
Definition: gz/rendering/base/BaseRayQuery.hh:36
STL class.
double distance
Intersection distance.
Definition: gz/rendering/RayQuery.hh:38
virtual math::Vector3d Direction() const override
Get ray direction.
Definition: gz/rendering/base/BaseRayQuery.hh:107
BaseRayQuery()
Constructor.
Definition: gz/rendering/base/BaseRayQuery.hh:74
virtual void SetOrigin(const math::Vector3d &_origin) override
Set ray origin.
Definition: gz/rendering/base/BaseRayQuery.hh:86
math::Vector3d origin
Ray origin.
Definition: gz/rendering/base/BaseRayQuery.hh:66
A class that stores ray query intersection results.
Definition: gz/rendering/RayQuery.hh:35
virtual void SetFromCamera(const CameraPtr &_camera, const math::Vector2d &_coord) override
Create the ray query from camera.
Definition: gz/rendering/base/BaseRayQuery.hh:114
A Ray Query class used for computing ray object intersections.
Definition: gz/rendering/RayQuery.hh:63
virtual ~BaseRayQuery() override
Destructor.
Definition: gz/rendering/base/BaseRayQuery.hh:80
virtual void SetDirection(const math::Vector3d &_dir) override
Set ray direction.
Definition: gz/rendering/base/BaseRayQuery.hh:100
math::Vector3d direction
Ray direction.
Definition: gz/rendering/base/BaseRayQuery.hh:69
virtual RayQueryResult ClosestPoint() override
Compute intersections.
Definition: gz/rendering/base/BaseRayQuery.hh:145
virtual math::Vector3d Origin() const override
Get ray origin.
Definition: gz/rendering/base/BaseRayQuery.hh:93