Program Listing for File VizUtils.h#
↰ Return to documentation for file (include/Karana/KUtils/VizUtils.h)
/*
* Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
*
* NOTICE TO USER:
*
* This source code and/or documentation (the "Licensed Materials") is
* the confidential and proprietary information of Karana Dynamics Inc.
* Use of these Licensed Materials is governed by the terms and conditions
* of a separate software license agreement between Karana Dynamics and the
* Licensee ("License Agreement"). Unless expressly permitted under that
* agreement, any reproduction, modification, distribution, or disclosure
* of the Licensed Materials, in whole or in part, to any third party
* without the prior written consent of Karana Dynamics is strictly prohibited.
*
* THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
* KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
* BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
* FITNESS FOR A PARTICULAR PURPOSE.
*
* IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
* INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
* OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
*
* U.S. Government End Users: The Licensed Materials are a "commercial item"
* as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
* only as a commercial end item under the terms of this license.
*
* Any use of the Licensed Materials in individual or commercial software must
* include, in the user documentation and internal source code comments,
* this Notice, Disclaimer, and U.S. Government Use Provision.
*/
/**
* @file
* @brief Contains visualization utils.
*/
#pragma once
#include "Karana/Collision/PenaltyContact.h"
#include "Karana/Frame/FrameToFrame.h"
#include "Karana/ProxyScene/ProxySceneNode.h"
#include "Karana/SOADyn/HingeNode.h"
#include "Karana/SOADyn/Node.h"
#include <functional>
namespace Karana::KUtils {
namespace km = Karana::Math;
namespace kc = Karana::Core;
namespace kf = Karana::Frame;
namespace kd = Karana::Dynamics;
namespace ks = Karana::Scene;
namespace kmo = Karana::Models;
/**
* @class VectorVisualizer
* @brief Visualize a vector using a frame and a function.
*
* The frame is origin of the vector, and the function provides the
* length/direction for the vector.
*/
class VectorVisualizer : public Karana::Core::Base {
public:
/**
* @brief VectorVisualizer constructor. The constructor is not meant to be called directly.
* Please use the create(...) method instead to create an instance.
*
* @param name Name of the visualizer.
* @param frame The Frame where the vector starts.
* @param scene The ProxyScene to add the vector to.
* @param fn The function used to update the vector.
*/
VectorVisualizer(std::string_view name,
const kc::ks_ptr<kf::Frame> &frame,
const kc::ks_ptr<ks::ProxyScene> &scene,
std::function<km::Vec3()> fn);
/**
* @brief Create a new instance of VectorVisualizer.
*
* @param name Name of the visualizer.
* @param frame The Frame where the vector starts.
* @param scene The ProxyScene to add the vector to.
* @param fn The function used to update the vector.
* @returns A pointer to the newly created VectorVisualizer.
*/
// Adding suppression, as CodeChecker complains this method has
// the same name as a method in the base version. This is just a
// CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
static kc::ks_ptr<VectorVisualizer> create(std::string_view name,
const kc::ks_ptr<kf::Frame> &frame,
const kc::ks_ptr<ks::ProxyScene> &scene,
std::function<km::Vec3()> fn);
/**
* @brief Get the radius of the vector.
*
* @return The radius of the vector.
*/
float getRadius();
/**
* @brief Set the radius of the vector.
*
* @param radius The new radius of the vector.
*/
void setRadius(float radius);
/**
* @brief Get the color of the vector.
*
* @return The color of the vector.
*/
ks::Color getColor();
/**
* @brief Set the color of the vector.
*
* @param color The new color of the vector.
*/
void setColor(ks::Color color);
/**
* @brief Get the starting offset of the vector.
*
* This is the offset between the frame origin and the start of the
* vector.
*
* @return The start offset of the vector.
*/
const km::Vec3 &getStartOffset();
/**
* @brief Set the start offset of the vector.
*
* This is the offset between the frame origin and the start of the
* vector.
*
* @param offset The new start offset of the vector.
*/
void setStartOffset(const km::Vec3 &offset);
/**
* @brief Register the callback with the scene.
*/
virtual void registerCallback();
/**
* @brief Unregister the callback with the scene.
*/
void unregisterCallback();
protected:
/// The color of the vector
ks::Color _color = ks::Color::RED;
/// The radius of the vector
float _radius = 0.075f;
/// The scalar to use when setting the scale of the vector
double _radius_ratio = 1.0;
/// The starting offset of the vector
km::Vec3 _start_offset{0.0, 0.0, 0.0};
/// SceneNode to attach the rod and tip to
kc::ks_ptr<ks::ProxySceneNode> _node;
/// Rod of the vector
kc::ks_ptr<ks::ScenePart> _rod;
/// Tip of the vector
kc::ks_ptr<ks::ScenePart> _tip;
/// The vector update function
std::function<km::Vec3()> _fn;
/// Whether or not the callback has been registered with the scene
bool _callback_registered = false;
/// The name of the scene callback
std::string _callback_name;
/// A pointer to the ProxyScene
std::weak_ptr<ks::ProxyScene> _scene;
};
/**
* @class ScaledVectorVisualizer
* @brief A scaled version of VectorVisualizer.
*
* This is like the VectorVisualizer class (see that class for more details)
* but the length of the vector can be scaled using get/setScale.
*/
class ScaledVectorVisualizer : public VectorVisualizer {
public:
using VectorVisualizer::VectorVisualizer;
/**
* @brief Create a new instance of VectorVisualizer.
*
* @param name Name of the visualizer.
* @param frame The Frame where the vector starts.
* @param scene The ProxyScene to add the vector to.
* @param fn The function used to update the vector.
* @returns A pointer to the newly created ScaledVectorVisualizer.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// the VectorVisualizer class. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
static kc::ks_ptr<ScaledVectorVisualizer> create(std::string_view name,
const kc::ks_ptr<kf::Frame> &frame,
const kc::ks_ptr<ks::ProxyScene> &scene,
std::function<km::Vec3()> fn);
/**
* @brief Get the current scale.
*
* @return The current scale.
*/
double getScale();
/**
* @brief Set the current scale.
*
* @param scale The current scale.
*/
void setScale(double scale);
/**
* @brief Register the update callback with the scene.
*/
void registerCallback() override;
protected:
/// The scale to use for the vector
double _scale = 1.0;
};
/**
* @class FrameToFrameVectorVisualizer
* @brief Visualize a vector that spans the two frames in a FrameToFrame.
*
* The origin of the vector is at the o-frame of the FrameToFrame, and the
* end of the vector is at the p-frame of the FrameToFrame.
*/
class FrameToFrameVectorVisualizer : public VectorVisualizer {
public:
/**
* @brief ScaledVectorVisualizer constructor. The constructor is not meant to be called
* directly. Please use the create(...) method instead to create an instance.
*
* @param name Name of the visualizer.
* @param frame_to_frame The FrameToFrame for the vector.
* @param scene The ProxyScene to add the vector to.
*/
FrameToFrameVectorVisualizer(std::string_view name,
const kc::ks_ptr<kf::FrameToFrame> &frame_to_frame,
const kc::ks_ptr<ks::ProxyScene> &scene);
/**
* @brief Create a new instance of VectorVisualizer.
*
* @param name Name of the visualizer.
* @param frame_to_frame The FrameToFrame for the vector.
* @param scene The ProxyScene to add the vector to.
* @returns A pointer to the newly created FrameToFrameVectorVisualizer.
*/
static kc::ks_ptr<FrameToFrameVectorVisualizer>
create(std::string_view name,
const kc::ks_ptr<kf::FrameToFrame> &frame_to_frame,
const kc::ks_ptr<ks::ProxyScene> &scene);
/**
* @brief Get the ending offset of the vector.
*
* This is the offset between the p-frame of the frame to frame and the end of the
* vector.
*
* @return The start offset of the vector.
*/
const km::Vec3 &getEndOffset();
/**
* @brief Set the end offset of the vector.
*
* This is the offset between the p-frame of the frame to frame and the end of the
* vector.
*
* @param offset The new end offset of the vector.
*/
void setEndOffset(const km::Vec3 &offset);
/**
* @brief Register the update callback with the scene.
*/
void registerCallback() override;
protected:
/// The starting offset of the vector
km::Vec3 _end_offset{0.0, 0.0, 0.0};
/// The associated frame_to_frame to visualize
kc::ks_ptr<kf::FrameToFrame> _frame_to_frame;
};
/**
* @class ContactForceVisualizer
* @brief Visualize the contact forces created by a PenaltyContact KModel.
*
* The frame provided is the frame the vectors will be drawn in.
*/
class ContactForceVisualizer : public Karana::Core::Base {
/// Type alias for an arrow. This consists of the parent ProxySceneNode, rod, and tip.
using arrow = std::tuple<kc::ks_ptr<ks::ProxySceneNode>,
kc::ks_ptr<ks::ScenePart>,
kc::ks_ptr<ks::ScenePart>>;
public:
/**
* @brief ContactForceVisualizer constructor. The constructor is not meant to be called
* directly. Please use the create(...) method instead to create an instance.
*
* @param name Name of the visualizer.
* @param frame The Frame where the vectors will be drawn in.
* @param scene The ProxyScene to add the vectors to.
* @param penalty_contact The PenaltyContact model to get the contact vectors from.
*/
ContactForceVisualizer(std::string_view name,
const kc::ks_ptr<kf::Frame> &frame,
const kc::ks_ptr<ks::ProxyScene> &scene,
const kc::ks_ptr<kmo::PenaltyContact> &penalty_contact);
/**
* @brief Create a new instance of ContactForceVisualizer.
*
* @param name Name of the visualizer.
* @param frame The Frame where the vectors will be drawn in.
* @param scene The ProxyScene to add the vectors to.
* @param penalty_contact The PenaltyContact model to get the contact vectors from.
* @returns A pointer to the newly created ContactForceVisualizer.
*/
static kc::ks_ptr<ContactForceVisualizer>
create(std::string_view name,
const kc::ks_ptr<kf::Frame> &frame,
const kc::ks_ptr<ks::ProxyScene> &scene,
const kc::ks_ptr<kmo::PenaltyContact> &penalty_contact);
/**
* @brief Get the radius of the vectors.
*
* @return The radius of the vectors.
*/
float getRadius();
/**
* @brief Set the radius of the vectors.
*
* @param radius The new radius of the vectors.
*/
void setRadius(float radius);
/**
* @brief Get the color of the vectors.
*
* @return The color of the vectors.
*/
ks::Color getColor();
/**
* @brief Set the color of the vectors.
*
* @param color The new color of the vectors.
*/
void setColor(ks::Color color);
/**
* @brief Get the current scale.
*
* @return The current scale.
*/
double getScale();
/**
* @brief Set the current scale.
*
* @param scale The current scale.
*/
void setScale(double scale);
/**
* @brief Register the callback with the scene.
*/
void registerCallback();
/**
* @brief Unregister the callback with the scene.
*/
void unregisterCallback();
protected:
/**
* @brief Get an arrow from the inactive pool, or create a new one if all arrows are
* currently active.
*
* @return An arrow to use for visualizing.
*/
arrow &_lookupOrCreateArrow();
/// The color of the vector
ks::Color _color = ks::Color::RED;
/// The radius of the vector
float _radius = 0.075f;
/// The scalar to use when setting the scale of the vector
double _radius_ratio = 1.0;
/// The starting offset of the vector
km::Vec3 _start_offset{0.0, 0.0, 0.0};
/**
* @brief A vector of arrows.
*
* Each consists of the ProxyScene node the arrow is attached to,
* the rod of the arrow, and the tip of the arrow.
*/
std::vector<arrow> _arrows;
/**
* In _arrows, values to the left of this (from begin() to begin() +
* index) are inactive and values to the right are active. Another way to say this is this
* index points to a node that is active, and it is the first active node in the list.
* Everything at this index and later is active.
*/
size_t _active_arrow_index = 0;
/// Whether or not the callback has been registered with the scene
bool _callback_registered = false;
/// The name of the scene callback
std::string _callback_name;
/// A pointer to the ProxyScene
std::weak_ptr<ks::ProxyScene> _scene;
/// The scale to use for the vector
double _scale = 1.0;
/// The Frame used to draw arrows from
kc::ks_ptr<kf::Frame> _frame;
/// The PenaltyContact model use to get the contact node pairs
kc::ks_ptr<kmo::PenaltyContact> _penalty_contact;
};
/** @brief Enums for the frame to frame rate vector types for visualization */
enum class FrameToFrameRateType {
VEL_LINEAR, //!< the relative translation velocity
VEL_ANGULAR, //!< the relative angular velocity
ACCEL_LINEAR, //!< the relative translation acceleration
ACCEL_ANGULAR //!< the relative angular acceleration
};
/**
* @brief Visualize the frame_to_frame vel/accel values
*
* Visualize the relative velocity/acceleration (as specified by the
* type argument) for the frame_to_frame as a vector originating from the
* pframe and whose size and direction is proportional to the
* selected relative rate. The orientation of the vector is in the
* oframe.
*
* @param frame_to_frame Visualize the vector for the frame_to_frame's rates.
* @param scene The ProxyScene to use for visualizing.
* @param rate_type The type of rate to visualize
* @returns A ScaledVectorVisualizer for visualizing the frame-to-frame rates.
*/
kc::ks_ptr<ScaledVectorVisualizer>
visualizeFrameToFrameRates(const kc::ks_ptr<kf::FrameToFrame> &frame_to_frame,
const kc::ks_ptr<ks::ProxyScene> &scene,
FrameToFrameRateType rate_type);
/**
* @brief Visualize the force of a node.
*
* @param node The node to visualize the force for.
* @param scene The ProxyScene to use for visualizing.
* @returns A ScaledVectorVisualizer for visualizing the node force.
*/
kc::ks_ptr<ScaledVectorVisualizer> visualizeNodeForce(const kc::ks_ptr<kd::Node> &node,
const kc::ks_ptr<ks::ProxyScene> &scene);
/**
* @brief Visualize the torque on a node.
*
* @param node The node to visualize the torque for.
* @param scene The ProxyScene to use for visualizing.
* @returns A ScaledVectorVisualizer for visualizing the node force.
*/
kc::ks_ptr<ScaledVectorVisualizer> visualizeNodeTorque(const kc::ks_ptr<kd::Node> &node,
const kc::ks_ptr<ks::ProxyScene> &scene);
/**
* @brief Visualize the interbody force on a node.
*
* @param onode The Onode to visualize the force for.
* @param scene The ProxyScene to use for visualizing.
* @param use_ta If true, use TA getInterBodyForceTAFwdDyn to calculate the force. Otherwise,
* use getInterBodyForceTreeFwdDyn.
* @returns A ScaledVectorVisualizer for visualizing the interbody force on the node.
*/
kc::ks_ptr<ScaledVectorVisualizer>
visualizeInterBodyForce(const kc::ks_ptr<kd::HingeOnode> &onode,
const kc::ks_ptr<ks::ProxyScene> &scene,
bool use_ta);
/**
* @brief Visualize the interbody torque on a node.
*
* @param onode The Onode to visualize the torque for.
* @param scene The ProxyScene to use for visualizing.
* @param use_ta If true, use TA getInterBodyForceTAFwdDyn to calculate the torque. Otherwise,
* use getInterBodyForceTreeFwdDyn.
* @returns A ScaledVectorVisualizer for visualizing the interbody torque on the node.
*/
kc::ks_ptr<ScaledVectorVisualizer>
visualizeInterBodyTorque(const kc::ks_ptr<kd::HingeOnode> &onode,
const kc::ks_ptr<ks::ProxyScene> &scene,
bool use_ta);
} // namespace Karana::KUtils