Visual Servoing Platform  version 3.6.1 under development (2025-02-19)
vpPanda3DBaseRenderer.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  */
30 
31 #include <visp3/ar/vpPanda3DBaseRenderer.h>
32 
33 #if defined(VISP_HAVE_PANDA3D)
34 
35 #include <visp3/ar/vpPanda3DFrameworkManager.h>
36 #include <visp3/core/vpMath.h>
37 
38 
39 #include <antialiasAttrib.h>
40 #include "boundingSphere.h"
41 #include "boundingBox.h"
42 #include "thread.h"
43 #include "load_prc_file.h"
44 #include "windowFramework.h"
45 #include "graphicsOutput.h"
46 
47 BEGIN_VISP_NAMESPACE
49  1.0, 0.0, 0.0, 0.0,
50  0.0, 0.0, -1., 0.0,
51  0.0, 1.0, 0.0, 0.0,
52  0.0, 0.0, 0.0, 1.0
53 });
55 
56 
57 
59 {
60  // if (m_window != nullptr) {
61  // for (GraphicsOutput *buffer: m_buffers) {
62  // buffer->get_engine()->remove_window(buffer);
63  // }
64  // }
65  // m_buffers.clear();
66  if (m_window != nullptr) {
67  for (GraphicsOutput *buffer: m_buffers) {
68  buffer->set_active(false);
69  buffer->clear_render_textures();
70  }
71  }
72  m_buffers.clear();
73  if (m_isWindowOwner) {
75  frameworkManager.registerDisabledWindow(m_window);
76  }
77 }
78 
80 {
81 
83  PandaFramework &framework = frameworkManager.getFramework();
84  frameworkManager.initFramework();
85 
88  "Panda3D renderer: Cannot create a window with 0 height or width.");
89  }
90 
91  m_isWindowOwner = true;
92 
93  WindowProperties winProps;
94  winProps.set_size(LVecBase2i(m_renderParameters.getImageWidth(), m_renderParameters.getImageHeight()));
95  int flags = GraphicsPipe::BF_refuse_window;
96  m_window = framework.open_window(winProps, flags, nullptr, nullptr);
97  // try and reopen with visible window
98  if (m_window == nullptr) {
99  winProps.set_minimized(true);
100  m_window = framework.open_window(winProps, 0, nullptr, nullptr);
101  }
102  if (m_window == nullptr) {
104  "Panda3D renderer: Could not create the requested window when performing initialization.");
105  }
106  m_window->set_background_type(WindowFramework::BackgroundType::BT_black);
107  setupScene();
108  setupCamera();
110  //m_window->get_display_region_3d()->set_camera(m_cameraPath);
111 }
112 
113 void vpPanda3DBaseRenderer::initFromParent(PointerTo<WindowFramework> window)
114 {
115  m_isWindowOwner = false;
116  m_window = window;
117  setupScene();
118  setupCamera();
120 }
121 
123 {
124  m_isWindowOwner = false;
125  initFromParent(renderer.m_window);
126 }
127 
129 {
130  m_renderRoot = m_window->get_render().attach_new_node(m_name);
131  //m_renderRoot.set_antialias(AntialiasAttrib::M_none);
132 }
133 
135 {
136  m_cameraPath = m_window->make_camera();
137  m_camera = (Camera *)m_cameraPath.node();
138  // m_camera = m_window->get_camera(0);
139  m_cameraPath = m_renderRoot.attach_new_node(m_camera);
141  m_camera->set_scene(m_renderRoot);
142 }
143 
145 {
147  // Disable rendering for all the other renderers
149  m_window->get_graphics_output()->get_engine()->render_frame();
152 }
153 
155 {
156  GraphicsOutput *mainBuffer = getMainOutputBuffer();
157  if (mainBuffer != nullptr) {
158  mainBuffer->get_engine()->extract_texture_data(mainBuffer->get_texture(), mainBuffer->get_gsg());
159  }
160 }
161 
163 {
164  unsigned int previousH = m_renderParameters.getImageHeight(), previousW = m_renderParameters.getImageWidth();
165  bool resize = previousH != params.getImageHeight() || previousW != params.getImageWidth();
166 
167  m_renderParameters = params;
168 
169  if (resize) {
170  for (GraphicsOutput *buffer: m_buffers) {
171  //buffer->get_type().is_derived_from()
172  GraphicsBuffer *buf = dynamic_cast<GraphicsBuffer *>(buffer);
173  if (buf == nullptr) {
174  throw vpException(vpException::fatalError, "Panda3D: could not cast to GraphicsBuffer when rendering.");
175  }
176  else {
178  }
179  }
180  }
181 
182  // If renderer is already initialized, modify camera properties
183  if (m_camera != nullptr) {
185  }
186 }
187 
189 {
190  if (m_camera.is_null() || m_cameraPath.is_empty()) {
191  throw vpException(vpException::notInitialized, "Camera was not initialized before trying to set its pose");
192  }
194 }
195 
197 {
198  if (m_camera.is_null()) {
199  throw vpException(vpException::notInitialized, "Camera was not initialized before trying to get its pose");
200  }
201  return getNodePose(m_cameraPath);
202 }
203 
204 void vpPanda3DBaseRenderer::setNodePose(const std::string &name, const vpHomogeneousMatrix &wTo)
205 {
206  NodePath object = m_renderRoot.find(name);
207  setNodePose(object, wTo);
208 }
209 
210 void vpPanda3DBaseRenderer::setNodePose(NodePath &object, const vpHomogeneousMatrix &wTo)
211 {
212  const vpHomogeneousMatrix wpTo = wTo * VISP_T_PANDA;
215  object.set_pos(t[0], t[1], t[2]);
216  object.set_quat(LQuaternion(q.w(), q.x(), q.y(), q.z()));
217 }
218 
220 {
221  NodePath object = m_renderRoot.find(name);
222  if (object.is_empty()) {
223  throw vpException(vpException::badValue, "Node %s was not found", name.c_str());
224  }
225  return getNodePose(object);
226 }
227 
229 {
230  const LPoint3 pos = object.get_pos();
231  const LQuaternion quat = object.get_quat();
232  const vpTranslationVector t(pos[0], pos[1], pos[2]);
233  const vpQuaternionVector q(quat.get_i(), quat.get_j(), quat.get_k(), quat.get_r());
234  return vpHomogeneousMatrix(t, q) * PANDA_T_VISP;
235 }
236 
237 void vpPanda3DBaseRenderer::computeNearAndFarPlanesFromNode(const std::string &name, float &nearV, float &farV, bool fast)
238 {
239  if (m_camera == nullptr) {
240  throw vpException(vpException::notInitialized, "Cannot compute planes when the camera is not initialized");
241  }
242  NodePath object = m_renderRoot.find(name);
243  if (object.is_empty()) {
244  throw vpException(vpException::badValue, "Node %s was not found", name.c_str());
245  }
246  if (!fast) {
247  LPoint3 minP, maxP;
248  object.calc_tight_bounds(minP, maxP);
249  const BoundingBox box(minP, maxP);
250  float minZ = std::numeric_limits<float>::max(), maxZ = 0.f;
251  const vpHomogeneousMatrix wTcam = getCameraPose();
253  const vpHomogeneousMatrix camTobj = wTcam.inverse() * wTobj;
254  for (unsigned int i = 0; i < 8; ++i) {
255  const LPoint3 p = box.get_point(i);
256  const vpColVector pv = vpColVector({ p.get_x(), -p.get_z(), p.get_y(), 1.0 });
257  vpColVector cpV = camTobj * pv;
258  cpV /= cpV[3];
259  float Z = cpV[2];
260  if (Z > maxZ) {
261  maxZ = Z;
262  }
263  if (Z < minZ) {
264  minZ = Z;
265  }
266  }
267 
268  nearV = minZ;
269  farV = maxZ;
270  }
271  else {
272  const BoundingVolume *volume = object.node()->get_bounds();
273  if (volume->get_type() == BoundingSphere::get_class_type()) {
274  const BoundingSphere *sphere = (const BoundingSphere *)volume;
275  const LPoint3 center = sphere->get_center();
276  const float distCenter = (center - m_cameraPath.get_pos()).length();
277  nearV = vpMath::maximum<float>(0.f, distCenter - sphere->get_radius());
278  farV = vpMath::maximum<float>(nearV, distCenter + sphere->get_radius());
279  }
280  else if (volume->get_type() == BoundingBox::get_class_type()) {
281  const vpHomogeneousMatrix wTcam = getCameraPose();
283  const vpHomogeneousMatrix camTobj = wTcam.inverse() * wTobj;
284  const BoundingBox *box = (const BoundingBox *)volume;
285  double minZ = std::numeric_limits<double>::max(), maxZ = 0.0;
286 
287  for (unsigned int i = 0; i < 8; ++i) {
288  const LPoint3 p = box->get_point(i);
289  vpColVector cp = camTobj * vpColVector({ p.get_x(), -p.get_z(), p.get_y(), 1.0 });
290  double Z = cp[2] / cp[3];
291  if (Z < minZ) {
292  minZ = Z;
293  }
294  if (Z > maxZ) {
295  maxZ = Z;
296  }
297  }
298  nearV = minZ;
299  farV = maxZ;
300  }
301  else {
302  throw vpException(vpException::fatalError, "Unhandled bounding volume %s type returned by Panda3d", volume->get_type().get_name().c_str());
303  }
304  }
305 }
306 
308 {
309  if (isRendering3DScene()) {
310  PointerTo<GraphicsOutput> buffer = getMainOutputBuffer();
311  if (buffer != nullptr) {
312  buffer->set_clear_depth_active(false);
313  if (!buffer->share_depth_buffer(sourceBuffer.getMainOutputBuffer())) {
314  throw vpException(vpException::fatalError, "Could not share depth buffer!");
315  }
316  }
317  }
318 }
319 
321 {
322  int previousOrder = m_renderOrder;
323  m_renderOrder = order;
324  for (GraphicsOutput *buffer: m_buffers) {
325  buffer->set_sort(buffer->get_sort() + (order - previousOrder));
326  }
327 }
328 
329 NodePath vpPanda3DBaseRenderer::loadObject(const std::string &nodeName, const std::string &modelPath)
330 {
331  PandaFramework &framework = vpPanda3DFrameworkManager::getInstance().getFramework();
332  NodePath model = m_window->load_model(framework.get_models(), modelPath);
333  if (model.is_empty()) {
334  throw vpException(vpException::ioError, "Could not load model %s", modelPath.c_str());
335  }
336  for (int i = 0; i < model.get_num_children(); ++i) {
337  model.get_child(i).clear_transform();
338  }
339 
340  model.detach_node();
341  model.set_name(nodeName);
342  return model;
343 }
344 
345 void vpPanda3DBaseRenderer::addNodeToScene(const NodePath &object)
346 {
347  NodePath objectInScene = object.copy_to(m_renderRoot);
348  objectInScene.set_name(object.get_name());
349  setNodePose(objectInScene, vpHomogeneousMatrix());
350 }
351 
353 {
354  if (useVsync) {
355  load_prc_file_data("", "sync-video true");
356  }
357  else {
358  load_prc_file_data("", "sync-video false");
359  }
360 }
362 {
363  if (abort) {
364  load_prc_file_data("", "assert-abort 1");
365  }
366  else {
367  load_prc_file_data("", "assert-abort 0");
368  }
369 }
370 
372 {
373  load_prc_file_data("", "gl-debug 1");
374  load_prc_file_data("", "notify-level-display spam");
375 }
376 
378 {
379  vpColVector pandaPos = PANDA_T_VISP * point;
380  pandaPos /= pandaPos[3];
381  return pandaPos;
382 }
384 {
385  vpColVector pandaPos = PANDA_T_VISP.getRotationMatrix() * point;
386  return pandaPos;
387 }
388 
390 {
391  m_renderRoot.ls();
392 }
393 
394 END_VISP_NAMESPACE
395 
396 #elif !defined(VISP_BUILD_SHARED_LIBS)
397 // Work around to avoid warning: libvisp_ar.a(vpPanda3DBaseRenderer.cpp.o) has no symbols
398 void dummy_vpPanda3DBaseRenderer() { };
399 
400 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ ioError
I/O error.
Definition: vpException.h:67
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:74
@ fatalError
Fatal error.
Definition: vpException.h:72
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
Base class for a panda3D renderer. This class handles basic functionalities, such as loading object,...
virtual void setupCamera()
Initialize camera. Should be called when the scene root of this render has already been created.
virtual void setupScene()
Initialize the scene for this specific renderer.
virtual vpHomogeneousMatrix getCameraPose()
Retrieve the camera's pose, in the world frame. The pose is specified using the ViSP convention (Y-do...
NodePath m_renderRoot
Rendering parameters.
void computeNearAndFarPlanesFromNode(const std::string &name, float &near, float &far, bool fast)
Compute the near and far planes for the camera at the current pose, given a certain node/part of the ...
PointerTo< Camera > m_camera
Node containing all the objects and the camera for this renderer.
virtual bool isRendering3DScene() const
Returns true if this renderer process 3D data and its scene root can be interacted with.
static vpColVector vispPointToPanda(const vpColVector &point)
virtual void initFramework()
Initialize the whole Panda3D framework. Create a new PandaFramework object and a new window.
void setVerticalSyncEnabled(bool useVsync)
set whether vertical sync is enabled. When vertical sync is enabled, render speed will be limited by ...
static const vpHomogeneousMatrix PANDA_T_VISP
Homogeneous transformation matrix to convert from the Panda coordinate system (right-handed Z-up) to ...
std::string m_name
Inverse of VISP_T_PANDA.
bool m_isWindowOwner
Set of buffers that this renderer uses. This storage contains weak refs to those buffers and should n...
virtual PointerTo< GraphicsOutput > getMainOutputBuffer()
virtual void addNodeToScene(const NodePath &object)
Add a node to the scene. Its pose is set as the identity matrix.
void setAbortOnPandaError(bool abort)
Set the behaviour when a Panda3D assertion fails. If abort is true, the program will stop....
static const vpHomogeneousMatrix VISP_T_PANDA
std::vector< PointerTo< GraphicsOutput > > m_buffers
NodePath of the camera.
PointerTo< WindowFramework > m_window
Rendering priority for this renderer and its buffers. A lower value will be rendered first....
virtual void enableSharedDepthBuffer(vpPanda3DBaseRenderer &sourceBuffer)
virtual void setupRenderTarget()
Initialize buffers and other objects that are required to save the render.
static vpColVector vispVectorToPanda(const vpColVector &vec)
virtual void setRenderParameters(const vpPanda3DRenderParameters &params)
Set new rendering parameters. If the scene has already been initialized, the renderer camera is updat...
virtual void initFromParent(PointerTo< WindowFramework > window)
NodePath loadObject(const std::string &nodeName, const std::string &modelPath)
Load a 3D object. To load an .obj file, Panda3D must be compiled with assimp support.
int m_renderOrder
name of the renderer
virtual void setNodePose(const std::string &name, const vpHomogeneousMatrix &wTo)
Set the pose of a node. This node can be any Panda object (light, mesh, camera). The pose is specifie...
virtual void beforeFrameRendered()
virtual void setCameraPose(const vpHomogeneousMatrix &wTc)
Set the camera's pose. The pose is specified using the ViSP convention (Y-down right handed).
vpPanda3DRenderParameters m_renderParameters
Pointer to owning window, which can create buffers etc. It is not necessarily visible.
virtual vpHomogeneousMatrix getNodePose(const std::string &name)
Get the pose of a Panda node, in world frame in the ViSP convention (Y-down right handed).
Base class for a panda3D renderer. This class handles basic functionalities, such as loading object,...
void registerDisabledWindow(PointerTo< WindowFramework > wf)
static vpPanda3DFrameworkManager & getInstance()
void disableAllOtherRenderers(PointerTo< WindowFramework > &active)
Rendering parameters for a panda3D simulation.
void setupPandaCamera(Camera *camera)
Update a Panda3D camera object to use this objects's parameters.
Implementation of a rotation vector as quaternion angle minimal representation.
const double & z() const
Returns the z-component of the quaternion.
const double & x() const
Returns the x-component of the quaternion.
const double & y() const
Returns the y-component of the quaternion.
const double & w() const
Returns the w-component of the quaternion.
Class that consider the case of a translation vector.