#![cfg_attr(not(feature = "std"), no_std)]
#![forbid(unsafe_code)]
#![doc = include_str!(concat!(env!("OUT_DIR"), "/README.md"))]
#![doc = ""]
#![doc = "## Coordinate System"]
#[cfg(test)]
extern crate std;
#[cfg(feature = "alloc")]
extern crate alloc;
pub mod bvh;
mod math;
pub mod render;
pub mod examples {
#[cfg(feature = "examples-armatron")]
#[path = "armatron/main.rs"]
pub mod armatron;
#[cfg(feature = "examples-ballet")]
pub mod ballet;
#[cfg(feature = "examples-clock")]
pub mod clock;
#[cfg(feature = "examples-skeleton-clock")]
pub mod skeleton_clock;
#[cfg(feature = "examples-armatron")]
mod ui;
}
#[cfg(feature = "alloc")]
use alloc::{borrow::ToOwned, boxed::Box, format, string::String, vec::Vec};
use core::fmt;
pub use embedded_graphics::pixelcolor::Rgb888;
pub use embedded_graphics::pixelcolor::WebColors;
pub use embedded_graphics::prelude::RgbColor;
use math::degrees_to_radians;
pub use math::{Mat3, Vec3};
use render::{Disk, Item3d, Projection, Sphere, Stroke};
#[derive(Clone, Copy, Debug)]
pub enum Step {
Start,
Yaw(StepArg),
Pitch(StepArg),
Roll(StepArg),
Forward(StepArg),
Left(StepArg),
Up(StepArg),
PenUp,
PenDown,
PenColor(Rgb888),
PenWidth(f32),
Disk(f32),
DiskParam(ParamArg),
Sphere(f32),
SphereParam(ParamArg),
Mark {
index: usize,
},
Restore {
index: usize,
},
}
#[derive(Clone, Copy, Debug)]
pub enum StepArg {
Fixed(f32),
Variable(ParamArg),
}
#[derive(Clone, Copy, Debug)]
pub struct ParamArg {
index: usize,
low: f32,
span: f32,
}
#[derive(Clone, Copy, Debug)]
pub struct Param {
name: &'static str,
default: f32,
}
impl Param {
const EMPTY: Self = Self {
name: "",
default: 0.0,
};
#[must_use]
pub const fn name(self) -> &'static str {
self.name
}
#[must_use]
pub const fn default(self) -> f32 {
self.default
}
}
impl StepArg {
fn resolve<const DOF: usize>(&self, params: &[f32; DOF]) -> f32 {
match self {
Self::Fixed(value) => *value,
Self::Variable(variable_arg) => variable_arg.resolve(params),
}
}
const fn offset_param(self, offset: usize) -> Self {
match self {
Self::Fixed(_) => self,
Self::Variable(v) => Self::Variable(v.offset(offset)),
}
}
}
impl ParamArg {
const fn new(index: usize, low: f32, high: f32) -> Self {
Self {
index,
low,
span: high - low,
}
}
const fn offset(self, offset: usize) -> Self {
Self {
index: self.index + offset,
..self
}
}
const fn from_degrees(index: usize, low: f32, high: f32) -> Self {
Self::new(index, degrees_to_radians(low), degrees_to_radians(high))
}
fn resolve<const DOF: usize>(&self, params: &[f32; DOF]) -> f32 {
let param = params[self.index];
self.low + param * self.span
}
#[must_use]
pub const fn index(self) -> usize {
self.index
}
#[must_use]
pub const fn low(self) -> f32 {
self.low
}
#[must_use]
pub const fn high(self) -> f32 {
self.low + self.span
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum Error {
InvalidParameter {
index: usize,
value: f32,
},
EmptyLinkage,
MarkNotFound,
MarkAmbiguous,
}
impl fmt::Display for Error {
fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::InvalidParameter { index, value } => {
write!(formatter, "parameter {index} is out of range: {value}")
}
Self::EmptyLinkage => formatter.write_str("linkage produced no poses"),
Self::MarkNotFound => formatter.write_str("mark was not found"),
Self::MarkAmbiguous => formatter.write_str("mark name is ambiguous"),
}
}
}
#[cfg(test)]
impl std::error::Error for Error {}
#[derive(Clone, Copy)]
pub struct LinkageView<'a, const DOF: usize, const MARKS: usize> {
params: &'a [Param; DOF],
steps: &'a [Step],
mark_names: &'a [&'static str; MARKS],
mark_len: usize,
}
impl<'a, const DOF: usize, const MARKS: usize> LinkageView<'a, DOF, MARKS> {
pub const DOF: usize = DOF;
pub const MARKS: usize = MARKS;
#[must_use]
pub(crate) const fn new(
params: &'a [Param; DOF],
steps: &'a [Step],
mark_names: &'a [&'static str; MARKS],
mark_len: usize,
) -> Self {
Self {
params,
steps,
mark_names,
mark_len,
}
}
#[must_use]
pub const fn dof(&self) -> usize {
DOF
}
#[must_use]
pub const fn len(&self) -> usize {
self.steps.len()
}
#[must_use]
pub const fn is_empty(&self) -> bool {
self.steps.is_empty()
}
#[doc(hidden)]
pub const fn step_count(&self) -> usize {
self.len()
}
#[must_use]
pub const fn param(&self, index: usize) -> Param {
self.params[index]
}
#[must_use]
pub const fn params(&self) -> &'a [Param; DOF] {
self.params
}
#[must_use]
pub const fn param_defaults(&self) -> [f32; DOF] {
let mut defaults = [0.0; DOF];
let mut param_index = 0;
while param_index < DOF {
defaults[param_index] = self.params[param_index].default();
param_index += 1;
}
defaults
}
#[must_use]
pub const fn steps(&self) -> &'a [Step] {
self.steps
}
#[must_use]
pub const fn scan_param_range(&self, index: usize) -> (f32, f32) {
let mut i = 0;
while i < self.steps.len() {
let arg = match self.steps[i] {
Step::Yaw(arg)
| Step::Pitch(arg)
| Step::Roll(arg)
| Step::Forward(arg)
| Step::Left(arg)
| Step::Up(arg) => arg,
Step::DiskParam(v) | Step::SphereParam(v) => StepArg::Variable(v),
_ => {
i += 1;
continue;
}
};
if let StepArg::Variable(v) = arg
&& v.index() == index
{
return (v.low(), v.high());
}
i += 1;
}
panic!("no step drives the parameter at this index")
}
#[must_use]
pub const fn mark_names(&self) -> &'a [&'static str; MARKS] {
self.mark_names
}
#[must_use]
pub const fn mark_len(&self) -> usize {
self.mark_len
}
#[must_use]
pub const fn mark_count(&self) -> usize {
self.mark_len()
}
#[must_use]
pub const fn marks(&self) -> usize {
MARKS
}
pub const fn to_fixed<const N: usize>(self) -> LinkageFixed<DOF, MARKS, N> {
assert!(
self.steps.len() <= N,
"fixed backing is too small for linkage view"
);
let mut steps = [const { Step::Start }; N];
let mut step_index = 0;
while step_index < self.steps.len() {
steps[step_index] = self.steps[step_index];
step_index += 1;
}
LinkageFixed {
steps,
len: self.steps.len(),
params: *self.params,
param_len: DOF,
mark_names: *self.mark_names,
mark_len: self.mark_len,
}
}
const fn combine_fixed<
const DOF2: usize,
const MARKS2: usize,
const DOF_OUT: usize,
const MARKS_OUT: usize,
const N_OUT: usize,
>(
self,
other: LinkageView<'_, DOF2, MARKS2>,
) -> LinkageFixed<DOF_OUT, MARKS_OUT, N_OUT> {
let needed_dof = DOF + DOF2;
assert!(DOF_OUT == needed_dof, "DOF_OUT must equal the combined DOF");
let needed_marks = self.mark_len + other.mark_len;
assert!(MARKS_OUT >= needed_marks, "MARKS_OUT is too small");
let needed_steps = self.steps.len() + other.steps.len() - 1;
assert!(N_OUT >= needed_steps, "N_OUT is too small");
let mut out = LinkageFixed {
steps: [const { Step::Start }; N_OUT],
len: self.steps.len(),
params: [Param::EMPTY; DOF_OUT],
param_len: DOF + DOF2,
mark_names: [""; MARKS_OUT],
mark_len: needed_marks,
};
let mut step_index = 0;
while step_index < self.steps.len() {
out.steps[step_index] = self.steps[step_index];
step_index += 1;
}
let mut other_step_index = 1;
while other_step_index < other.steps.len() {
out.steps[out.len] = other.steps[other_step_index].offset_params(DOF, self.mark_len);
out.len += 1;
other_step_index += 1;
}
let mut param_index = 0;
while param_index < DOF {
out.params[param_index] = self.params[param_index];
param_index += 1;
}
let mut other_param_index = 0;
while other_param_index < DOF2 {
out.params[DOF + other_param_index] = other.params[other_param_index];
other_param_index += 1;
}
let mut mark_index = 0;
while mark_index < self.mark_len {
out.mark_names[mark_index] = self.mark_names[mark_index];
mark_index += 1;
}
let mut other_mark_index = 0;
while other_mark_index < other.mark_len {
out.mark_names[self.mark_len + other_mark_index] = other.mark_names[other_mark_index];
other_mark_index += 1;
}
out
}
#[cfg(feature = "alloc")]
#[must_use]
pub fn to_lb_rs(&self) -> String {
let named_params = self.params.iter().filter(|p| !p.name().is_empty()).count();
let mut source = format!(
"// DOF={} MARKS={} STEPS={}\n",
named_params,
self.mark_len,
self.len()
);
source.push_str("linkage![\n");
for param in self.params {
if !param.name().is_empty() {
source.push_str(" .define_param(\"");
source.push_str(param.name());
source.push_str("\", ");
push_f32(&mut source, param.default());
source.push_str(")\n");
}
}
for &step in self.steps {
match step {
Step::Start => {}
Step::Yaw(arg) => push_arg_step(self, &mut source, "yaw", "yaw_param", arg, true),
Step::Pitch(arg) => {
push_arg_step(self, &mut source, "pitch", "pitch_param", arg, true);
}
Step::Roll(arg) => {
push_arg_step(self, &mut source, "roll", "roll_param", arg, true);
}
Step::Forward(arg) => {
push_arg_step(self, &mut source, "forward", "forward_param", arg, false);
}
Step::Left(arg) => {
push_arg_step(self, &mut source, "left", "left_param", arg, false);
}
Step::Up(arg) => push_arg_step(self, &mut source, "up", "up_param", arg, false),
Step::PenUp => source.push_str(" .pen_up()\n"),
Step::PenDown => source.push_str(" .pen_down()\n"),
Step::PenColor(color) => {
source.push_str(" .pen_color(Rgb888::new(");
source.push_str(&format!("{}, {}, {}", color.r(), color.g(), color.b()));
source.push_str("))\n");
}
Step::PenWidth(width) => {
source.push_str(" .pen_width(");
push_f32(&mut source, width);
source.push_str(")\n");
}
Step::Disk(radius) => push_fixed_step(&mut source, "disk", radius),
Step::DiskParam(arg) => push_variable_step(self, &mut source, "disk_param", arg),
Step::Sphere(radius) => push_fixed_step(&mut source, "sphere", radius),
Step::SphereParam(arg) => {
push_variable_step(self, &mut source, "sphere_param", arg);
}
Step::Mark { index } => {
let name = self.mark_names[index];
source.push_str(" .mark(\"");
source.push_str(name);
source.push_str("\")\n");
}
Step::Restore { index } => {
let name = self.mark_names[index];
source.push_str(" .restore(\"");
source.push_str(name);
source.push_str("\")\n");
}
}
}
source.push_str("]\n");
source
}
#[must_use]
pub const fn param_index(&self, name: &str, n: usize) -> usize {
let mut found = 0;
let mut i = 0;
while i < DOF {
if str_eq(self.params[i].name, name) {
if found == n {
return i;
}
found += 1;
}
i += 1;
}
panic!("parameter name not found or occurrence index out of range")
}
pub fn final_pose(&self, params: &[f32; DOF]) -> Result<Pose, Error> {
self.poses(params)?.last().ok_or(Error::EmptyLinkage)
}
pub fn poses<'b>(
&'b self,
params: &'b [f32; DOF],
) -> Result<impl Iterator<Item = Pose> + 'b, Error> {
Ok(self.styled_poses(params)?.map(|sp| sp.pose()))
}
pub fn styled_poses<'b>(
&'b self,
params: &'b [f32; DOF],
) -> Result<impl Iterator<Item = StyledPose> + 'b, Error> {
StyledPosesView::<DOF, MARKS>::new(self.steps, params)
}
pub fn draw_items_3d<'b>(
&'b self,
params: &'b [f32; DOF],
) -> Result<DrawItem3dIter<'b, DOF, MARKS>, Error> {
DrawItem3dIter::<DOF, MARKS>::new(self.steps, self.mark_names, self.mark_len, params)
}
#[must_use]
pub const fn draw_item_3d_count(&self) -> usize {
let mut count = 0;
let mut pen_down = true;
let mut marked = [true; MARKS];
let mut index = 0;
while index < self.steps.len() {
match &self.steps[index] {
Step::Start => pen_down = true,
Step::PenUp => pen_down = false,
Step::PenDown => pen_down = true,
Step::Mark { index: mark_index } => marked[*mark_index] = pen_down,
Step::Restore { index: mark_index } => pen_down = marked[*mark_index],
Step::Forward(_) | Step::Left(_) | Step::Up(_) => {
if pen_down {
count += 1;
}
}
Step::Disk(_) | Step::DiskParam(_) | Step::Sphere(_) | Step::SphereParam(_) => {
count += 1;
}
Step::Yaw(_)
| Step::Pitch(_)
| Step::Roll(_)
| Step::PenColor(_)
| Step::PenWidth(_) => {}
}
index += 1;
}
count
}
}
impl<'a, const DOF: usize, const MARKS: usize, const N: usize> From<&'a LinkageFixed<DOF, MARKS, N>>
for LinkageView<'a, DOF, MARKS>
{
fn from(linkage: &'a LinkageFixed<DOF, MARKS, N>) -> Self {
linkage.view()
}
}
#[cfg(feature = "alloc")]
fn push_arg_step<const DOF: usize, const MARKS: usize>(
linkage_view: &LinkageView<'_, DOF, MARKS>,
source: &mut String,
fixed_method: &str,
variable_method: &str,
arg: StepArg,
degrees: bool,
) {
match arg {
StepArg::Fixed(value) => {
let value = if degrees { value.to_degrees() } else { value };
push_fixed_step(source, fixed_method, value);
}
StepArg::Variable(variable_arg) => {
let low = if degrees {
variable_arg.low().to_degrees()
} else {
variable_arg.low()
};
let high = if degrees {
variable_arg.high().to_degrees()
} else {
variable_arg.high()
};
source.push_str(" .");
source.push_str(variable_method);
source.push_str("(\"");
source.push_str(linkage_view.param(variable_arg.index()).name());
source.push_str("\", ");
push_f32(source, low);
source.push_str(", ");
push_f32(source, high);
source.push_str(")\n");
}
}
}
#[cfg(feature = "alloc")]
fn push_fixed_step(source: &mut String, method: &str, value: f32) {
source.push_str(" .");
source.push_str(method);
source.push('(');
push_f32(source, value);
source.push_str(")\n");
}
#[cfg(feature = "alloc")]
fn push_variable_step<const DOF: usize, const MARKS: usize>(
linkage_view: &LinkageView<'_, DOF, MARKS>,
source: &mut String,
method: &str,
variable_arg: ParamArg,
) {
source.push_str(" .");
source.push_str(method);
source.push_str("(\"");
source.push_str(linkage_view.param(variable_arg.index()).name());
source.push_str("\", ");
push_f32(source, variable_arg.low());
source.push_str(", ");
push_f32(source, variable_arg.high());
source.push_str(")\n");
}
#[cfg(feature = "alloc")]
fn push_f32(source: &mut String, value: f32) {
source.push_str(&format!("{value:?}"));
}
#[cfg(feature = "alloc")]
fn parse_lb_rs<const DOF: usize, const MARKS: usize>(
source: &str,
) -> Result<LinkageBuf<DOF, MARKS>, String> {
let mut linkage = LinkageBuf::start();
for (line_index, line) in source.lines().enumerate() {
let line_number = line_index + 1;
let Some(method_call) = parse_method_call(line_number, line)? else {
continue;
};
linkage = apply_parsed_method(line_number, linkage, &method_call)?;
}
Ok(linkage)
}
#[cfg(feature = "alloc")]
#[derive(Clone, Debug)]
struct ParsedMethodCall {
name: String,
args: Vec<String>,
}
#[cfg(feature = "alloc")]
fn parse_method_call(line_number: usize, line: &str) -> Result<Option<ParsedMethodCall>, String> {
let line = strip_rust_comment(line).trim();
if line.is_empty() || line == "LinkageFixed::start()" || is_linkage_macro_wrapper(line) {
return Ok(None);
}
if line.contains("LinkageFixed::start()") {
return Ok(None);
}
let line = line.trim_end_matches(';').trim_end_matches(',').trim();
let line = line.strip_prefix('.').unwrap_or(line);
let open = line
.find('(')
.ok_or_else(|| format!("line {line_number}: expected `(`"))?;
let close = line
.rfind(')')
.ok_or_else(|| format!("line {line_number}: expected `)`"))?;
if close < open {
return Err(format!("line {line_number}: `)` appears before `(`"));
}
let trailing = line[close + 1..].trim();
if !trailing.is_empty() {
return Err(format!(
"line {line_number}: unexpected text after method call `{trailing}`"
));
}
let name = line[..open].trim();
if name.is_empty() {
return Err(format!("line {line_number}: method name is empty"));
}
Ok(Some(ParsedMethodCall {
name: name.to_owned(),
args: split_args(line_number, &line[open + 1..close])?,
}))
}
#[cfg(feature = "alloc")]
fn is_linkage_macro_wrapper(line: &str) -> bool {
let line = line.trim_end_matches(';').trim();
matches!(line, "linkage![" | "linkage! [" | "]")
}
#[cfg(feature = "alloc")]
fn split_args(line_number: usize, args: &str) -> Result<Vec<String>, String> {
let mut split_args = Vec::new();
let mut current_arg = String::new();
let mut parenthesis_depth = 0;
let mut in_string = false;
for character in args.chars() {
match character {
'"' => {
in_string = !in_string;
current_arg.push(character);
}
'(' if !in_string => {
parenthesis_depth += 1;
current_arg.push(character);
}
')' if !in_string => {
if parenthesis_depth == 0 {
return Err(format!(
"line {line_number}: unexpected `)` in argument list"
));
}
parenthesis_depth -= 1;
current_arg.push(character);
}
',' if !in_string && parenthesis_depth == 0 => {
let trimmed_arg = current_arg.trim();
if !trimmed_arg.is_empty() {
split_args.push(trimmed_arg.to_owned());
}
current_arg = String::new();
}
_ => current_arg.push(character),
}
}
if in_string {
return Err(format!("line {line_number}: unterminated string literal"));
}
if parenthesis_depth != 0 {
return Err(format!("line {line_number}: unterminated nested argument"));
}
let current_arg = current_arg.trim();
if !current_arg.is_empty() {
split_args.push(current_arg.to_owned());
}
Ok(split_args)
}
#[cfg(feature = "alloc")]
fn apply_parsed_method<const DOF: usize, const MARKS: usize>(
line_number: usize,
linkage: LinkageBuf<DOF, MARKS>,
method_call: &ParsedMethodCall,
) -> Result<LinkageBuf<DOF, MARKS>, String> {
match method_call.name.as_str() {
"define_param" => {
expect_arg_count(line_number, method_call, 2)?;
let name = parse_static_string_arg(line_number, method_call, 0)?;
let default = parse_number_arg(line_number, method_call, 1)?;
if !(0.0..=1.0).contains(&default) {
return Err(format!(
"line {line_number}: define_param default must be between 0.0 and 1.0"
));
}
Ok(linkage.define_param(name, default))
}
"forward" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.forward(parse_number_arg(line_number, method_call, 0)?))
}
"forward_param" => apply_translation_param(line_number, linkage, method_call, "forward"),
"left" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.left(parse_number_arg(line_number, method_call, 0)?))
}
"left_param" => apply_translation_param(line_number, linkage, method_call, "left"),
"up" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.up(parse_number_arg(line_number, method_call, 0)?))
}
"up_param" => apply_translation_param(line_number, linkage, method_call, "up"),
"yaw" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.yaw(parse_number_arg(line_number, method_call, 0)?))
}
"yaw_param" => apply_rotation_param(line_number, linkage, method_call, "yaw"),
"pitch" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.pitch(parse_number_arg(line_number, method_call, 0)?))
}
"pitch_param" => apply_rotation_param(line_number, linkage, method_call, "pitch"),
"roll" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.roll(parse_number_arg(line_number, method_call, 0)?))
}
"roll_param" => apply_rotation_param(line_number, linkage, method_call, "roll"),
"pen_up" => {
expect_arg_count(line_number, method_call, 0)?;
Ok(linkage.pen_up())
}
"pen_down" => {
expect_arg_count(line_number, method_call, 0)?;
Ok(linkage.pen_down())
}
"pen_color" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.pen_color(parse_color_arg(line_number, method_call)?))
}
"pen_width" => {
expect_arg_count(line_number, method_call, 1)?;
let width = parse_number_arg(line_number, method_call, 0)?;
if width < 0.0 {
return Err(format!(
"line {line_number}: pen_width must be non-negative"
));
}
Ok(linkage.pen_width(width))
}
"mark" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.mark(parse_static_string_arg(line_number, method_call, 0)?))
}
"restore" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.restore(parse_static_string_arg(line_number, method_call, 0)?))
}
"disk" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.disk(parse_radius(line_number, method_call, 0)?))
}
"disk_param" => apply_radius_param(line_number, linkage, method_call, "disk"),
"sphere" => {
expect_arg_count(line_number, method_call, 1)?;
Ok(linkage.sphere(parse_radius(line_number, method_call, 0)?))
}
"sphere_param" => apply_radius_param(line_number, linkage, method_call, "sphere"),
_ => Err(format!(
"line {line_number}: unknown method `{}`",
method_call.name
)),
}
}
#[cfg(feature = "alloc")]
fn apply_translation_param<const DOF: usize, const MARKS: usize>(
line_number: usize,
linkage: LinkageBuf<DOF, MARKS>,
method_call: &ParsedMethodCall,
axis: &str,
) -> Result<LinkageBuf<DOF, MARKS>, String> {
expect_arg_count(line_number, method_call, 3)?;
let name = parse_string_arg(line_number, method_call, 0)?;
let low = parse_number_arg(line_number, method_call, 1)?;
let high = parse_number_arg(line_number, method_call, 2)?;
match axis {
"forward" => Ok(linkage.forward_param(name, low, high)),
"left" => Ok(linkage.left_param(name, low, high)),
"up" => Ok(linkage.up_param(name, low, high)),
_ => unreachable!(),
}
}
#[cfg(feature = "alloc")]
fn apply_rotation_param<const DOF: usize, const MARKS: usize>(
line_number: usize,
linkage: LinkageBuf<DOF, MARKS>,
method_call: &ParsedMethodCall,
axis: &str,
) -> Result<LinkageBuf<DOF, MARKS>, String> {
expect_arg_count(line_number, method_call, 3)?;
let name = parse_string_arg(line_number, method_call, 0)?;
let low = parse_number_arg(line_number, method_call, 1)?;
let high = parse_number_arg(line_number, method_call, 2)?;
match axis {
"yaw" => Ok(linkage.yaw_param(name, low, high)),
"pitch" => Ok(linkage.pitch_param(name, low, high)),
"roll" => Ok(linkage.roll_param(name, low, high)),
_ => unreachable!(),
}
}
#[cfg(feature = "alloc")]
fn apply_radius_param<const DOF: usize, const MARKS: usize>(
line_number: usize,
linkage: LinkageBuf<DOF, MARKS>,
method_call: &ParsedMethodCall,
shape: &str,
) -> Result<LinkageBuf<DOF, MARKS>, String> {
expect_arg_count(line_number, method_call, 3)?;
let name = parse_string_arg(line_number, method_call, 0)?;
let low = parse_number_arg(line_number, method_call, 1)?;
let high = parse_number_arg(line_number, method_call, 2)?;
if low < 0.0 || high < 0.0 {
return Err(format!(
"line {line_number}: `{}` radius range must be non-negative",
method_call.name
));
}
match shape {
"disk" => Ok(linkage.disk_param(name, low, high)),
"sphere" => Ok(linkage.sphere_param(name, low, high)),
_ => unreachable!(),
}
}
#[cfg(feature = "alloc")]
fn strip_rust_comment(line: &str) -> &str {
line.split_once("//")
.map_or(line, |(before_comment, _)| before_comment)
}
#[cfg(feature = "alloc")]
fn expect_arg_count(
line_number: usize,
method_call: &ParsedMethodCall,
expected: usize,
) -> Result<(), String> {
if method_call.args.len() == expected {
Ok(())
} else {
Err(format!(
"line {line_number}: `{}` expects {expected} argument(s), got {}",
method_call.name,
method_call.args.len()
))
}
}
#[cfg(feature = "alloc")]
fn parse_number_arg(
line_number: usize,
method_call: &ParsedMethodCall,
arg_index: usize,
) -> Result<f32, String> {
let value = &method_call.args[arg_index];
parse_number_or_constant(line_number, method_call.name.as_str(), value)
}
#[cfg(feature = "alloc")]
fn parse_static_string_arg(
line_number: usize,
method_call: &ParsedMethodCall,
arg_index: usize,
) -> Result<&'static str, String> {
let value = parse_string_arg(line_number, method_call, arg_index)?;
Ok(Box::leak(value.to_owned().into_boxed_str()))
}
#[cfg(feature = "alloc")]
fn parse_string_arg(
line_number: usize,
method_call: &ParsedMethodCall,
arg_index: usize,
) -> Result<&str, String> {
let value = method_call.args[arg_index].as_str();
value
.strip_prefix('"')
.and_then(|value| value.strip_suffix('"'))
.ok_or_else(|| {
format!(
"line {line_number}: `{}` argument `{value}` must be a string literal",
method_call.name
)
})
}
#[cfg(feature = "alloc")]
fn parse_number_or_constant(
line_number: usize,
method_name: &str,
value: &str,
) -> Result<f32, String> {
let numeric_value: String = value
.chars()
.filter(|character| *character != '_')
.collect();
if let Ok(parsed) = numeric_value.parse::<f32>() {
if looks_like_integer(&numeric_value) {
return Err(format!(
"line {line_number}: `{method_name}` argument `{value}` is an integer; use `{value}.0`"
));
}
return Ok(parsed);
}
number_constant(value).ok_or_else(|| {
format!(
"line {line_number}: `{method_name}` argument `{value}` is not a number or known constant"
)
})
}
#[cfg(feature = "alloc")]
fn looks_like_integer(s: &str) -> bool {
let digits = s
.strip_prefix('-')
.or_else(|| s.strip_prefix('+'))
.unwrap_or(s);
!digits.is_empty() && digits.chars().all(|c| c.is_ascii_digit())
}
#[cfg(feature = "alloc")]
fn parse_radius(
line_number: usize,
method_call: &ParsedMethodCall,
arg_index: usize,
) -> Result<f32, String> {
let radius = parse_number_arg(line_number, method_call, arg_index)?;
if radius < 0.0 {
Err(format!(
"line {line_number}: `{}` radius must be non-negative",
method_call.name
))
} else {
Ok(radius)
}
}
#[cfg(feature = "alloc")]
fn number_constant(name: &str) -> Option<f32> {
match name {
"ARM_WIDTH" => Some(3.0),
"AXIS_WIDTH" => Some(1.0),
_ => None,
}
}
#[cfg(feature = "alloc")]
fn parse_color_arg(line_number: usize, method_call: &ParsedMethodCall) -> Result<Rgb888, String> {
let value = method_call.args[0].as_str();
if let Some(color_args) = value
.strip_prefix("Rgb888::new(")
.and_then(|value| value.strip_suffix(')'))
{
return parse_rgb888_new_color(line_number, color_args);
}
match value {
"Rgb888::CSS_BLACK" => Ok(Rgb888::CSS_BLACK),
"Rgb888::CSS_BLUE" => Ok(Rgb888::CSS_BLUE),
"Rgb888::CSS_CRIMSON" => Ok(Rgb888::CSS_CRIMSON),
"Rgb888::CSS_DARK_SLATE_GRAY" => Ok(Rgb888::CSS_DARK_SLATE_GRAY),
"Rgb888::CSS_DIM_GRAY" => Ok(Rgb888::CSS_DIM_GRAY),
"Rgb888::CSS_GRAY" => Ok(Rgb888::CSS_GRAY),
"Rgb888::CSS_LIGHT_GRAY" => Ok(Rgb888::CSS_LIGHT_GRAY),
"Rgb888::CSS_LIGHT_SLATE_GRAY" => Ok(Rgb888::CSS_LIGHT_SLATE_GRAY),
"Rgb888::CSS_RED" => Ok(Rgb888::CSS_RED),
"Rgb888::CSS_SLATE_GRAY" => Ok(Rgb888::CSS_SLATE_GRAY),
"Rgb888::CSS_STEEL_BLUE" => Ok(Rgb888::CSS_STEEL_BLUE),
"Rgb888::CSS_WHITE" => Ok(Rgb888::CSS_WHITE),
_ => Err(format!(
"line {line_number}: unknown color `{value}`; use `Rgb888::CSS_*` or `Rgb888::new(r, g, b)`"
)),
}
}
#[cfg(feature = "alloc")]
fn parse_rgb888_new_color(line_number: usize, args: &str) -> Result<Rgb888, String> {
let args = split_args(line_number, args)?;
if args.len() != 3 {
return Err(format!(
"line {line_number}: `Rgb888::new` expects 3 argument(s), got {}",
args.len()
));
}
Ok(Rgb888::new(
parse_u8_arg(line_number, "Rgb888::new", &args[0])?,
parse_u8_arg(line_number, "Rgb888::new", &args[1])?,
parse_u8_arg(line_number, "Rgb888::new", &args[2])?,
))
}
#[cfg(feature = "alloc")]
fn parse_u8_arg(line_number: usize, method_name: &str, value: &str) -> Result<u8, String> {
let numeric_value: String = value
.chars()
.filter(|character| *character != '_')
.collect();
numeric_value
.parse::<u8>()
.map_err(|_| format!("line {line_number}: `{method_name}` argument `{value}` is not a u8"))
}
macro_rules! emit_fixed_step_methods {
() => {
pub const fn yaw(self, degrees: f32) -> Self {
self.push(Step::Yaw(StepArg::Fixed(degrees_to_radians(degrees))))
}
pub const fn pitch(self, degrees: f32) -> Self {
self.push(Step::Pitch(StepArg::Fixed(degrees_to_radians(degrees))))
}
pub const fn roll(self, degrees: f32) -> Self {
self.push(Step::Roll(StepArg::Fixed(degrees_to_radians(degrees))))
}
pub const fn forward(self, distance: f32) -> Self {
self.push(Step::Forward(StepArg::Fixed(distance)))
}
pub const fn left(self, distance: f32) -> Self {
self.push(Step::Left(StepArg::Fixed(distance)))
}
pub const fn up(self, distance: f32) -> Self {
self.push(Step::Up(StepArg::Fixed(distance)))
}
pub const fn pen_up(self) -> Self {
self.push(Step::PenUp)
}
pub const fn pen_down(self) -> Self {
self.push(Step::PenDown)
}
pub const fn pen_color(self, color: Rgb888) -> Self {
self.push(Step::PenColor(color))
}
pub const fn pen_width(self, width: f32) -> Self {
assert!(width >= 0.0, "pen width must be non-negative");
self.push(Step::PenWidth(width))
}
pub const fn disk(self, radius: f32) -> Self {
self.push(Step::Disk(radius))
}
pub const fn sphere(self, radius: f32) -> Self {
self.push(Step::Sphere(radius))
}
pub const fn yaw_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push(Step::Yaw(StepArg::Variable(ParamArg::from_degrees(
index, low, high,
))))
}
pub const fn pitch_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push(Step::Pitch(StepArg::Variable(ParamArg::from_degrees(
index, low, high,
))))
}
pub const fn roll_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push(Step::Roll(StepArg::Variable(ParamArg::from_degrees(
index, low, high,
))))
}
pub const fn forward_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push(Step::Forward(StepArg::Variable(ParamArg::new(
index, low, high,
))))
}
pub const fn left_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push(Step::Left(StepArg::Variable(ParamArg::new(
index, low, high,
))))
}
pub const fn up_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push(Step::Up(StepArg::Variable(ParamArg::new(index, low, high))))
}
pub const fn disk_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push(Step::DiskParam(ParamArg::new(index, low, high)))
}
pub const fn sphere_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push(Step::SphereParam(ParamArg::new(index, low, high)))
}
pub const fn restore(self, name: &'static str) -> Self {
let index = match self.mark_index(name) {
Some(i) => i,
None => {
panic!("restore: no mark found with name (mark must be defined before restore)")
}
};
self.push(Step::Restore { index })
}
};
}
#[cfg(feature = "alloc")]
macro_rules! emit_buf_step_methods {
() => {
pub fn yaw(self, degrees: f32) -> Self {
self.push_step(Step::Yaw(StepArg::Fixed(degrees_to_radians(degrees))))
}
pub fn pitch(self, degrees: f32) -> Self {
self.push_step(Step::Pitch(StepArg::Fixed(degrees_to_radians(degrees))))
}
pub fn roll(self, degrees: f32) -> Self {
self.push_step(Step::Roll(StepArg::Fixed(degrees_to_radians(degrees))))
}
pub fn forward(self, distance: f32) -> Self {
self.push_step(Step::Forward(StepArg::Fixed(distance)))
}
pub fn left(self, distance: f32) -> Self {
self.push_step(Step::Left(StepArg::Fixed(distance)))
}
pub fn up(self, distance: f32) -> Self {
self.push_step(Step::Up(StepArg::Fixed(distance)))
}
pub fn pen_up(self) -> Self {
self.push_step(Step::PenUp)
}
pub fn pen_down(self) -> Self {
self.push_step(Step::PenDown)
}
pub fn pen_color(self, color: Rgb888) -> Self {
self.push_step(Step::PenColor(color))
}
pub fn pen_width(self, width: f32) -> Self {
assert!(width >= 0.0, "pen width must be non-negative");
self.push_step(Step::PenWidth(width))
}
pub fn disk(self, radius: f32) -> Self {
self.push_step(Step::Disk(radius))
}
pub fn sphere(self, radius: f32) -> Self {
self.push_step(Step::Sphere(radius))
}
pub fn yaw_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push_step(Step::Yaw(StepArg::Variable(ParamArg::from_degrees(
index, low, high,
))))
}
pub fn pitch_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push_step(Step::Pitch(StepArg::Variable(ParamArg::from_degrees(
index, low, high,
))))
}
pub fn roll_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push_step(Step::Roll(StepArg::Variable(ParamArg::from_degrees(
index, low, high,
))))
}
pub fn forward_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push_step(Step::Forward(StepArg::Variable(ParamArg::new(
index, low, high,
))))
}
pub fn left_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push_step(Step::Left(StepArg::Variable(ParamArg::new(
index, low, high,
))))
}
pub fn up_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push_step(Step::Up(StepArg::Variable(ParamArg::new(index, low, high))))
}
pub fn disk_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push_step(Step::DiskParam(ParamArg::new(index, low, high)))
}
pub fn sphere_param(self, name: &str, low: f32, high: f32) -> Self {
let index = self.expect_param_index(name);
self.push_step(Step::SphereParam(ParamArg::new(index, low, high)))
}
};
}
#[doc(hidden)]
pub struct LinkageStepCount {
step_count: usize,
param_count: usize,
mark_count: usize,
}
impl LinkageStepCount {
pub const fn start() -> Self {
Self {
step_count: 1,
param_count: 0,
mark_count: 0,
}
}
pub const fn step_count(self) -> usize {
self.step_count
}
pub const fn param_count(self) -> usize {
self.param_count
}
pub const fn mark_count(self) -> usize {
self.mark_count
}
const fn push(mut self) -> Self {
self.step_count += 1;
self
}
pub const fn define_param(mut self, _name: &'static str, _default: f32) -> Self {
self.param_count += 1;
self
}
pub const fn mark(mut self, _name: &'static str) -> Self {
self.mark_count += 1;
self.push()
}
pub const fn restore(self, _name: &'static str) -> Self {
self.push()
}
pub const fn yaw(self, _degrees: f32) -> Self {
self.push()
}
pub const fn pitch(self, _degrees: f32) -> Self {
self.push()
}
pub const fn roll(self, _degrees: f32) -> Self {
self.push()
}
pub const fn forward(self, _distance: f32) -> Self {
self.push()
}
pub const fn left(self, _distance: f32) -> Self {
self.push()
}
pub const fn up(self, _distance: f32) -> Self {
self.push()
}
pub const fn pen_up(self) -> Self {
self.push()
}
pub const fn pen_down(self) -> Self {
self.push()
}
pub const fn pen_color(self, _color: Rgb888) -> Self {
self.push()
}
pub const fn pen_width(self, _width: f32) -> Self {
self.push()
}
pub const fn disk(self, _radius: f32) -> Self {
self.push()
}
pub const fn sphere(self, _radius: f32) -> Self {
self.push()
}
pub const fn yaw_param(self, _name: &str, _low: f32, _high: f32) -> Self {
self.push()
}
pub const fn pitch_param(self, _name: &str, _low: f32, _high: f32) -> Self {
self.push()
}
pub const fn roll_param(self, _name: &str, _low: f32, _high: f32) -> Self {
self.push()
}
pub const fn forward_param(self, _name: &str, _low: f32, _high: f32) -> Self {
self.push()
}
pub const fn left_param(self, _name: &str, _low: f32, _high: f32) -> Self {
self.push()
}
pub const fn up_param(self, _name: &str, _low: f32, _high: f32) -> Self {
self.push()
}
pub const fn disk_param(self, _name: &str, _low: f32, _high: f32) -> Self {
self.push()
}
pub const fn sphere_param(self, _name: &str, _low: f32, _high: f32) -> Self {
self.push()
}
}
pub struct LinkageFixed<const DOF: usize, const MARKS: usize, const N: usize> {
steps: [Step; N],
len: usize,
params: [Param; DOF],
param_len: usize,
mark_names: [&'static str; MARKS],
mark_len: usize,
}
impl<const DOF: usize, const MARKS: usize, const N: usize> LinkageFixed<DOF, MARKS, N> {
pub const fn start() -> Self {
assert!(N > 0, "linkage must have room for the implicit start step");
Self {
steps: [const { Step::Start }; N],
len: 1,
params: [Param::EMPTY; DOF],
param_len: 0,
mark_names: [""; MARKS],
mark_len: 0,
}
}
pub const DOF: usize = DOF;
pub const N: usize = N;
pub const MARKS: usize = MARKS;
#[must_use]
#[inline]
pub const fn view(&self) -> LinkageView<'_, DOF, MARKS> {
LinkageView::new(
&self.params,
self.steps.split_at(self.len).0,
&self.mark_names,
self.mark_len,
)
}
#[must_use]
pub const fn dof(&self) -> usize {
DOF
}
#[must_use]
pub const fn step_count(&self) -> usize {
self.len
}
#[doc(hidden)]
pub const fn len(&self) -> usize {
self.len
}
#[doc(hidden)]
#[must_use]
pub const fn is_empty(&self) -> bool {
self.len == 0
}
#[must_use]
pub const fn param_count(&self) -> usize {
self.param_len
}
#[must_use]
pub const fn mark_count(&self) -> usize {
self.mark_len
}
pub const fn define_param(mut self, name: &'static str, default: f32) -> Self {
assert!(
self.param_len < DOF,
"DOF is too small; use a large DOF then call .param_count() to find the exact value"
);
assert!(default >= 0.0, "parameter default must be at least 0.0");
assert!(default <= 1.0, "parameter default must be at most 1.0");
self.params[self.param_len] = Param { name, default };
self.param_len += 1;
self
}
emit_fixed_step_methods!();
pub const fn mark(mut self, name: &'static str) -> Self {
let index = match self.mark_index(name) {
Some(index) => index,
None => {
assert!(
self.mark_len < MARKS,
"MARKS is too small; use a large MARKS then call .mark_count() to find the exact value"
);
let index = self.mark_len;
self.mark_names[index] = name;
self.mark_len += 1;
index
}
};
self.push(Step::Mark { index })
}
const fn mark_index(&self, name: &str) -> Option<usize> {
let mut mark_index = 0;
while mark_index < self.mark_len {
if str_eq(self.mark_names[mark_index], name) {
return Some(mark_index);
}
mark_index += 1;
}
None
}
const fn push(mut self, step: Step) -> Self {
assert!(
self.len < N,
"N is too small; use a large N then call .step_count() to find the exact value"
);
self.steps[self.len] = step;
self.len += 1;
self
}
const fn last_param_index(&self, name: &str) -> Option<usize> {
let mut i = self.param_len;
while i > 0 {
i -= 1;
if str_eq(self.params[i].name, name) {
return Some(i);
}
}
None
}
const fn expect_param_index(&self, name: &str) -> usize {
match self.last_param_index(name) {
Some(index) => index,
None => panic!("unknown parameter name"),
}
}
pub const fn freeze_param_index<const OUT_DOF: usize>(
self,
param_index: usize,
raw_value: f32,
) -> LinkageFixed<OUT_DOF, MARKS, N> {
let mut is_frozen = [false; DOF];
let frozen_at_default = [false; DOF];
let mut frozen_raw = [0.0f32; DOF];
assert!(
param_index < self.param_len,
"freeze param index out of bounds"
);
is_frozen[param_index] = true;
frozen_raw[param_index] = raw_value;
self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
}
pub const fn freeze_param_name<const OUT_DOF: usize>(
self,
name: &'static str,
raw_value: f32,
) -> LinkageFixed<OUT_DOF, MARKS, N> {
let mut found_index = 0usize;
let mut found_count = 0usize;
let mut param_index = 0;
while param_index < self.param_len {
if str_eq(self.params[param_index].name, name) {
found_index = param_index;
found_count += 1;
}
param_index += 1;
}
assert!(found_count > 0, "freeze name not found in params");
assert!(found_count == 1, "freeze name is ambiguous");
self.freeze_param_index(found_index, raw_value)
}
pub const fn freeze_param_index_at_default<const OUT_DOF: usize>(
self,
param_index: usize,
) -> LinkageFixed<OUT_DOF, MARKS, N> {
let mut is_frozen = [false; DOF];
let mut frozen_at_default = [false; DOF];
let frozen_raw = [0.0f32; DOF];
assert!(
param_index < self.param_len,
"freeze param index out of bounds"
);
is_frozen[param_index] = true;
frozen_at_default[param_index] = true;
self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
}
pub const fn retain_param_indexes<const OUT_DOF: usize>(
self,
indexes: &[usize],
) -> LinkageFixed<OUT_DOF, MARKS, N> {
let mut index_index = 0;
while index_index < indexes.len() {
let retain_index = indexes[index_index];
assert!(
retain_index < self.param_len,
"retain param index out of bounds"
);
let mut previous_index = 0;
while previous_index < index_index {
assert!(
indexes[previous_index] != retain_index,
"duplicate index in retain list"
);
previous_index += 1;
}
index_index += 1;
}
let mut is_frozen = [false; DOF];
let mut frozen_at_default = [false; DOF];
let frozen_raw = [0.0f32; DOF];
let mut param_index = 0;
while param_index < self.param_len {
let mut found = false;
let mut index_index = 0;
while index_index < indexes.len() {
if param_index == indexes[index_index] {
found = true;
break;
}
index_index += 1;
}
if !found {
is_frozen[param_index] = true;
frozen_at_default[param_index] = true;
}
param_index += 1;
}
self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
}
pub const fn retain_param_names<const OUT_DOF: usize>(
self,
names: &[&'static str],
) -> LinkageFixed<OUT_DOF, MARKS, N> {
let mut name_index = 0;
while name_index < names.len() {
let retain_name = names[name_index];
let mut previous_name_index = 0;
while previous_name_index < name_index {
assert!(
!str_eq(names[previous_name_index], retain_name),
"duplicate name in retain list"
);
previous_name_index += 1;
}
let mut found = false;
let mut param_index = 0;
while param_index < self.param_len {
if str_eq(self.params[param_index].name, retain_name) {
found = true;
break;
}
param_index += 1;
}
assert!(found, "retain name not found in params");
name_index += 1;
}
let mut is_frozen = [false; DOF];
let mut frozen_at_default = [false; DOF];
let frozen_raw = [0.0f32; DOF];
let mut param_index = 0;
while param_index < self.param_len {
let mut found = false;
let mut name_index = 0;
while name_index < names.len() {
if str_eq(self.params[param_index].name, names[name_index]) {
found = true;
break;
}
name_index += 1;
}
if !found {
is_frozen[param_index] = true;
frozen_at_default[param_index] = true;
}
param_index += 1;
}
self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
}
const fn freeze_with_map<const OUT_DOF: usize>(
self,
is_frozen: [bool; DOF],
frozen_at_default: [bool; DOF],
frozen_raw: [f32; DOF],
) -> LinkageFixed<OUT_DOF, MARKS, N> {
let mut new_param_index = [0usize; DOF];
let mut new_param_len = 0usize;
let mut param_index = 0;
while param_index < self.param_len {
if !is_frozen[param_index] {
new_param_index[param_index] = new_param_len;
new_param_len += 1;
}
param_index += 1;
}
assert!(
new_param_len == OUT_DOF,
"OUT_DOF must equal DOF minus the number of frozen parameters"
);
let mut out = LinkageFixed {
steps: [const { Step::Start }; N],
len: 0,
params: [Param::EMPTY; OUT_DOF],
param_len: 0,
mark_names: [""; MARKS],
mark_len: self.mark_len,
};
let mut mark_index = 0;
while mark_index < self.mark_len {
out.mark_names[mark_index] = self.mark_names[mark_index];
mark_index += 1;
}
let mut param_index = 0;
while param_index < self.param_len {
if !is_frozen[param_index] {
out.params[new_param_index[param_index]] = self.params[param_index];
out.param_len += 1;
}
param_index += 1;
}
let mut step_index = 0;
while step_index < self.len {
out.steps[step_index] = rewrite_step_for_freeze(
self.steps[step_index],
&is_frozen,
&frozen_at_default,
&frozen_raw,
&self.params,
&new_param_index,
);
step_index += 1;
}
out.len = self.len;
out.strip_fixed_noops_same_capacity()
.merge_adjacent_fixed_same_capacity()
.strip_fixed_noops_same_capacity()
}
#[doc(hidden)]
#[doc(hidden)]
const fn strip_fixed_noops_same_capacity(self) -> Self {
let mut out_steps = [const { Step::Start }; N];
let mut out_len = 0usize;
let mut step_index = 0;
while step_index < self.len {
let step = self.steps[step_index];
if !is_fixed_noop(step) {
out_steps[out_len] = step;
out_len += 1;
}
step_index += 1;
}
Self {
steps: out_steps,
len: out_len,
params: self.params,
param_len: self.param_len,
mark_names: self.mark_names,
mark_len: self.mark_len,
}
}
const fn merge_adjacent_fixed_same_capacity(self) -> Self {
let mut out_steps = [const { Step::Start }; N];
let mut out_len = 0usize;
let mut i = 0;
while i < self.len {
let step = self.steps[i];
i += 1;
let merged = match step {
Step::Yaw(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.len {
if let Step::Yaw(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Yaw(StepArg::Fixed(total))
}
Step::Pitch(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.len {
if let Step::Pitch(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Pitch(StepArg::Fixed(total))
}
Step::Roll(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.len {
if let Step::Roll(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Roll(StepArg::Fixed(total))
}
Step::Forward(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.len {
if let Step::Forward(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Forward(StepArg::Fixed(total))
}
Step::Left(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.len {
if let Step::Left(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Left(StepArg::Fixed(total))
}
Step::Up(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.len {
if let Step::Up(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Up(StepArg::Fixed(total))
}
other => other,
};
out_steps[out_len] = merged;
out_len += 1;
}
Self {
steps: out_steps,
len: out_len,
params: self.params,
param_len: self.param_len,
mark_names: self.mark_names,
mark_len: self.mark_len,
}
}
pub const fn combine<
const DOF2: usize,
const MARKS2: usize,
const DOF_OUT: usize,
const MARKS_OUT: usize,
const N_OUT: usize,
>(
self,
other: LinkageView<'_, DOF2, MARKS2>,
) -> LinkageFixed<DOF_OUT, MARKS_OUT, N_OUT> {
self.view().combine_fixed(other)
}
}
#[cfg(feature = "alloc")]
#[derive(Clone)]
pub struct LinkageBuf<const DOF: usize, const MARKS: usize> {
params: [Param; DOF],
param_len: usize,
steps: alloc::vec::Vec<Step>,
mark_names: [&'static str; MARKS],
mark_len: usize,
}
#[cfg(feature = "alloc")]
impl<const DOF: usize, const MARKS: usize> LinkageBuf<DOF, MARKS> {
pub fn start() -> Self {
Self {
params: [Param::EMPTY; DOF],
param_len: 0,
steps: alloc::vec![Step::Start],
mark_names: [""; MARKS],
mark_len: 0,
}
}
pub fn from_lb_rs(source: &str) -> Result<Self, String> {
parse_lb_rs(source)
}
pub const DOF: usize = DOF;
pub const MARKS: usize = MARKS;
#[must_use]
#[inline]
pub fn view(&self) -> LinkageView<'_, DOF, MARKS> {
LinkageView::new(&self.params, &self.steps, &self.mark_names, self.mark_len)
}
pub fn define_param(mut self, name: &'static str, default: f32) -> Self {
assert!(self.param_len < DOF, "linkage has more params than DOF");
assert!(default >= 0.0, "parameter default must be at least 0.0");
assert!(default <= 1.0, "parameter default must be at most 1.0");
self.params[self.param_len] = Param { name, default };
self.param_len += 1;
self
}
emit_buf_step_methods!();
pub fn mark(mut self, name: &'static str) -> Self {
let index = match self.mark_index(name) {
Some(index) => index,
None => {
assert!(self.mark_len < MARKS, "linkage has more marks than MARKS");
let index = self.mark_len;
self.mark_names[index] = name;
self.mark_len += 1;
index
}
};
self.push_step_internal(Step::Mark { index });
self
}
pub fn restore(self, name: &'static str) -> Self {
let index = match self.mark_index(name) {
Some(i) => i,
None => {
panic!("restore: no mark found with name (mark must be defined before restore)")
}
};
self.push_step(Step::Restore { index })
}
fn push_step(mut self, step: Step) -> Self {
self.steps.push(step);
self
}
fn push_step_internal(&mut self, step: Step) {
self.steps.push(step);
}
fn mark_index(&self, name: &str) -> Option<usize> {
let mut mark_index = 0;
while mark_index < self.mark_len {
if str_eq(self.mark_names[mark_index], name) {
return Some(mark_index);
}
mark_index += 1;
}
None
}
fn last_param_index(&self, name: &str) -> Option<usize> {
let mut i = self.param_len;
while i > 0 {
i -= 1;
if str_eq(self.params[i].name, name) {
return Some(i);
}
}
None
}
fn expect_param_index(&self, name: &str) -> usize {
match self.last_param_index(name) {
Some(index) => index,
None => panic!("unknown parameter name"),
}
}
pub fn combine<
const DOF2: usize,
const MARKS2: usize,
const DOF_OUT: usize,
const MARKS_OUT: usize,
>(
self,
other: LinkageView<'_, DOF2, MARKS2>,
) -> LinkageBuf<DOF_OUT, MARKS_OUT> {
assert!(DOF_OUT == DOF + DOF2, "DOF_OUT must equal DOF + DOF2");
assert!(
MARKS_OUT >= self.mark_len + other.mark_len,
"MARKS_OUT must fit all marks from both linkages"
);
let mut out = LinkageBuf {
params: [Param::EMPTY; DOF_OUT],
param_len: 0,
steps: alloc::vec::Vec::new(),
mark_names: [""; MARKS_OUT],
mark_len: 0,
};
out.steps.extend_from_slice(&self.steps);
let mark_offset = self.mark_len;
for i in 1..other.steps.len() {
let step = other.steps[i].offset_params(DOF, mark_offset);
out.steps.push(step);
}
let mut i = 0;
while i < self.param_len {
out.params[i] = self.params[i];
i += 1;
}
let mut i = 0;
while i < DOF2 {
out.params[DOF + i] = other.params[i];
i += 1;
}
out.param_len = self.param_len + DOF2;
let mut i = 0;
while i < self.mark_len {
out.mark_names[i] = self.mark_names[i];
i += 1;
}
let mut i = 0;
while i < other.mark_len {
out.mark_names[self.mark_len + i] = other.mark_names[i];
i += 1;
}
out.mark_len = self.mark_len + other.mark_len;
out
}
}
#[cfg(feature = "alloc")]
impl<const DOF: usize, const MARKS: usize> LinkageBuf<DOF, MARKS> {
pub fn freeze_param_index<const OUT_DOF: usize>(
self,
param_index: usize,
raw_value: f32,
) -> LinkageBuf<OUT_DOF, MARKS> {
let mut is_frozen = [false; DOF];
let frozen_at_default = [false; DOF];
let mut frozen_raw = [0.0f32; DOF];
assert!(
param_index < self.param_len,
"freeze param index out of bounds"
);
is_frozen[param_index] = true;
frozen_raw[param_index] = raw_value;
self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
}
pub fn freeze_param_name<const OUT_DOF: usize>(
self,
name: &'static str,
raw_value: f32,
) -> LinkageBuf<OUT_DOF, MARKS> {
let mut found_index = 0usize;
let mut found_count = 0usize;
let mut param_index = 0;
while param_index < self.param_len {
if str_eq(self.params[param_index].name, name) {
found_index = param_index;
found_count += 1;
}
param_index += 1;
}
assert!(found_count > 0, "freeze name not found in params");
assert!(found_count == 1, "freeze name is ambiguous");
self.freeze_param_index(found_index, raw_value)
}
pub fn freeze_param_index_at_default<const OUT_DOF: usize>(
self,
param_index: usize,
) -> LinkageBuf<OUT_DOF, MARKS> {
let mut is_frozen = [false; DOF];
let mut frozen_at_default = [false; DOF];
let frozen_raw = [0.0f32; DOF];
assert!(
param_index < self.param_len,
"freeze param index out of bounds"
);
is_frozen[param_index] = true;
frozen_at_default[param_index] = true;
self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
}
pub fn retain_param_indexes<const OUT_DOF: usize>(
self,
indexes: &[usize],
) -> LinkageBuf<OUT_DOF, MARKS> {
let mut index_index = 0;
while index_index < indexes.len() {
let retain_index = indexes[index_index];
assert!(
retain_index < self.param_len,
"retain param index out of bounds"
);
let mut previous_index = 0;
while previous_index < index_index {
assert!(
indexes[previous_index] != retain_index,
"duplicate index in retain list"
);
previous_index += 1;
}
index_index += 1;
}
let mut is_frozen = [false; DOF];
let mut frozen_at_default = [false; DOF];
let frozen_raw = [0.0f32; DOF];
let mut param_index = 0;
while param_index < self.param_len {
let mut found = false;
let mut index_index = 0;
while index_index < indexes.len() {
if param_index == indexes[index_index] {
found = true;
break;
}
index_index += 1;
}
if !found {
is_frozen[param_index] = true;
frozen_at_default[param_index] = true;
}
param_index += 1;
}
self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
}
pub fn retain_param_names<const OUT_DOF: usize>(
self,
names: &[&'static str],
) -> LinkageBuf<OUT_DOF, MARKS> {
let mut ni = 0;
while ni < names.len() {
let retain_name = names[ni];
let mut ni2 = 0;
while ni2 < ni {
assert!(
!str_eq(names[ni2], retain_name),
"duplicate name in retain list"
);
ni2 += 1;
}
let mut found = false;
let mut pi = 0;
while pi < self.param_len {
if str_eq(self.params[pi].name, retain_name) {
found = true;
break;
}
pi += 1;
}
assert!(found, "retain name not found in params");
ni += 1;
}
let mut is_frozen = [false; DOF];
let mut frozen_at_default = [false; DOF];
let frozen_raw = [0.0f32; DOF];
let mut param_index = 0;
while param_index < self.param_len {
let mut found = false;
let mut name_index = 0;
while name_index < names.len() {
if str_eq(self.params[param_index].name, names[name_index]) {
found = true;
break;
}
name_index += 1;
}
if !found {
is_frozen[param_index] = true;
frozen_at_default[param_index] = true;
}
param_index += 1;
}
self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
}
fn freeze_with_map<const OUT_DOF: usize>(
self,
is_frozen: [bool; DOF],
frozen_at_default: [bool; DOF],
frozen_raw: [f32; DOF],
) -> LinkageBuf<OUT_DOF, MARKS> {
let mut new_param_index = [0usize; DOF];
let mut new_param_len = 0usize;
let mut param_index = 0;
while param_index < self.param_len {
if !is_frozen[param_index] {
new_param_index[param_index] = new_param_len;
new_param_len += 1;
}
param_index += 1;
}
assert!(
new_param_len == OUT_DOF,
"OUT_DOF must equal DOF minus the number of frozen parameters"
);
let mut out = LinkageBuf {
params: [Param::EMPTY; OUT_DOF],
param_len: 0,
steps: Vec::new(),
mark_names: [""; MARKS],
mark_len: self.mark_len,
};
let mut mark_index = 0;
while mark_index < self.mark_len {
out.mark_names[mark_index] = self.mark_names[mark_index];
mark_index += 1;
}
let mut param_index = 0;
while param_index < self.param_len {
if !is_frozen[param_index] {
out.params[new_param_index[param_index]] = self.params[param_index];
out.param_len += 1;
}
param_index += 1;
}
out.steps = self
.steps
.into_iter()
.map(|step| {
rewrite_step_for_freeze(
step,
&is_frozen,
&frozen_at_default,
&frozen_raw,
&self.params,
&new_param_index,
)
})
.collect();
out.strip_fixed_noops()
.merge_adjacent_fixed()
.strip_fixed_noops()
}
pub(crate) fn strip_fixed_noops(mut self) -> Self {
self.steps.retain(|&step| !is_fixed_noop(step));
self
}
pub(crate) fn merge_adjacent_fixed(self) -> Self {
let mut out = Vec::with_capacity(self.steps.len());
let mut i = 0;
while i < self.steps.len() {
let step = self.steps[i];
i += 1;
let merged = match step {
Step::Yaw(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.steps.len() {
if let Step::Yaw(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Yaw(StepArg::Fixed(total))
}
Step::Pitch(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.steps.len() {
if let Step::Pitch(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Pitch(StepArg::Fixed(total))
}
Step::Roll(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.steps.len() {
if let Step::Roll(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Roll(StepArg::Fixed(total))
}
Step::Forward(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.steps.len() {
if let Step::Forward(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Forward(StepArg::Fixed(total))
}
Step::Left(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.steps.len() {
if let Step::Left(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Left(StepArg::Fixed(total))
}
Step::Up(StepArg::Fixed(v)) => {
let mut total = v;
while i < self.steps.len() {
if let Step::Up(StepArg::Fixed(v2)) = self.steps[i] {
total += v2;
i += 1;
} else {
break;
}
}
Step::Up(StepArg::Fixed(total))
}
other => other,
};
out.push(merged);
}
Self {
steps: out,
params: self.params,
param_len: self.param_len,
mark_names: self.mark_names,
mark_len: self.mark_len,
}
}
}
#[cfg(feature = "alloc")]
impl<const DOF: usize, const MARKS: usize, const N: usize> From<&LinkageFixed<DOF, MARKS, N>>
for LinkageBuf<DOF, MARKS>
{
fn from(linkage: &LinkageFixed<DOF, MARKS, N>) -> Self {
Self {
params: linkage.params,
param_len: linkage.param_len,
steps: linkage.steps[..linkage.len].to_vec(),
mark_names: linkage.mark_names,
mark_len: linkage.mark_len,
}
}
}
#[cfg(feature = "alloc")]
impl<'a, const DOF: usize, const MARKS: usize> From<&'a LinkageBuf<DOF, MARKS>>
for LinkageView<'a, DOF, MARKS>
{
fn from(linkage: &'a LinkageBuf<DOF, MARKS>) -> Self {
linkage.view()
}
}
impl Step {
const fn offset_params(self, param_offset: usize, remember_offset: usize) -> Self {
match self {
Self::Yaw(arg) => Self::Yaw(arg.offset_param(param_offset)),
Self::Pitch(arg) => Self::Pitch(arg.offset_param(param_offset)),
Self::Roll(arg) => Self::Roll(arg.offset_param(param_offset)),
Self::Forward(arg) => Self::Forward(arg.offset_param(param_offset)),
Self::Left(arg) => Self::Left(arg.offset_param(param_offset)),
Self::Up(arg) => Self::Up(arg.offset_param(param_offset)),
Self::DiskParam(v) => Self::DiskParam(v.offset(param_offset)),
Self::SphereParam(v) => Self::SphereParam(v.offset(param_offset)),
Self::Mark { index } => Self::Mark {
index: index + remember_offset,
},
Self::Restore { index } => Self::Restore {
index: index + remember_offset,
},
other => other,
}
}
}
const fn str_eq(left: &str, right: &str) -> bool {
let left = left.as_bytes();
let right = right.as_bytes();
if left.len() != right.len() {
return false;
}
let mut byte_index = 0;
while byte_index < left.len() {
if left[byte_index] != right[byte_index] {
return false;
}
byte_index += 1;
}
true
}
const fn is_fixed_noop(step: Step) -> bool {
matches!(
step,
Step::Yaw(StepArg::Fixed(v))
| Step::Pitch(StepArg::Fixed(v))
| Step::Roll(StepArg::Fixed(v))
| Step::Forward(StepArg::Fixed(v))
| Step::Left(StepArg::Fixed(v))
| Step::Up(StepArg::Fixed(v))
if v == 0.0
)
}
const fn rewrite_arg_for_freeze(
arg: StepArg,
is_frozen: &[bool],
frozen_at_default: &[bool],
frozen_raw: &[f32],
params: &[Param],
new_param_index: &[usize],
is_rotation: bool,
) -> StepArg {
match arg {
StepArg::Fixed(_) => arg,
StepArg::Variable(variable_arg) => {
if is_frozen[variable_arg.index] {
let physical = frozen_physical_value(
variable_arg,
frozen_at_default,
frozen_raw,
params,
is_rotation,
);
StepArg::Fixed(physical)
} else {
StepArg::Variable(ParamArg {
index: new_param_index[variable_arg.index],
low: variable_arg.low,
span: variable_arg.span,
})
}
}
}
}
const fn rewrite_step_for_freeze(
step: Step,
is_frozen: &[bool],
frozen_at_default: &[bool],
frozen_raw: &[f32],
params: &[Param],
new_param_index: &[usize],
) -> Step {
match step {
Step::Yaw(arg) => Step::Yaw(rewrite_arg_for_freeze(
arg,
is_frozen,
frozen_at_default,
frozen_raw,
params,
new_param_index,
true,
)),
Step::Pitch(arg) => Step::Pitch(rewrite_arg_for_freeze(
arg,
is_frozen,
frozen_at_default,
frozen_raw,
params,
new_param_index,
true,
)),
Step::Roll(arg) => Step::Roll(rewrite_arg_for_freeze(
arg,
is_frozen,
frozen_at_default,
frozen_raw,
params,
new_param_index,
true,
)),
Step::Forward(arg) => Step::Forward(rewrite_arg_for_freeze(
arg,
is_frozen,
frozen_at_default,
frozen_raw,
params,
new_param_index,
false,
)),
Step::Left(arg) => Step::Left(rewrite_arg_for_freeze(
arg,
is_frozen,
frozen_at_default,
frozen_raw,
params,
new_param_index,
false,
)),
Step::Up(arg) => Step::Up(rewrite_arg_for_freeze(
arg,
is_frozen,
frozen_at_default,
frozen_raw,
params,
new_param_index,
false,
)),
Step::DiskParam(variable_arg) => {
if is_frozen[variable_arg.index] {
let physical = frozen_physical_value(
variable_arg,
frozen_at_default,
frozen_raw,
params,
false,
);
Step::Disk(physical)
} else {
Step::DiskParam(ParamArg {
index: new_param_index[variable_arg.index],
low: variable_arg.low,
span: variable_arg.span,
})
}
}
Step::SphereParam(variable_arg) => {
if is_frozen[variable_arg.index] {
let physical = frozen_physical_value(
variable_arg,
frozen_at_default,
frozen_raw,
params,
false,
);
Step::Sphere(physical)
} else {
Step::SphereParam(ParamArg {
index: new_param_index[variable_arg.index],
low: variable_arg.low,
span: variable_arg.span,
})
}
}
other => other,
}
}
const fn frozen_physical_value(
variable_arg: ParamArg,
frozen_at_default: &[bool],
frozen_raw: &[f32],
params: &[Param],
is_rotation: bool,
) -> f32 {
if frozen_at_default[variable_arg.index] {
variable_arg.low + params[variable_arg.index].default * variable_arg.span
} else {
let raw_value = frozen_raw[variable_arg.index];
let physical = if is_rotation {
degrees_to_radians(raw_value)
} else {
raw_value
};
assert_raw_value_in_range(variable_arg, physical);
physical
}
}
const fn assert_raw_value_in_range(variable_arg: ParamArg, physical: f32) {
let high = variable_arg.low + variable_arg.span;
let (min, max) = if variable_arg.low <= high {
(variable_arg.low, high)
} else {
(high, variable_arg.low)
};
assert!(
physical >= min && physical <= max,
"raw freeze value out of range"
);
}
fn validate_params<const DOF: usize>(params: &[f32; DOF]) -> Result<(), Error> {
for (index, &value) in params.iter().enumerate() {
if !(0.0..=1.0).contains(&value) {
return Err(Error::InvalidParameter { index, value });
}
}
Ok(())
}
fn rotation_matrix<const DOF: usize>(step: &Step, params: &[f32; DOF]) -> Mat3 {
let radians = match step {
Step::Yaw(arg) | Step::Pitch(arg) | Step::Roll(arg) => arg.resolve(params),
Step::Start
| Step::Forward(_)
| Step::Left(_)
| Step::Up(_)
| Step::PenUp
| Step::PenDown
| Step::PenColor(_)
| Step::PenWidth(_)
| Step::Disk(_)
| Step::DiskParam(_)
| Step::Sphere(_)
| Step::SphereParam(_)
| Step::Mark { .. }
| Step::Restore { .. } => return Mat3::IDENTITY,
};
match step {
Step::Yaw(_) => Mat3::yaw(radians),
Step::Pitch(_) => Mat3::pitch(radians),
Step::Roll(_) => Mat3::roll(radians),
Step::Start
| Step::Forward(_)
| Step::Left(_)
| Step::Up(_)
| Step::PenUp
| Step::PenDown
| Step::PenColor(_)
| Step::PenWidth(_)
| Step::Disk(_)
| Step::DiskParam(_)
| Step::Sphere(_)
| Step::SphereParam(_)
| Step::Mark { .. }
| Step::Restore { .. } => Mat3::IDENTITY,
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum PenState {
Up,
Down,
}
#[derive(Clone, Copy, Debug)]
struct PenStyle {
pen: PenState,
color: Rgb888,
width: f32,
}
impl PenStyle {
#[must_use]
pub const fn new() -> Self {
Self {
pen: PenState::Down,
color: Rgb888::new(255, 255, 255),
width: 0.1,
}
}
fn reset(&mut self) {
*self = Self::new();
}
#[must_use]
pub const fn pen(self) -> PenState {
self.pen
}
#[must_use]
pub const fn color(self) -> Rgb888 {
self.color
}
#[must_use]
pub const fn width(self) -> f32 {
self.width
}
fn apply(&mut self, step: &Step) {
match step {
Step::Start => self.reset(),
Step::PenUp => self.pen = PenState::Up,
Step::PenDown => self.pen = PenState::Down,
Step::PenColor(color) => self.color = *color,
Step::PenWidth(width) => self.width = *width,
Step::Yaw(_)
| Step::Pitch(_)
| Step::Roll(_)
| Step::Forward(_)
| Step::Left(_)
| Step::Up(_)
| Step::Disk(_)
| Step::DiskParam(_)
| Step::Sphere(_)
| Step::SphereParam(_)
| Step::Mark { .. }
| Step::Restore { .. } => {}
}
}
}
impl Default for PenStyle {
fn default() -> Self {
Self::new()
}
}
#[derive(Clone, Copy, Debug)]
pub struct Pose {
orientation: Mat3,
position: Vec3,
}
impl Pose {
#[must_use]
pub const fn new(orientation: Mat3, position: Vec3) -> Self {
Self {
orientation,
position,
}
}
#[must_use]
pub fn project(self, projection: &Projection) -> (f32, f32) {
let c = projection.world_to_camera(self.position());
let k = projection.scale * projection.depth_factor(c[0]);
(
projection.target_origin.x as f32 - c[1] * k,
projection.target_origin.y as f32 - c[2] * k,
)
}
#[must_use]
pub const fn start() -> Self {
Self {
orientation: Mat3::IDENTITY,
position: Vec3::ZERO,
}
}
#[must_use]
pub const fn orientation(self) -> Mat3 {
self.orientation
}
#[must_use]
pub const fn position(self) -> Vec3 {
self.position
}
#[must_use]
pub fn is_close_to(&self, other: &Self, tolerance: f32) -> bool {
self.orientation.is_close_to(&other.orientation, tolerance)
&& self.position.is_close_to(&other.position, tolerance)
}
fn apply<const DOF: usize>(&mut self, step: &Step, params: &[f32; DOF]) {
match step {
Step::Start => {
*self = Self::start();
}
Step::Forward(arg) => {
self.position += self.orientation.forward() * arg.resolve(params);
}
Step::Left(arg) => {
self.position += self.orientation.left() * arg.resolve(params);
}
Step::Up(arg) => {
self.position += self.orientation.up() * arg.resolve(params);
}
Step::Yaw(_) | Step::Pitch(_) | Step::Roll(_) => {
self.orientation = self.orientation * rotation_matrix(step, params);
}
Step::PenUp
| Step::PenDown
| Step::PenColor(_)
| Step::PenWidth(_)
| Step::Disk(_)
| Step::DiskParam(_)
| Step::Sphere(_)
| Step::SphereParam(_)
| Step::Mark { .. }
| Step::Restore { .. } => {}
}
}
}
#[derive(Clone, Copy, Debug)]
pub struct StyledPose {
pose: Pose,
pen_style: PenStyle,
}
impl StyledPose {
#[must_use]
pub const fn pose(self) -> Pose {
self.pose
}
#[must_use]
pub const fn pen(self) -> PenState {
self.pen_style.pen()
}
#[must_use]
pub const fn color(self) -> Rgb888 {
self.pen_style.color()
}
#[must_use]
pub const fn width(self) -> f32 {
self.pen_style.width()
}
}
#[derive(Clone, Copy)]
struct MarkedState {
pose: Pose,
pen_style: PenStyle,
}
struct StyledPosesView<'a, const DOF: usize, const MARKS: usize> {
steps: &'a [Step],
params: &'a [f32; DOF],
index: usize,
pose: Pose,
pen_style: PenStyle,
marked: [MarkedState; MARKS],
}
impl<'a, const DOF: usize, const MARKS: usize> StyledPosesView<'a, DOF, MARKS> {
fn new(steps: &'a [Step], params_values: &'a [f32; DOF]) -> Result<Self, Error> {
validate_params(params_values)?;
Ok(Self {
steps,
params: params_values,
index: 0,
pose: Pose::start(),
pen_style: PenStyle::new(),
marked: [MarkedState {
pose: Pose::start(),
pen_style: PenStyle::new(),
}; MARKS],
})
}
}
impl<const DOF: usize, const MARKS: usize> Iterator for StyledPosesView<'_, DOF, MARKS> {
type Item = StyledPose;
fn next(&mut self) -> Option<Self::Item> {
loop {
if self.index >= self.steps.len() {
return None;
}
let step = &self.steps[self.index];
self.index += 1;
match step {
Step::Mark { index } => {
self.marked[*index] = MarkedState {
pose: self.pose,
pen_style: self.pen_style,
};
continue;
}
Step::Restore { index } => {
let marked_state = self.marked[*index];
self.pose = marked_state.pose;
self.pen_style = marked_state.pen_style;
continue;
}
_ => {}
}
self.pose.apply(step, self.params);
self.pen_style.apply(step);
return Some(StyledPose {
pose: self.pose,
pen_style: self.pen_style,
});
}
}
}
pub struct DrawItem3dIter<'a, const DOF: usize, const MARKS: usize> {
steps: &'a [Step],
mark_names: &'a [&'static str; MARKS],
mark_len: usize,
params: &'a [f32; DOF],
index: usize,
pose: Pose,
pen_style: PenStyle,
marked: [MarkedState; MARKS],
}
impl<'a, const DOF: usize, const MARKS: usize> DrawItem3dIter<'a, DOF, MARKS> {
fn new(
steps: &'a [Step],
mark_names: &'a [&'static str; MARKS],
mark_len: usize,
params_values: &'a [f32; DOF],
) -> Result<Self, Error> {
validate_params(params_values)?;
Ok(Self {
steps,
mark_names,
mark_len,
params: params_values,
index: 0,
pose: Pose::start(),
pen_style: PenStyle::new(),
marked: [MarkedState {
pose: Pose::start(),
pen_style: PenStyle::new(),
}; MARKS],
})
}
pub fn pose_by_mark_name(&self, name: &str) -> Result<Pose, Error> {
let mut found = None;
for (index, &n) in self.mark_names[..].iter().enumerate().take(self.mark_len) {
if n == name {
if found.is_some() {
return Err(Error::MarkAmbiguous);
}
found = Some(index);
}
}
found
.map(|index| self.marked[index].pose)
.ok_or(Error::MarkNotFound)
}
#[must_use]
pub fn final_pose(&self) -> Pose {
self.pose
}
}
impl<const DOF: usize, const MARKS: usize> Iterator for DrawItem3dIter<'_, DOF, MARKS> {
type Item = Item3d;
fn next(&mut self) -> Option<Self::Item> {
while self.index < self.steps.len() {
let step = &self.steps[self.index];
self.index += 1;
match step {
Step::Mark { index } => {
self.marked[*index] = MarkedState {
pose: self.pose,
pen_style: self.pen_style,
};
continue;
}
Step::Restore { index } => {
let marked_state = self.marked[*index];
self.pose = marked_state.pose;
self.pen_style = marked_state.pen_style;
continue;
}
_ => {}
}
let start_pose = self.pose;
let pen_style = self.pen_style;
self.pose.apply(step, self.params);
self.pen_style.apply(step);
match step {
Step::Forward(_) | Step::Left(_) | Step::Up(_)
if matches!(pen_style.pen(), PenState::Down) =>
{
return Some(Item3d::Stroke(Stroke {
start: start_pose,
end: self.pose,
color: pen_style.color(),
width: pen_style.width(),
}));
}
Step::Disk(radius) => {
return Some(Item3d::Disk(Disk {
pose: start_pose,
radius: *radius,
color: pen_style.color(),
}));
}
Step::DiskParam(var_arg) => {
return Some(Item3d::Disk(Disk {
pose: start_pose,
radius: var_arg.resolve(self.params),
color: pen_style.color(),
}));
}
Step::Sphere(radius) => {
return Some(Item3d::Sphere(Sphere {
pose: start_pose,
radius: *radius,
color: pen_style.color(),
}));
}
Step::SphereParam(var_arg) => {
return Some(Item3d::Sphere(Sphere {
pose: start_pose,
radius: var_arg.resolve(self.params),
color: pen_style.color(),
}));
}
_ => continue,
}
}
None
}
}
#[macro_export]
macro_rules! linkage {
($($chain:tt)*) => {
(__linkage_blaze_start!()) $($chain)*
};
}
#[cfg(feature = "alloc")]
#[doc(hidden)]
#[macro_export]
macro_rules! __linkage_file_buf_items {
($path:literal) => {
pub type Buf = $crate::LinkageBuf<DOF, MARKS>;
pub fn buf() -> Buf {
macro_rules! __linkage_blaze_start {
() => {
$crate::LinkageBuf::<DOF, MARKS>::start()
};
}
include!($path)
}
const _: fn() -> Buf = buf;
};
}
#[cfg(not(feature = "alloc"))]
#[doc(hidden)]
#[macro_export]
macro_rules! __linkage_file_buf_items {
($path:literal) => {};
}
#[macro_export]
macro_rules! linkage_file {
($(#[$attribute:meta])* $visibility:vis $name:ident {
file: $path:literal $(,)?
}) => {
$(#[$attribute])*
$visibility mod $name {
use $crate::{linkage, Rgb888, WebColors};
const _: Rgb888 = Rgb888::CSS_BLACK;
const __CANDIDATE: $crate::LinkageFixed<
{ __METADATA.param_count() },
{ __METADATA.mark_count() },
{ __METADATA.step_count() },
> = {
macro_rules! __linkage_blaze_start {
() => {
$crate::LinkageFixed::<
{ __METADATA.param_count() },
{ __METADATA.mark_count() },
{ __METADATA.step_count() },
>::start()
};
}
include!($path)
};
pub const DOF: usize = __CANDIDATE.param_count();
pub const MARKS: usize = __CANDIDATE.mark_count();
pub const STEP_COUNT: usize = __METADATA.step_count();
pub type Fixed = $crate::LinkageFixed<DOF, MARKS, STEP_COUNT>;
pub type View = $crate::LinkageView<'static, DOF, MARKS>;
const __METADATA: $crate::LinkageStepCount = {
macro_rules! __linkage_blaze_start {
() => { $crate::LinkageStepCount::start() };
}
include!($path)
};
const __FIXED: Fixed = {
macro_rules! __linkage_blaze_start {
() => {
$crate::LinkageFixed::<DOF, MARKS, STEP_COUNT>::start()
};
}
let linkage = include!($path);
assert!(
linkage.param_count() == DOF,
"DOF must equal the number of defined parameters"
);
assert!(
linkage.mark_count() == MARKS,
"MARKS must equal the number of defined marks"
);
linkage
};
pub const fn fixed() -> Fixed {
__FIXED
}
const __VIEW: View = fixed().view();
pub const fn view() -> View {
__VIEW
}
const _: View = view();
$crate::__linkage_file_buf_items!($path);
}
};
($($tokens:tt)*) => {
compile_error!("linkage_file! accepts exactly one file declaration per invocation");
};
}
#[cfg(test)]
mod test_helpers;
#[cfg(test)]
mod tests {
#[cfg(feature = "alloc")]
use super::LinkageBuf;
use super::{Error, LinkageFixed, LinkageView, Mat3, Pose, Rgb888, Step, StepArg, Vec3};
use crate::render::{Item3d, Projection};
use crate::test_helpers::{
assert_png_matches_expected, assert_pose_approx_eq, assert_pose_trace_matches_expected,
draw_linkage_xy_canvas,
};
use device_envoy_core::pixel_target::{
rgb565_from_rgb888, rgb565_from_rgb888_components, rgb888_from_rgb565,
};
use embedded_graphics::{pixelcolor::Rgb565, prelude::Point};
use std::{boxed::Box, error::Error as StdError};
const LINKAGE0: LinkageFixed<6, 0, 24> = LinkageFixed::start()
.define_param("raise hand", 0.5)
.define_param("bend elbow", 0.5)
.define_param("close hand", 0.5)
.define_param("lower arm", 0.5)
.define_param("spin whole arm", 0.5)
.define_param("spin hand", 0.5)
.yaw(90.0)
.yaw_param("spin whole arm", 180.0, -180.0)
.pitch(90.0)
.forward(2.5)
.pitch(-90.0)
.pitch_param("lower arm", 30.0, 0.0)
.forward(3.0)
.yaw_param("bend elbow", 90.0, -90.0)
.forward(3.0)
.pitch_param("raise hand", 90.0, -90.0)
.forward(1.0)
.roll_param("spin hand", -180.0, 180.0)
.forward(0.5)
.yaw(90.0)
.forward_param("close hand", 0.0, 0.5)
.yaw(-90.0)
.forward(1.0)
.yaw(180.0)
.forward(1.0)
.yaw(90.0)
.forward_param("close hand", 0.0, 1.0)
.yaw(90.0)
.forward(1.0);
const LINKAGE1: LinkageFixed<3, 0, 16> = LinkageFixed::start()
.define_param("spin whole arm", 0.5)
.define_param("bend elbow", 0.5)
.define_param("close hand", 0.5)
.yaw(90.0)
.yaw_param("spin whole arm", 180.0, -180.0)
.forward(3.0)
.yaw_param("bend elbow", 90.0, -90.0)
.forward(3.0)
.yaw(90.0)
.forward_param("close hand", 0.5, 0.0)
.yaw(-90.0)
.forward(1.0)
.yaw(-180.0)
.forward(1.0)
.yaw(90.0)
.forward_param("close hand", 1.0, 0.0)
.yaw(90.0)
.forward(1.0);
fn assert_float_close(actual: f32, expected: f32, tolerance: f32) {
assert!(
(actual - expected).abs() <= tolerance,
"actual {actual} was not within {tolerance} of expected {expected}",
);
}
#[test]
fn rgb888_from_rgb565_is_const() {
const BLACK: Rgb888 = rgb888_from_rgb565(0x0000); const WHITE: Rgb888 = rgb888_from_rgb565(0xffff); const RED: Rgb888 = rgb888_from_rgb565(0xf800); const GREEN: Rgb888 = rgb888_from_rgb565(0x07e0); const BLUE: Rgb888 = rgb888_from_rgb565(0x001f);
assert_eq!(BLACK, Rgb888::new(0, 0, 0));
assert_eq!(WHITE, Rgb888::new(255, 255, 255));
assert_eq!(RED, Rgb888::new(255, 0, 0));
assert_eq!(GREEN, Rgb888::new(0, 255, 0));
assert_eq!(BLUE, Rgb888::new(0, 0, 255));
}
#[test]
fn rgb565_from_rgb888_components_is_const() {
const BLACK: Rgb565 = rgb565_from_rgb888_components(0, 0, 0); const WHITE: Rgb565 = rgb565_from_rgb888_components(255, 255, 255); const RED: Rgb565 = rgb565_from_rgb888_components(255, 0, 0); const GREEN: Rgb565 = rgb565_from_rgb888_components(0, 255, 0); const BLUE: Rgb565 = rgb565_from_rgb888_components(0, 0, 255);
assert_eq!(BLACK, rgb565_from_rgb888(Rgb888::new(0, 0, 0)));
assert_eq!(WHITE, rgb565_from_rgb888(Rgb888::new(255, 255, 255)));
assert_eq!(RED, rgb565_from_rgb888(Rgb888::new(255, 0, 0)));
assert_eq!(GREEN, rgb565_from_rgb888(Rgb888::new(0, 255, 0)));
assert_eq!(BLUE, rgb565_from_rgb888(Rgb888::new(0, 0, 255)));
}
#[test]
fn top_orthographic_projects_xy_plane() {
let projection = Projection::top_orthographic(Point::new(100, 100), 2.0);
let forward_pose = Pose::new(Mat3::IDENTITY, Vec3::from([3.0, 0.0, 0.0]));
let left_pose = Pose::new(Mat3::IDENTITY, Vec3::from([0.0, 4.0, 0.0]));
let forward_projected = forward_pose.project(&projection);
let left_projected = left_pose.project(&projection);
assert_float_close(forward_projected.0, 100.0, 1e-5);
assert_float_close(forward_projected.1, 94.0, 1e-5);
assert_float_close(left_projected.0, 92.0, 1e-5);
assert_float_close(left_projected.1, 100.0, 1e-5);
let forward_axis = projection.project_dir(Pose::start(), Vec3::from([1.0, 0.0, 0.0]), 5.0);
let left_axis = projection.project_dir(Pose::start(), Vec3::from([0.0, 1.0, 0.0]), 5.0);
assert_float_close(forward_axis.0, 0.0, 1e-5);
assert_float_close(forward_axis.1, -10.0, 1e-5);
assert_float_close(left_axis.0, -10.0, 1e-5);
assert_float_close(left_axis.1, 0.0, 1e-5);
}
#[test]
fn zero_pen_width_still_draws() -> Result<(), Box<dyn StdError>> {
const LINKAGE: LinkageFixed<0, 0, 4> = LinkageFixed::start().pen_width(0.0).forward(1.0);
let params = [];
let draw_item_3d = LINKAGE
.view()
.draw_items_3d(¶ms)?
.next()
.expect("zero-width pen should still produce a stroke");
match draw_item_3d {
Item3d::Stroke(stroke_segment) => {
assert_eq!(stroke_segment.width(), 0.0);
}
_ => panic!("expected stroke from zero-width pen"),
}
Ok(())
}
#[cfg(feature = "alloc")]
#[test]
fn serializes_linkage_view_to_lb_rs() {
const LINKAGE: LinkageFixed<1, 1, 10> = LinkageFixed::start()
.define_param("distance", 0.5)
.pen_up()
.mark("origin")
.pen_color(Rgb888::new(10, 20, 30))
.pen_width(2.0)
.pen_down()
.forward_param("distance", 1.0, 5.0)
.restore("origin");
let source = LINKAGE.view().to_lb_rs();
assert!(source.starts_with("// DOF="));
assert!(source.contains("linkage![\n"));
assert!(source.trim_end().ends_with(']'));
assert!(source.contains(".define_param(\"distance\", 0.5)"));
assert!(source.contains(".pen_color(Rgb888::new(10, 20, 30))"));
assert!(source.contains(".forward_param(\"distance\", 1.0, 5.0)"));
assert!(source.contains(".restore(\"origin\")"));
}
#[cfg(feature = "alloc")]
#[test]
fn parses_lb_rs_into_linkage_buf() -> Result<(), Box<dyn StdError>> {
let source = r#"linkage![
.define_param("distance", 0.5)
.pen_color(Rgb888::new(10, 20, 30))
.forward_param("distance", 1.0, 5.0)
]"#;
let linkage = LinkageBuf::<1, 0>::from_lb_rs(source).expect("source should parse");
let pose = linkage.view().final_pose(&[0.5])?;
assert!(
pose.position()
.is_close_to(&Vec3::from([3.0, 0.0, 0.0]), 1e-5)
);
Ok(())
}
#[cfg(feature = "alloc")]
#[test]
fn lb_rs_parser_rejects_integer_arguments() {
let error = match LinkageBuf::<0, 0>::from_lb_rs("linkage![\n.forward(1)\n]") {
Ok(_) => panic!("integer argument should fail"),
Err(error) => error,
};
assert!(error.contains("is an integer"));
}
#[test]
fn forward_moves_along_positive_x() -> Result<(), Box<dyn StdError>> {
const LINKAGE: LinkageFixed<0, 0, 2> = LinkageFixed::start().forward(10.0);
let params = [];
let actual = LINKAGE.view().final_pose(¶ms)?.position();
assert!(actual.is_close_to(&Vec3::from([10.0, 0.0, 0.0]), 1e-6));
Ok(())
}
#[test]
fn link_view_finds_named_mark_pose() -> Result<(), Box<dyn StdError>> {
const LINKAGE: LinkageFixed<0, 1, 4> = LinkageFixed::start().mark("tip").forward(2.0);
let view = LINKAGE.view();
let mut items = view.draw_items_3d(&[])?;
while items.next().is_some() {}
let pose = items.pose_by_mark_name("tip")?;
assert_eq!(pose.position(), Vec3::from([0.0, 0.0, 0.0]));
Ok(())
}
#[test]
fn link_view_reports_missing_mark() {
const LINKAGE: LinkageFixed<0, 1, 2> = LinkageFixed::start().mark("tip");
let view = LINKAGE.view();
let mut items = view.draw_items_3d(&[]).expect("valid parameters");
while items.next().is_some() {}
assert!(matches!(
items.pose_by_mark_name("missing"),
Err(Error::MarkNotFound)
));
}
#[cfg(feature = "alloc")]
#[test]
fn link_view_reports_ambiguous_mark() {
let first = LinkageBuf::<0, 1>::start().mark("tip");
let second = LinkageBuf::<0, 1>::start().mark("tip");
let linkage: LinkageBuf<0, 2> = first.combine(second.view());
let view = linkage.view();
let mut items = view.draw_items_3d(&[]).expect("valid parameters");
while items.next().is_some() {}
assert!(matches!(
items.pose_by_mark_name("tip"),
Err(Error::MarkAmbiguous)
));
}
#[test]
fn yaw_then_forward_moves_along_positive_y() -> Result<(), Box<dyn StdError>> {
const LINKAGE: LinkageFixed<0, 0, 3> = LinkageFixed::start().yaw(90.0).forward(10.0);
let params = [];
let actual = LINKAGE.view().final_pose(¶ms)?.position();
assert!(actual.is_close_to(&Vec3::from([0.0, 10.0, 0.0]), 1e-5));
Ok(())
}
#[test]
fn left_moves_along_positive_y() -> Result<(), Box<dyn StdError>> {
const LINKAGE: LinkageFixed<0, 0, 2> = LinkageFixed::start().left(10.0);
let params = [];
let actual = LINKAGE.view().final_pose(¶ms)?.position();
assert!(actual.is_close_to(&Vec3::from([0.0, 10.0, 0.0]), 1e-6));
Ok(())
}
#[test]
fn up_moves_along_positive_z() -> Result<(), Box<dyn StdError>> {
const LINKAGE: LinkageFixed<0, 0, 2> = LinkageFixed::start().up(10.0);
let params = [];
let actual = LINKAGE.view().final_pose(¶ms)?.position();
assert!(actual.is_close_to(&Vec3::from([0.0, 0.0, 10.0]), 1e-6));
Ok(())
}
#[test]
fn translation_params_move_along_named_axes() -> Result<(), Box<dyn StdError>> {
const LINKAGE: LinkageFixed<3, 0, 7> = LinkageFixed::start()
.define_param("forward", 0.5)
.define_param("left", 0.5)
.define_param("up", 0.5)
.forward_param("forward", 0.0, 10.0)
.left_param("left", 0.0, 20.0)
.up_param("up", 0.0, 30.0);
let params = [0.2, 0.3, 0.4];
let actual = LINKAGE.view().final_pose(¶ms)?.position();
assert!(actual.is_close_to(&Vec3::from([2.0, 6.0, 12.0]), 1e-6));
Ok(())
}
#[test]
fn planar_two_link_arm_uses_yaw_then_forward() -> Result<(), Box<dyn StdError>> {
const LINKAGE: LinkageFixed<0, 0, 5> = LinkageFixed::start()
.yaw(0.0)
.forward(10.0)
.yaw(90.0)
.forward(5.0);
let params = [];
let actual = LINKAGE.view().final_pose(¶ms)?.position();
assert!(actual.is_close_to(&Vec3::from([10.0, 5.0, 0.0]), 1e-5));
Ok(())
}
#[test]
fn test_excel_pose_trace0_matches_expected() -> Result<(), Box<dyn StdError>> {
let params = [0.751_450_1, 0.500_200_4, 0.5, 1.0, 0.625_438_7, 0.0];
assert_pose_trace_matches_expected("excel_pose_trace0.csv", LINKAGE0.view().poses(¶ms)?)
}
#[test]
fn test_excel_pose_trace1_matches_expected() -> Result<(), Box<dyn StdError>> {
let params = [0.30, 0.02, 0.10];
assert_pose_trace_matches_expected("excel_pose_trace1.csv", LINKAGE1.view().poses(¶ms)?)
}
#[test]
fn test_setting0_matches_excel_final_pose() -> Result<(), Box<dyn StdError>> {
let params = [
0.751_450_1, 0.49, 0.50011957, 1.0, 0.625_438_7, 1.0, ];
let pose = LINKAGE0.view().final_pose(¶ms)?;
let expected = Pose::new(
[
[0.48325038, 0.7270788, 0.48767346],
[0.5117748, -0.68655396, 0.51645917],
[0.7103207, 0.0, -0.70387816],
]
.into(),
[5.213134, 5.747819, -0.7241982].into(),
);
assert_pose_approx_eq(pose, expected);
Ok(())
}
#[test]
fn test_setting1_matches_excel_final_pose() -> Result<(), Box<dyn StdError>> {
let params = [
0.30, 0.02, 0.10, ];
let pose = LINKAGE1.view().final_pose(¶ms)?;
let expected = Pose::new(
[
[-0.368_124_5, 0.929_776_43, 0.0],
[-0.929_776_43, -0.368_124_5, 0.0],
[0.0, 0.0, 1.0],
]
.into(),
[-4.744_067, -2.626_399, 0.0].into(),
);
assert_pose_approx_eq(pose, expected);
Ok(())
}
#[test]
fn test_mid_setting0_matches_excel_final_pose_and_png() -> Result<(), Box<dyn StdError>> {
let params = [
0.5, 0.3, 1.0, 0.5, 0.5, 0.5, ];
let pose = LINKAGE0.view().final_pose(¶ms)?;
let expected = Pose::new(
[
[-0.5877855, -0.80901694, 0.0],
[0.78145033, -0.5677572, 0.25881904],
[-0.20938899, 0.15213005, 0.9659258],
]
.into(),
[-2.828311, 7.4796333, -4.504162].into(),
);
assert_pose_approx_eq(pose, expected);
let canvas = draw_linkage_xy_canvas(&LINKAGE0, ¶ms)?;
assert_png_matches_expected("linkage0_xy_mid_fraction.png", &canvas)
}
#[test]
fn test_linkage0_png_matches_expected() -> Result<(), Box<dyn StdError>> {
let params = [0.751_450_1, 0.500_200_4, 0.5, 1.0, 0.625_438_7, 0.0];
let canvas = draw_linkage_xy_canvas(&LINKAGE0, ¶ms)?;
assert_png_matches_expected("linkage0_xy.png", &canvas)
}
#[test]
fn test_linkage1_png_matches_expected() -> Result<(), Box<dyn StdError>> {
let params = [0.30, 0.02, 0.10];
let canvas = draw_linkage_xy_canvas(&LINKAGE1, ¶ms)?;
assert_png_matches_expected("linkage1_xy.png", &canvas)
}
#[test]
fn test_params_are_range_checked() {
let params = [
0.0, 0.5, 1.1, 1.0, 0.0, 0.5, ];
assert!(matches!(
LINKAGE0.view().final_pose(¶ms),
Err(Error::InvalidParameter { index: 2, .. })
));
}
#[test]
fn duplicate_define_param_does_not_panic() {
const _: LinkageFixed<2, 0, 2> = LinkageFixed::start()
.define_param("angle", 0.25) .define_param("angle", 0.75); }
#[test]
fn shadowing_builder_binds_to_most_recent_definition() -> Result<(), Box<dyn StdError>> {
const LINKAGE: LinkageFixed<2, 0, 5> = LinkageFixed::start()
.define_param("angle", 0.0) .define_param("angle", 1.0) .yaw_param("angle", 0.0, 90.0) .forward(10.0);
let params = [1.0, 0.0]; let pos = LINKAGE.view().final_pose(¶ms)?.position();
assert!(pos.is_close_to(&Vec3::from([10.0, 0.0, 0.0]), 1e-5));
Ok(())
}
#[test]
fn freeze_param_index_uses_raw_rotation_degrees() -> Result<(), Box<dyn StdError>> {
const BASE: LinkageFixed<2, 0, 5> = LinkageFixed::start()
.define_param("angle", 0.5)
.define_param("len", 0.5)
.yaw_param("angle", -180.0, 180.0)
.forward_param("len", 0.0, 10.0);
const FROZEN: LinkageFixed<1, 0, 5> = BASE.freeze_param_index(0, 90.0);
assert_specialized_matches_original(BASE, &[0.75, 1.0], FROZEN, &[1.0])?;
let pos = FROZEN.view().final_pose(&[1.0])?.position();
assert!(pos.is_close_to(&Vec3::from([0.0, 10.0, 0.0]), 1e-4));
Ok(())
}
#[test]
fn freeze_param_name_matches_unique_slot() -> Result<(), Box<dyn StdError>> {
const BASE: LinkageFixed<2, 0, 5> = LinkageFixed::start()
.define_param("yaw", 0.25)
.define_param("dist", 0.5)
.yaw_param("yaw", 0.0, 180.0)
.forward_param("dist", 0.0, 8.0);
const FROZEN_BY_NAME: LinkageFixed<1, 0, 5> = BASE.freeze_param_name("yaw", 45.0);
const FROZEN_BY_DEFAULT: LinkageFixed<1, 0, 5> = BASE.freeze_param_index_at_default(0);
let pos_by_name = FROZEN_BY_NAME.view().final_pose(&[0.5])?.position();
let pos_by_default = FROZEN_BY_DEFAULT.view().final_pose(&[0.5])?.position();
assert!(pos_by_name.is_close_to(&pos_by_default, 1e-6));
Ok(())
}
#[test]
fn retain_param_names_freezes_unlisted_at_default() -> Result<(), Box<dyn StdError>> {
const BASE: LinkageFixed<2, 0, 5> = LinkageFixed::start()
.define_param("angle", 0.5)
.define_param("dist", 0.5)
.yaw_param("angle", -90.0, 90.0)
.forward_param("dist", 0.0, 4.0);
const RETAINED: LinkageFixed<1, 0, 5> = BASE.retain_param_names(&["dist"]);
assert_specialized_matches_original(BASE, &[0.5, 1.0], RETAINED, &[1.0])?;
let pos = RETAINED.view().final_pose(&[1.0])?.position();
assert!(pos.is_close_to(&Vec3::from([4.0, 0.0, 0.0]), 1e-4));
Ok(())
}
#[test]
fn freeze_param_index_freezes_only_that_shadowed_slot() -> Result<(), Box<dyn StdError>> {
const BASE: LinkageFixed<3, 0, 5> = LinkageFixed::start()
.define_param("x", 0.1)
.forward_param("x", 0.0, 10.0)
.define_param("y", 0.2)
.left_param("y", 0.0, 10.0)
.define_param("x", 0.3)
.up_param("x", 0.0, 10.0);
const FIRST_X: LinkageFixed<2, 0, 5> = BASE.freeze_param_index(0, 4.0);
const SECOND_X: LinkageFixed<2, 0, 5> = BASE.freeze_param_index(2, 6.0);
assert_specialized_matches_original(BASE, &[0.4, 0.8, 0.6], FIRST_X, &[0.8, 0.6])?;
assert_specialized_matches_original(BASE, &[0.4, 0.8, 0.6], SECOND_X, &[0.4, 0.8])?;
Ok(())
}
#[test]
fn retain_param_names_retains_every_shadowed_slot_in_original_order()
-> Result<(), Box<dyn StdError>> {
const BASE: LinkageFixed<3, 0, 5> = LinkageFixed::start()
.define_param("x", 0.25)
.forward_param("x", 0.0, 10.0)
.define_param("y", 0.5)
.left_param("y", 0.0, 10.0)
.define_param("x", 0.75)
.up_param("x", 0.0, 10.0);
const RETAINED: LinkageFixed<2, 0, 5> = BASE.retain_param_names(&["x"]);
let params = RETAINED.view().params();
assert_eq!(params[0].name(), "x");
assert_eq!(params[0].default(), 0.25);
assert_eq!(params[1].name(), "x");
assert_eq!(params[1].default(), 0.75);
assert_specialized_matches_original(BASE, &[1.0, 0.5, 0.0], RETAINED, &[1.0, 0.0])?;
Ok(())
}
#[test]
fn retain_param_indexes_keeps_exact_shadowed_slots() -> Result<(), Box<dyn StdError>> {
const RETAINED: LinkageFixed<2, 0, 8> = BRUCE.retain_param_indexes(&[0, 2]);
let params = RETAINED.view().params();
assert_eq!(params[0].name(), "Bruce");
assert_eq!(params[0].default(), 0.10);
assert_eq!(params[1].name(), "Bruce");
assert_eq!(params[1].default(), 0.30);
assert_specialized_matches_original(
BRUCE,
&[0.11, 0.20, 0.33, 0.40],
RETAINED,
&[0.11, 0.33],
)?;
Ok(())
}
#[test]
fn retain_param_names_freezes_non_retained_shadowed_slots_at_their_own_defaults()
-> Result<(), Box<dyn StdError>> {
const BASE: LinkageFixed<3, 0, 5> = LinkageFixed::start()
.define_param("x", 0.25)
.forward_param("x", 0.0, 10.0)
.define_param("y", 0.5)
.left_param("y", 0.0, 10.0)
.define_param("x", 0.75)
.up_param("x", 0.0, 10.0);
const RETAINED: LinkageFixed<1, 0, 5> = BASE.retain_param_names(&["y"]);
assert_specialized_matches_original(BASE, &[0.25, 1.0, 0.75], RETAINED, &[1.0])?;
Ok(())
}
#[test]
fn specialization_matches_original_for_index_and_name_selectors()
-> Result<(), Box<dyn StdError>> {
const BASE: LinkageFixed<3, 0, 5> = LinkageFixed::start()
.define_param("x", 0.25)
.forward_param("x", 0.0, 10.0)
.define_param("y", 0.5)
.left_param("y", 0.0, 10.0)
.define_param("z", 0.75)
.up_param("z", 0.0, 10.0);
const SPECIALIZED: LinkageFixed<1, 0, 5> = BASE
.freeze_param_index::<2>(0, 4.0)
.retain_param_names(&["y"]);
assert_specialized_matches_original(BASE, &[0.4, 0.8, 0.75], SPECIALIZED, &[0.8])?;
Ok(())
}
const BRUCE: LinkageFixed<4, 0, 8> = LinkageFixed::start()
.define_param("Bruce", 0.10)
.pen_down()
.forward_param("Bruce", 0.0, 100.0)
.define_param("Bruce", 0.20)
.left_param("Bruce", 0.0, 100.0)
.define_param("Bruce", 0.30)
.up_param("Bruce", 0.0, 100.0)
.define_param("Bruce", 0.40)
.yaw_param("Bruce", -180.0, 180.0);
#[test]
fn bruce_full_evaluation_uses_each_slot_bound_at_step_creation() -> Result<(), Box<dyn StdError>>
{
let pose = BRUCE.view().final_pose(&[0.11, 0.22, 0.33, 0.44])?;
assert_pose_close(
pose,
Pose::new(pose.orientation(), Vec3::from([11.0, 22.0, 33.0])),
1e-4,
);
Ok(())
}
#[test]
fn bruce_freeze_param_index_freezes_one_slot() -> Result<(), Box<dyn StdError>> {
const FROZEN: LinkageFixed<3, 0, 8> = BRUCE.freeze_param_index(2, 33.0);
assert_specialized_matches_original(
BRUCE,
&[0.11, 0.22, 0.33, 0.44],
FROZEN,
&[0.11, 0.22, 0.44],
)?;
Ok(())
}
#[test]
fn bruce_freeze_param_index_at_default_freezes_one_slot() -> Result<(), Box<dyn StdError>> {
const FROZEN: LinkageFixed<3, 0, 8> = BRUCE.freeze_param_index_at_default(2);
assert_specialized_matches_original(
BRUCE,
&[0.11, 0.22, 0.30, 0.44],
FROZEN,
&[0.11, 0.22, 0.44],
)?;
Ok(())
}
#[test]
fn bruce_retain_param_names_retains_all_slots_named_bruce() -> Result<(), Box<dyn StdError>> {
const RETAINED: LinkageFixed<4, 0, 8> = BRUCE.retain_param_names(&["Bruce"]);
let params = RETAINED.view().params();
assert_eq!(params[0].name(), "Bruce");
assert_eq!(params[1].name(), "Bruce");
assert_eq!(params[2].name(), "Bruce");
assert_eq!(params[3].name(), "Bruce");
assert_specialized_matches_original(
BRUCE,
&[0.11, 0.22, 0.33, 0.44],
RETAINED,
&[0.11, 0.22, 0.33, 0.44],
)?;
Ok(())
}
#[test]
fn bruce_retain_param_names_freezes_non_bruce_at_own_default() -> Result<(), Box<dyn StdError>>
{
const BASE: LinkageFixed<3, 0, 7> = LinkageFixed::start()
.define_param("Bruce", 0.10)
.pen_down()
.forward_param("Bruce", 0.0, 100.0)
.define_param("Terry", 0.77)
.left_param("Terry", 0.0, 100.0)
.define_param("Bruce", 0.30)
.up_param("Bruce", 0.0, 100.0);
const RETAINED: LinkageFixed<2, 0, 7> = BASE.retain_param_names(&["Bruce"]);
assert_specialized_matches_original(BASE, &[0.11, 0.77, 0.33], RETAINED, &[0.11, 0.33])?;
Ok(())
}
#[cfg(feature = "alloc")]
#[test]
fn linkage_buf_retain_param_indexes_drops_multiple_params() -> Result<(), Box<dyn StdError>> {
let base: LinkageBuf<3, 0> = LinkageBuf::start()
.define_param("yaw", 0.5)
.define_param("pitch", 0.5)
.define_param("dist", 0.5)
.yaw_param("yaw", -90.0, 90.0)
.pitch_param("pitch", -90.0, 90.0)
.forward_param("dist", 0.0, 6.0);
let frozen: LinkageBuf<1, 0> = base.retain_param_indexes(&[2]);
let pos = frozen.view().final_pose(&[1.0])?.position();
assert!(pos.is_close_to(&Vec3::from([6.0, 0.0, 0.0]), 1e-4));
Ok(())
}
#[cfg(feature = "alloc")]
#[test]
fn linkage_buf_retain_params_handles_shadowed_non_retained_params_by_slot()
-> Result<(), Box<dyn StdError>> {
let base: LinkageBuf<3, 0> = LinkageBuf::start()
.define_param("x", 0.25)
.forward_param("x", 0.0, 10.0)
.define_param("y", 0.5)
.left_param("y", 0.0, 10.0)
.define_param("x", 0.75)
.up_param("x", 0.0, 10.0);
let retained: LinkageBuf<1, 0> = base.retain_param_names(&["y"]);
let pos = retained.view().final_pose(&[1.0])?.position();
assert!(pos.is_close_to(&Vec3::from([2.5, 10.0, 7.5]), 1e-4));
Ok(())
}
#[cfg(feature = "alloc")]
#[test]
fn linkage_buf_retain_param_names_uses_each_shadowed_slots_own_default()
-> Result<(), Box<dyn StdError>> {
let base: LinkageBuf<3, 0> = LinkageBuf::start()
.define_param("x", 0.25)
.forward_param("x", 0.0, 10.0)
.define_param("y", 0.5)
.left_param("y", 0.0, 10.0)
.define_param("x", 0.75)
.up_param("x", 0.0, 10.0);
let frozen: LinkageBuf<1, 0> = base.retain_param_names(&["y"]);
let pos = frozen.view().final_pose(&[1.0])?.position();
assert!(pos.is_close_to(&Vec3::from([2.5, 10.0, 7.5]), 1e-4));
Ok(())
}
#[test]
#[should_panic(expected = "freeze name not found in params")]
fn freeze_param_name_rejects_unknown_name() {
let linkage: LinkageFixed<1, 0, 4> = LinkageFixed::start()
.define_param("angle", 0.5)
.yaw_param("angle", -90.0, 90.0);
let _: LinkageFixed<0, 0, 4> = linkage.freeze_param_name("typo", 0.0);
}
#[test]
#[should_panic(expected = "freeze param index out of bounds")]
fn freeze_param_index_rejects_out_of_bounds_index() {
let linkage: LinkageFixed<1, 0, 4> = LinkageFixed::start()
.define_param("angle", 0.5)
.yaw_param("angle", -90.0, 90.0);
let _: LinkageFixed<0, 0, 4> = linkage.freeze_param_index(9, 0.0);
}
#[test]
#[should_panic(expected = "freeze name is ambiguous")]
fn freeze_param_name_rejects_duplicate_name() {
let linkage: LinkageFixed<2, 0, 4> = LinkageFixed::start()
.define_param("angle", 0.5)
.define_param("angle", 0.5)
.yaw_param("angle", -90.0, 90.0);
let _: LinkageFixed<1, 0, 4> = linkage.freeze_param_name("angle", 0.0);
}
#[test]
#[should_panic(expected = "raw freeze value out of range")]
fn freeze_param_index_rejects_raw_value_above_range() {
let linkage: LinkageFixed<1, 0, 4> = LinkageFixed::start()
.define_param("angle", 0.5)
.yaw_param("angle", -180.0, 180.0);
let _: LinkageFixed<0, 0, 4> = linkage.freeze_param_index(0, 999.0);
}
#[test]
#[should_panic(expected = "raw freeze value out of range")]
fn freeze_param_index_rejects_raw_value_outside_one_referenced_range() {
let linkage: LinkageFixed<1, 0, 4> = LinkageFixed::start()
.define_param("x", 0.5)
.yaw_param("x", -180.0, 180.0)
.forward_param("x", 0.0, 10.0);
let _: LinkageFixed<0, 0, 4> = linkage.freeze_param_index(0, 90.0);
}
#[test]
#[should_panic(expected = "retain name not found in params")]
fn retain_params_rejects_unknown_name() {
let linkage: LinkageFixed<2, 0, 5> = LinkageFixed::start()
.define_param("angle", 0.5)
.define_param("dist", 0.5)
.yaw_param("angle", -90.0, 90.0)
.forward_param("dist", 0.0, 10.0);
let _: LinkageFixed<1, 0, 5> = linkage.retain_param_names(&["unknown_param"]);
}
#[test]
#[should_panic(expected = "duplicate name in retain list")]
fn retain_params_rejects_duplicate_name() {
let linkage: LinkageFixed<2, 0, 5> = LinkageFixed::start()
.define_param("angle", 0.5)
.define_param("dist", 0.5)
.yaw_param("angle", -90.0, 90.0)
.forward_param("dist", 0.0, 10.0);
let _: LinkageFixed<2, 0, 5> = linkage.retain_param_names(&["angle", "angle"]);
}
#[test]
#[should_panic(expected = "duplicate index in retain list")]
fn retain_param_indexes_rejects_duplicate_index() {
let linkage: LinkageFixed<2, 0, 5> = LinkageFixed::start()
.define_param("angle", 0.5)
.define_param("dist", 0.5)
.yaw_param("angle", -90.0, 90.0)
.forward_param("dist", 0.0, 10.0);
let _: LinkageFixed<2, 0, 5> = linkage.retain_param_indexes(&[0, 0]);
}
#[test]
fn retain_params_output_follows_original_param_order() -> Result<(), Box<dyn StdError>> {
const BASE: LinkageFixed<3, 0, 6> = LinkageFixed::start()
.define_param("x", 0.5)
.define_param("y", 0.5)
.define_param("z", 0.5)
.forward_param("x", 0.0, 3.0)
.left_param("z", 0.0, 7.0);
const RETAINED: LinkageFixed<2, 0, 6> = BASE.retain_param_names(&["z", "x"]);
let pos = RETAINED.view().final_pose(&[1.0, 0.0])?.position();
assert!(pos.is_close_to(&Vec3::from([3.0, 0.0, 0.0]), 1e-4));
let pos2 = RETAINED.view().final_pose(&[1.0, 1.0])?.position();
assert!(pos2.is_close_to(&Vec3::from([3.0, 7.0, 0.0]), 1e-4));
Ok(())
}
#[test]
fn freeze_covers_all_translation_and_rotation_step_types() -> Result<(), Box<dyn StdError>> {
const BASE: LinkageFixed<1, 0, 8> = LinkageFixed::start()
.define_param("t", 0.5)
.forward_param("t", 0.0, 1.0) .left_param("t", 0.0, 2.0) .up_param("t", 0.0, 3.0) .yaw_param("t", 0.0, 0.0) .pitch_param("t", 0.0, 0.0) .roll_param("t", 0.0, 0.0);
const FROZEN: LinkageFixed<0, 0, 8> = BASE.retain_param_indexes(&[]);
let pos = FROZEN.view().final_pose(&[])?.position();
assert!(pos.is_close_to(&Vec3::from([0.5, 1.0, 1.5]), 1e-4));
Ok(())
}
#[test]
fn freeze_param_index_at_default_supports_multiple_step_ranges() -> Result<(), Box<dyn StdError>>
{
const BASE: LinkageFixed<1, 0, 4> = LinkageFixed::start()
.define_param("t", 0.5)
.forward_param("t", 0.0, 10.0) .left_param("t", 0.0, 20.0);
const FROZEN: LinkageFixed<0, 0, 4> = BASE.freeze_param_index_at_default(0);
let pos = FROZEN.view().final_pose(&[])?.position();
assert!(pos.is_close_to(&Vec3::from([5.0, 10.0, 0.0]), 1e-4));
Ok(())
}
#[test]
fn linkage_view_materializes_active_step_count() {
const BASE: LinkageFixed<0, 0, 5> = LinkageFixed::start()
.yaw(0.0)
.forward(2.0)
.left(0.0)
.up(1.0);
const STRIPPED: LinkageView<'static, 0, 0> = BASE.view();
let steps = STRIPPED.steps();
assert_eq!(steps.len(), 5);
assert!(matches!(steps[0], Step::Start));
assert!(matches!(steps[1], Step::Yaw(StepArg::Fixed(value)) if value == 0.0));
assert_fixed_move(steps[2], 2.0);
assert!(matches!(steps[3], Step::Left(StepArg::Fixed(value)) if value == 0.0));
assert_fixed_up(steps[4], 1.0);
}
#[test]
fn linkage_fixed_freeze_runs_cleanup_passes() -> Result<(), Box<dyn StdError>> {
const BASE: LinkageFixed<1, 0, 6> = LinkageFixed::start()
.define_param("t", 0.5)
.yaw_param("t", -90.0, 90.0)
.forward_param("t", 0.0, 4.0)
.forward(6.0)
.left_param("t", -2.0, 2.0)
.up(1.0);
const FROZEN: LinkageFixed<0, 0, 6> = BASE.freeze_param_index_at_default(0);
let steps = FROZEN.view().steps();
assert_eq!(steps.len(), 3);
assert!(matches!(steps[0], Step::Start));
assert_fixed_move(steps[1], 8.0);
assert_fixed_up(steps[2], 1.0);
let original_pose = BASE.view().final_pose(&[0.5])?;
let frozen_pose = FROZEN.view().final_pose(&[])?;
assert_pose_close(original_pose, frozen_pose, 1e-5);
Ok(())
}
#[cfg(feature = "alloc")]
#[test]
fn linkage_buf_freeze_runs_cleanup_passes() -> Result<(), Box<dyn StdError>> {
let base: LinkageBuf<1, 0> = LinkageBuf::start()
.define_param("t", 0.5)
.yaw_param("t", -90.0, 90.0)
.forward_param("t", 0.0, 4.0)
.forward(6.0)
.left_param("t", -2.0, 2.0)
.up(1.0);
let frozen: LinkageBuf<0, 0> = base.clone().freeze_param_index_at_default(0);
let steps = frozen.view().steps();
assert_eq!(steps.len(), 3);
assert!(matches!(steps[0], Step::Start));
assert_fixed_move(steps[1], 8.0);
assert_fixed_up(steps[2], 1.0);
let original_pose = base.view().final_pose(&[0.5])?;
let frozen_pose = frozen.view().final_pose(&[])?;
assert_pose_close(original_pose, frozen_pose, 1e-5);
Ok(())
}
#[cfg(feature = "alloc")]
#[test]
#[should_panic(expected = "freeze name not found in params")]
fn linkage_buf_freeze_param_name_rejects_unknown_name() {
let linkage: LinkageBuf<1, 0> = LinkageBuf::start()
.define_param("angle", 0.5)
.yaw_param("angle", -90.0, 90.0);
let _: LinkageBuf<0, 0> = linkage.freeze_param_name("typo", 0.0);
}
#[cfg(feature = "alloc")]
#[test]
#[should_panic(expected = "retain name not found in params")]
fn linkage_buf_retain_params_rejects_unknown_name() {
let linkage: LinkageBuf<2, 0> = LinkageBuf::start()
.define_param("angle", 0.5)
.define_param("dist", 0.5)
.yaw_param("angle", -90.0, 90.0)
.forward_param("dist", 0.0, 10.0);
let _: LinkageBuf<1, 0> = linkage.retain_param_names(&["unknown_param"]);
}
#[cfg(feature = "alloc")]
#[test]
#[should_panic(expected = "duplicate name in retain list")]
fn linkage_buf_retain_params_rejects_duplicate_name() {
let linkage: LinkageBuf<2, 0> = LinkageBuf::start()
.define_param("angle", 0.5)
.define_param("dist", 0.5)
.yaw_param("angle", -90.0, 90.0)
.forward_param("dist", 0.0, 10.0);
let _: LinkageBuf<2, 0> = linkage.retain_param_names(&["angle", "angle"]);
}
#[cfg(feature = "alloc")]
#[test]
fn linkage_buf_freeze_output_matches_linkage_fixed_freeze_output()
-> Result<(), Box<dyn StdError>> {
const FIXED_BASE: LinkageFixed<2, 0, 5> = LinkageFixed::start()
.define_param("angle", 0.5)
.define_param("dist", 0.5)
.yaw_param("angle", -180.0, 180.0)
.forward_param("dist", 0.0, 8.0);
const FIXED_FROZEN: LinkageFixed<1, 0, 5> = FIXED_BASE.freeze_param_name("angle", -90.0);
let buf_base: LinkageBuf<2, 0> = LinkageBuf::start()
.define_param("angle", 0.5)
.define_param("dist", 0.5)
.yaw_param("angle", -180.0, 180.0)
.forward_param("dist", 0.0, 8.0);
let buf_frozen: LinkageBuf<1, 0> = buf_base.freeze_param_name("angle", -90.0);
for t in [0.0f32, 0.25, 0.5, 0.75, 1.0] {
let pos_fixed = FIXED_FROZEN.view().final_pose(&[t])?.position();
let pos_buf = buf_frozen.view().final_pose(&[t])?.position();
assert!(pos_fixed.is_close_to(&pos_buf, 1e-5));
}
Ok(())
}
fn assert_specialized_matches_original<
const DOF: usize,
const OUT_DOF: usize,
const MARKS: usize,
const N: usize,
>(
original: LinkageFixed<DOF, MARKS, N>,
original_params: &[f32; DOF],
specialized: LinkageFixed<OUT_DOF, MARKS, N>,
specialized_params: &[f32; OUT_DOF],
) -> Result<(), Box<dyn StdError>> {
assert_pose_close(
original.view().final_pose(original_params)?,
specialized.view().final_pose(specialized_params)?,
1e-4,
);
assert_draw_items_3d_close(
original.view().draw_items_3d(original_params)?,
specialized.view().draw_items_3d(specialized_params)?,
1e-4,
);
Ok(())
}
fn assert_draw_items_3d_close(
mut left: impl Iterator<Item = Item3d>,
mut right: impl Iterator<Item = Item3d>,
tolerance: f32,
) {
loop {
match (left.next(), right.next()) {
(Some(left), Some(right)) => assert_draw_item_3d_close(left, right, tolerance),
(None, None) => break,
(Some(_), None) => panic!("specialized linkage emitted fewer draw items"),
(None, Some(_)) => panic!("specialized linkage emitted more draw items"),
}
}
}
fn assert_draw_item_3d_close(left: Item3d, right: Item3d, tolerance: f32) {
match (left, right) {
(Item3d::Stroke(left), Item3d::Stroke(right)) => {
assert_pose_close(left.start(), right.start(), tolerance);
assert_pose_close(left.end(), right.end(), tolerance);
assert!((left.width() - right.width()).abs() <= tolerance);
}
(Item3d::Disk(left), Item3d::Disk(right)) => {
assert_pose_close(left.pose(), right.pose(), tolerance);
assert!((left.radius() - right.radius()).abs() <= tolerance);
}
(Item3d::Sphere(left), Item3d::Sphere(right)) => {
assert_pose_close(left.pose(), right.pose(), tolerance);
assert!((left.radius() - right.radius()).abs() <= tolerance);
}
_ => panic!("draw item variants differ"),
}
}
fn assert_pose_close(left: Pose, right: Pose, tolerance: f32) {
assert!(left.position().is_close_to(&right.position(), tolerance));
assert!(
left.orientation()
.is_close_to(&right.orientation(), tolerance)
);
}
fn assert_fixed_move(step: Step, expected: f32) {
match step {
Step::Forward(StepArg::Fixed(actual)) => assert!((actual - expected).abs() <= 1e-6),
_ => panic!("expected fixed forward step"),
}
}
fn assert_fixed_up(step: Step, expected: f32) {
match step {
Step::Up(StepArg::Fixed(actual)) => assert!((actual - expected).abs() <= 1e-6),
_ => panic!("expected fixed up step"),
}
}
}