# 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.
"""Classes for working with directed graphs."""
from abc import ABC, abstractmethod
from typing import TypeVar, Generic, cast
from Karana.Dynamics import PhysicalBody, Multibody
from Karana.Frame import Frame, FrameContainer
T = TypeVar("T")
[docs]
class GraphAdapter(ABC, Generic[T]):
"""Common interface for various types of directed graphs.
The purpose of this class is to define a common interface for
wrapper classes that adapt different graphs, each with their own
interface, to a single common interface.
"""
[docs]
@abstractmethod
def parents(self, node: T, /) -> list[T]:
"""Get the list of parents for a given node."""
[docs]
@abstractmethod
def children(self, node: T, /) -> list[T]:
"""Get the list of children for a given node."""
[docs]
@abstractmethod
def nodes(self) -> list[T]:
"""Get the list of all nodes in the graph."""
[docs]
def descendants(self, node: T, /) -> list[T]:
"""Get the list of all descendants for a given node."""
# Use a dictionary instead of set to preserve ordering
visited = {}
stack = []
stack.append(node)
results = []
# do a dfs traversal
while stack:
curr = stack.pop()
for child in self.children(curr):
if child not in visited:
visited[child] = None
stack.append(child)
return list(visited)
[docs]
def ancestors(self, node: T, /) -> list[T]:
"""Get the list of all ancestors for a given node."""
# Use a dictionary instead of set to preserve ordering
visited = {}
stack = []
stack.append(node)
results = []
# do a dfs traversal
while stack:
curr = stack.pop()
for parent in self.parents(curr):
if parent not in visited:
visited[parent] = None
stack.append(parent)
return list(visited)
[docs]
class BodyGraphAdapter(GraphAdapter[PhysicalBody]):
"""Multibody wrapper implementing the GraphAdapter interface."""
def __init__(self, multibody: Multibody):
"""Create an instance of BodyGraphAdapter.
Parameters
----------
multibody : Multibody
The Multibody to create the GraphAdapter interface for.
"""
self.multibody = multibody
[docs]
def parents(self, body: PhysicalBody) -> list[PhysicalBody]:
"""Get the parents of a body.
Parameters
----------
body : PhysicalBody
The body to get the parents for.
Returns
-------
list[PhysicalBody]
The parents of the given body.
"""
if body == self.multibody.virtualRoot():
return []
parent = body.physicalParentBody()
return [parent]
[docs]
def children(self, body: PhysicalBody) -> list[PhysicalBody]:
"""Get the children of a body.
Parameters
----------
body : PhysicalBody
The body to get the children of.
Returns
-------
list[PhysicalBody]
The children of a the given body.
"""
# return body.childBodies()
return body.multibody().childrenBodies(body)
[docs]
def nodes(self) -> list[PhysicalBody]:
"""Get all the bodies in the multibody (these are all the nodes of the graph).
Returns
-------
list[PhysicalBody]
All the bodies in the Multibody.
"""
return [
cast(PhysicalBody, self.multibody.virtualRoot())
] + self.multibody.sortedPhysicalBodiesList()
[docs]
class FrameGraphAdapter(GraphAdapter[Frame]):
"""FrameContainer wrapper implementing the GraphAdapter interface."""
def __init__(self, frame_container: FrameContainer):
"""Create an instance of FrameGraphAdapter.
Parameters
----------
frame_container : FrameContainer
The frame container to create the GraphAdapter interface for.
"""
self.frame_container = frame_container
[docs]
def parents(self, frame: Frame) -> list[Frame]:
"""Get the parents of a Frame.
Parameters
----------
frame : Frame
The Frame to get the parents of.
Returns
-------
list[Frame]
The parent frame of the given frame.
"""
parent = frame.parent()
if parent is None:
return []
return [parent]
[docs]
def children(self, frame: Frame) -> list[Frame]:
"""Get the children of a Frame.
Parameters
----------
frame : Frame
The Frame to get the children of.
Returns
-------
list[Frame]
The children of the provided Frame.
"""
# Not ideal, but there doesn't seem to be a way to get the
# children directly
children = []
for candidate in self.frame_container.frames():
if candidate.parent() == frame:
children.append(candidate)
return children
[docs]
def nodes(self) -> list[Frame]:
"""Get all Frames of the FrameContainer.
Returns
-------
list[Frame]
All Frames of the FrameContainer.
"""
return self.frame_container.frames()
