1#![cfg_attr(not(feature = "std"), no_std)]
2#![forbid(unsafe_code)]
3#![doc = include_str!(concat!(env!("OUT_DIR"), "/README.md"))]
4#![doc = ""]
5#![doc = "## Coordinate System"]
6#[cfg(test)]
23extern crate std;
24
25#[cfg(feature = "alloc")]
26extern crate alloc;
27
28pub mod bvh;
29mod math;
30pub mod render;
31
32pub mod examples {
39 #[cfg(feature = "examples-armatron")]
40 #[path = "armatron/main.rs"]
41 pub mod armatron;
42 #[cfg(feature = "examples-ballet")]
43 pub mod ballet;
44 #[cfg(feature = "examples-clock")]
45 pub mod clock;
46 #[cfg(feature = "examples-skeleton-clock")]
47 pub mod skeleton_clock;
48 #[cfg(feature = "examples-armatron")]
49 mod ui;
50}
51
52#[cfg(feature = "alloc")]
53use alloc::{borrow::ToOwned, boxed::Box, format, string::String, vec::Vec};
54
55use core::fmt;
56
57pub use embedded_graphics::pixelcolor::Rgb888;
80pub use embedded_graphics::pixelcolor::WebColors;
83pub use embedded_graphics::prelude::RgbColor;
86use math::degrees_to_radians;
87pub use math::{Mat3, Vec3};
88
89use render::{Disk, Item3d, Projection, Sphere, Stroke};
90
91#[derive(Clone, Copy, Debug)]
105pub enum Step {
106 Start,
108 Yaw(StepArg),
110 Pitch(StepArg),
112 Roll(StepArg),
114 Forward(StepArg),
116 Left(StepArg),
118 Up(StepArg),
120 PenUp,
122 PenDown,
124 PenColor(Rgb888),
126 PenWidth(f32),
128 Disk(f32),
130 DiskParam(ParamArg),
132 Sphere(f32),
134 SphereParam(ParamArg),
136 Mark {
138 index: usize,
140 },
141 Restore {
143 index: usize,
145 },
146}
147
148#[derive(Clone, Copy, Debug)]
156pub enum StepArg {
157 Fixed(f32),
159 Variable(ParamArg),
161}
162
163#[derive(Clone, Copy, Debug)]
169pub struct ParamArg {
170 index: usize,
171 low: f32,
172 span: f32,
173}
174
175#[derive(Clone, Copy, Debug)]
180pub struct Param {
181 name: &'static str,
182 default: f32,
183}
184
185impl Param {
186 const EMPTY: Self = Self {
187 name: "",
188 default: 0.0,
189 };
190
191 #[must_use]
195 pub const fn name(self) -> &'static str {
196 self.name
197 }
198
199 #[must_use]
203 pub const fn default(self) -> f32 {
204 self.default
205 }
206}
207
208impl StepArg {
209 fn resolve<const DOF: usize>(&self, params: &[f32; DOF]) -> f32 {
210 match self {
211 Self::Fixed(value) => *value,
212 Self::Variable(variable_arg) => variable_arg.resolve(params),
213 }
214 }
215
216 const fn offset_param(self, offset: usize) -> Self {
217 match self {
218 Self::Fixed(_) => self,
219 Self::Variable(v) => Self::Variable(v.offset(offset)),
220 }
221 }
222}
223
224impl ParamArg {
225 const fn new(index: usize, low: f32, high: f32) -> Self {
226 Self {
227 index,
228 low,
229 span: high - low,
230 }
231 }
232
233 const fn offset(self, offset: usize) -> Self {
234 Self {
235 index: self.index + offset,
236 ..self
237 }
238 }
239
240 const fn from_degrees(index: usize, low: f32, high: f32) -> Self {
241 Self::new(index, degrees_to_radians(low), degrees_to_radians(high))
242 }
243
244 fn resolve<const DOF: usize>(&self, params: &[f32; DOF]) -> f32 {
245 let param = params[self.index];
246 self.low + param * self.span
247 }
248
249 #[must_use]
254 pub const fn index(self) -> usize {
255 self.index
256 }
257
258 #[must_use]
262 pub const fn low(self) -> f32 {
263 self.low
264 }
265
266 #[must_use]
270 pub const fn high(self) -> f32 {
271 self.low + self.span
272 }
273}
274
275#[derive(Clone, Copy, Debug, PartialEq)]
298pub enum Error {
299 InvalidParameter {
301 index: usize,
303 value: f32,
305 },
306 EmptyLinkage,
308 MarkNotFound,
310 MarkAmbiguous,
312}
313
314impl fmt::Display for Error {
315 fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
316 match self {
317 Self::InvalidParameter { index, value } => {
318 write!(formatter, "parameter {index} is out of range: {value}")
319 }
320 Self::EmptyLinkage => formatter.write_str("linkage produced no poses"),
321 Self::MarkNotFound => formatter.write_str("mark was not found"),
322 Self::MarkAmbiguous => formatter.write_str("mark name is ambiguous"),
323 }
324 }
325}
326
327#[cfg(test)]
328impl std::error::Error for Error {}
329
330#[derive(Clone, Copy)]
354pub struct LinkageView<'a, const DOF: usize, const MARKS: usize> {
355 params: &'a [Param; DOF],
356 steps: &'a [Step],
357 mark_names: &'a [&'static str; MARKS],
358 mark_len: usize,
359}
360
361impl<'a, const DOF: usize, const MARKS: usize> LinkageView<'a, DOF, MARKS> {
362 pub const DOF: usize = DOF;
364
365 pub const MARKS: usize = MARKS;
367
368 #[must_use]
370 pub(crate) const fn new(
371 params: &'a [Param; DOF],
372 steps: &'a [Step],
373 mark_names: &'a [&'static str; MARKS],
374 mark_len: usize,
375 ) -> Self {
376 Self {
377 params,
378 steps,
379 mark_names,
380 mark_len,
381 }
382 }
383
384 #[must_use]
386 pub const fn dof(&self) -> usize {
387 DOF
388 }
389
390 #[must_use]
392 pub const fn len(&self) -> usize {
393 self.steps.len()
394 }
395
396 #[must_use]
398 pub const fn is_empty(&self) -> bool {
399 self.steps.is_empty()
400 }
401
402 #[doc(hidden)]
404 pub const fn step_count(&self) -> usize {
405 self.len()
406 }
407
408 #[must_use]
428 pub const fn param(&self, index: usize) -> Param {
429 self.params[index]
430 }
431
432 #[must_use]
447 pub const fn params(&self) -> &'a [Param; DOF] {
448 self.params
449 }
450
451 #[must_use]
465 pub const fn param_defaults(&self) -> [f32; DOF] {
466 let mut defaults = [0.0; DOF];
467 let mut param_index = 0;
468 while param_index < DOF {
469 defaults[param_index] = self.params[param_index].default();
470 param_index += 1;
471 }
472 defaults
473 }
474
475 #[must_use]
477 pub const fn steps(&self) -> &'a [Step] {
478 self.steps
479 }
480
481 #[must_use]
503 pub const fn scan_param_range(&self, index: usize) -> (f32, f32) {
504 let mut i = 0;
505 while i < self.steps.len() {
506 let arg = match self.steps[i] {
507 Step::Yaw(arg)
508 | Step::Pitch(arg)
509 | Step::Roll(arg)
510 | Step::Forward(arg)
511 | Step::Left(arg)
512 | Step::Up(arg) => arg,
513 Step::DiskParam(v) | Step::SphereParam(v) => StepArg::Variable(v),
514 _ => {
515 i += 1;
516 continue;
517 }
518 };
519 if let StepArg::Variable(v) = arg
520 && v.index() == index
521 {
522 return (v.low(), v.high());
523 }
524 i += 1;
525 }
526 panic!("no step drives the parameter at this index")
527 }
528
529 #[must_use]
531 pub const fn mark_names(&self) -> &'a [&'static str; MARKS] {
532 self.mark_names
533 }
534
535 #[must_use]
537 pub const fn mark_len(&self) -> usize {
538 self.mark_len
539 }
540
541 #[must_use]
543 pub const fn mark_count(&self) -> usize {
544 self.mark_len()
545 }
546
547 #[must_use]
549 pub const fn marks(&self) -> usize {
550 MARKS
551 }
552
553 pub const fn to_fixed<const N: usize>(self) -> LinkageFixed<DOF, MARKS, N> {
559 assert!(
560 self.steps.len() <= N,
561 "fixed backing is too small for linkage view"
562 );
563 let mut steps = [const { Step::Start }; N];
564 let mut step_index = 0;
565 while step_index < self.steps.len() {
566 steps[step_index] = self.steps[step_index];
567 step_index += 1;
568 }
569 LinkageFixed {
570 steps,
571 len: self.steps.len(),
572 params: *self.params,
573 param_len: DOF,
574 mark_names: *self.mark_names,
575 mark_len: self.mark_len,
576 }
577 }
578
579 const fn combine_fixed<
580 const DOF2: usize,
581 const MARKS2: usize,
582 const DOF_OUT: usize,
583 const MARKS_OUT: usize,
584 const N_OUT: usize,
585 >(
586 self,
587 other: LinkageView<'_, DOF2, MARKS2>,
588 ) -> LinkageFixed<DOF_OUT, MARKS_OUT, N_OUT> {
589 let needed_dof = DOF + DOF2;
590 assert!(DOF_OUT == needed_dof, "DOF_OUT must equal the combined DOF");
591 let needed_marks = self.mark_len + other.mark_len;
592 assert!(MARKS_OUT >= needed_marks, "MARKS_OUT is too small");
593 let needed_steps = self.steps.len() + other.steps.len() - 1;
594 assert!(N_OUT >= needed_steps, "N_OUT is too small");
595
596 let mut out = LinkageFixed {
597 steps: [const { Step::Start }; N_OUT],
598 len: self.steps.len(),
599 params: [Param::EMPTY; DOF_OUT],
600 param_len: DOF + DOF2,
601 mark_names: [""; MARKS_OUT],
602 mark_len: needed_marks,
603 };
604
605 let mut step_index = 0;
606 while step_index < self.steps.len() {
607 out.steps[step_index] = self.steps[step_index];
608 step_index += 1;
609 }
610 let mut other_step_index = 1;
611 while other_step_index < other.steps.len() {
612 out.steps[out.len] = other.steps[other_step_index].offset_params(DOF, self.mark_len);
613 out.len += 1;
614 other_step_index += 1;
615 }
616
617 let mut param_index = 0;
618 while param_index < DOF {
619 out.params[param_index] = self.params[param_index];
620 param_index += 1;
621 }
622 let mut other_param_index = 0;
623 while other_param_index < DOF2 {
624 out.params[DOF + other_param_index] = other.params[other_param_index];
625 other_param_index += 1;
626 }
627
628 let mut mark_index = 0;
629 while mark_index < self.mark_len {
630 out.mark_names[mark_index] = self.mark_names[mark_index];
631 mark_index += 1;
632 }
633 let mut other_mark_index = 0;
634 while other_mark_index < other.mark_len {
635 out.mark_names[self.mark_len + other_mark_index] = other.mark_names[other_mark_index];
636 other_mark_index += 1;
637 }
638 out
639 }
640
641 #[cfg(feature = "alloc")]
648 #[must_use]
649 pub fn to_lb_rs(&self) -> String {
650 let named_params = self.params.iter().filter(|p| !p.name().is_empty()).count();
651 let mut source = format!(
652 "// DOF={} MARKS={} STEPS={}\n",
653 named_params,
654 self.mark_len,
655 self.len()
656 );
657 source.push_str("linkage![\n");
658 for param in self.params {
659 if !param.name().is_empty() {
660 source.push_str(" .define_param(\"");
661 source.push_str(param.name());
662 source.push_str("\", ");
663 push_f32(&mut source, param.default());
664 source.push_str(")\n");
665 }
666 }
667
668 for &step in self.steps {
669 match step {
670 Step::Start => {}
671 Step::Yaw(arg) => push_arg_step(self, &mut source, "yaw", "yaw_param", arg, true),
672 Step::Pitch(arg) => {
673 push_arg_step(self, &mut source, "pitch", "pitch_param", arg, true);
674 }
675 Step::Roll(arg) => {
676 push_arg_step(self, &mut source, "roll", "roll_param", arg, true);
677 }
678 Step::Forward(arg) => {
679 push_arg_step(self, &mut source, "forward", "forward_param", arg, false);
680 }
681 Step::Left(arg) => {
682 push_arg_step(self, &mut source, "left", "left_param", arg, false);
683 }
684 Step::Up(arg) => push_arg_step(self, &mut source, "up", "up_param", arg, false),
685 Step::PenUp => source.push_str(" .pen_up()\n"),
686 Step::PenDown => source.push_str(" .pen_down()\n"),
687 Step::PenColor(color) => {
688 source.push_str(" .pen_color(Rgb888::new(");
689 source.push_str(&format!("{}, {}, {}", color.r(), color.g(), color.b()));
690 source.push_str("))\n");
691 }
692 Step::PenWidth(width) => {
693 source.push_str(" .pen_width(");
694 push_f32(&mut source, width);
695 source.push_str(")\n");
696 }
697 Step::Disk(radius) => push_fixed_step(&mut source, "disk", radius),
698 Step::DiskParam(arg) => push_variable_step(self, &mut source, "disk_param", arg),
699 Step::Sphere(radius) => push_fixed_step(&mut source, "sphere", radius),
700 Step::SphereParam(arg) => {
701 push_variable_step(self, &mut source, "sphere_param", arg);
702 }
703 Step::Mark { index } => {
704 let name = self.mark_names[index];
705 source.push_str(" .mark(\"");
706 source.push_str(name);
707 source.push_str("\")\n");
708 }
709 Step::Restore { index } => {
710 let name = self.mark_names[index];
711 source.push_str(" .restore(\"");
712 source.push_str(name);
713 source.push_str("\")\n");
714 }
715 }
716 }
717 source.push_str("]\n");
718 source
719 }
720
721 #[must_use]
741 pub const fn param_index(&self, name: &str, n: usize) -> usize {
742 let mut found = 0;
743 let mut i = 0;
744 while i < DOF {
745 if str_eq(self.params[i].name, name) {
746 if found == n {
747 return i;
748 }
749 found += 1;
750 }
751 i += 1;
752 }
753 panic!("parameter name not found or occurrence index out of range")
754 }
755
756 pub fn final_pose(&self, params: &[f32; DOF]) -> Result<Pose, Error> {
780 self.poses(params)?.last().ok_or(Error::EmptyLinkage)
781 }
782
783 pub fn poses<'b>(
804 &'b self,
805 params: &'b [f32; DOF],
806 ) -> Result<impl Iterator<Item = Pose> + 'b, Error> {
807 Ok(self.styled_poses(params)?.map(|sp| sp.pose()))
808 }
809
810 pub fn styled_poses<'b>(
833 &'b self,
834 params: &'b [f32; DOF],
835 ) -> Result<impl Iterator<Item = StyledPose> + 'b, Error> {
836 StyledPosesView::<DOF, MARKS>::new(self.steps, params)
837 }
838
839 pub fn draw_items_3d<'b>(
858 &'b self,
859 params: &'b [f32; DOF],
860 ) -> Result<DrawItem3dIter<'b, DOF, MARKS>, Error> {
861 DrawItem3dIter::<DOF, MARKS>::new(self.steps, self.mark_names, self.mark_len, params)
862 }
863
864 #[must_use]
882 pub const fn draw_item_3d_count(&self) -> usize {
883 let mut count = 0;
884 let mut pen_down = true;
885 let mut marked = [true; MARKS];
886 let mut index = 0;
887 while index < self.steps.len() {
888 match &self.steps[index] {
889 Step::Start => pen_down = true,
890 Step::PenUp => pen_down = false,
891 Step::PenDown => pen_down = true,
892 Step::Mark { index: mark_index } => marked[*mark_index] = pen_down,
893 Step::Restore { index: mark_index } => pen_down = marked[*mark_index],
894 Step::Forward(_) | Step::Left(_) | Step::Up(_) => {
895 if pen_down {
896 count += 1;
897 }
898 }
899 Step::Disk(_) | Step::DiskParam(_) | Step::Sphere(_) | Step::SphereParam(_) => {
900 count += 1;
901 }
902 Step::Yaw(_)
903 | Step::Pitch(_)
904 | Step::Roll(_)
905 | Step::PenColor(_)
906 | Step::PenWidth(_) => {}
907 }
908 index += 1;
909 }
910 count
911 }
912}
913
914impl<'a, const DOF: usize, const MARKS: usize, const N: usize> From<&'a LinkageFixed<DOF, MARKS, N>>
915 for LinkageView<'a, DOF, MARKS>
916{
917 fn from(linkage: &'a LinkageFixed<DOF, MARKS, N>) -> Self {
918 linkage.view()
919 }
920}
921
922#[cfg(feature = "alloc")]
923fn push_arg_step<const DOF: usize, const MARKS: usize>(
924 linkage_view: &LinkageView<'_, DOF, MARKS>,
925 source: &mut String,
926 fixed_method: &str,
927 variable_method: &str,
928 arg: StepArg,
929 degrees: bool,
930) {
931 match arg {
932 StepArg::Fixed(value) => {
933 let value = if degrees { value.to_degrees() } else { value };
934 push_fixed_step(source, fixed_method, value);
935 }
936 StepArg::Variable(variable_arg) => {
937 let low = if degrees {
938 variable_arg.low().to_degrees()
939 } else {
940 variable_arg.low()
941 };
942 let high = if degrees {
943 variable_arg.high().to_degrees()
944 } else {
945 variable_arg.high()
946 };
947 source.push_str(" .");
948 source.push_str(variable_method);
949 source.push_str("(\"");
950 source.push_str(linkage_view.param(variable_arg.index()).name());
951 source.push_str("\", ");
952 push_f32(source, low);
953 source.push_str(", ");
954 push_f32(source, high);
955 source.push_str(")\n");
956 }
957 }
958}
959
960#[cfg(feature = "alloc")]
961fn push_fixed_step(source: &mut String, method: &str, value: f32) {
962 source.push_str(" .");
963 source.push_str(method);
964 source.push('(');
965 push_f32(source, value);
966 source.push_str(")\n");
967}
968
969#[cfg(feature = "alloc")]
970fn push_variable_step<const DOF: usize, const MARKS: usize>(
971 linkage_view: &LinkageView<'_, DOF, MARKS>,
972 source: &mut String,
973 method: &str,
974 variable_arg: ParamArg,
975) {
976 source.push_str(" .");
977 source.push_str(method);
978 source.push_str("(\"");
979 source.push_str(linkage_view.param(variable_arg.index()).name());
980 source.push_str("\", ");
981 push_f32(source, variable_arg.low());
982 source.push_str(", ");
983 push_f32(source, variable_arg.high());
984 source.push_str(")\n");
985}
986
987#[cfg(feature = "alloc")]
988fn push_f32(source: &mut String, value: f32) {
989 source.push_str(&format!("{value:?}"));
990}
991
992#[cfg(feature = "alloc")]
993fn parse_lb_rs<const DOF: usize, const MARKS: usize>(
994 source: &str,
995) -> Result<LinkageBuf<DOF, MARKS>, String> {
996 let mut linkage = LinkageBuf::start();
997
998 for (line_index, line) in source.lines().enumerate() {
999 let line_number = line_index + 1;
1000 let Some(method_call) = parse_method_call(line_number, line)? else {
1001 continue;
1002 };
1003 linkage = apply_parsed_method(line_number, linkage, &method_call)?;
1004 }
1005
1006 Ok(linkage)
1007}
1008
1009#[cfg(feature = "alloc")]
1010#[derive(Clone, Debug)]
1011struct ParsedMethodCall {
1012 name: String,
1013 args: Vec<String>,
1014}
1015
1016#[cfg(feature = "alloc")]
1017fn parse_method_call(line_number: usize, line: &str) -> Result<Option<ParsedMethodCall>, String> {
1018 let line = strip_rust_comment(line).trim();
1019 if line.is_empty() || line == "LinkageFixed::start()" || is_linkage_macro_wrapper(line) {
1020 return Ok(None);
1021 }
1022
1023 if line.contains("LinkageFixed::start()") {
1024 return Ok(None);
1025 }
1026
1027 let line = line.trim_end_matches(';').trim_end_matches(',').trim();
1028 let line = line.strip_prefix('.').unwrap_or(line);
1029
1030 let open = line
1031 .find('(')
1032 .ok_or_else(|| format!("line {line_number}: expected `(`"))?;
1033 let close = line
1034 .rfind(')')
1035 .ok_or_else(|| format!("line {line_number}: expected `)`"))?;
1036 if close < open {
1037 return Err(format!("line {line_number}: `)` appears before `(`"));
1038 }
1039 let trailing = line[close + 1..].trim();
1040 if !trailing.is_empty() {
1041 return Err(format!(
1042 "line {line_number}: unexpected text after method call `{trailing}`"
1043 ));
1044 }
1045
1046 let name = line[..open].trim();
1047 if name.is_empty() {
1048 return Err(format!("line {line_number}: method name is empty"));
1049 }
1050
1051 Ok(Some(ParsedMethodCall {
1052 name: name.to_owned(),
1053 args: split_args(line_number, &line[open + 1..close])?,
1054 }))
1055}
1056
1057#[cfg(feature = "alloc")]
1058fn is_linkage_macro_wrapper(line: &str) -> bool {
1059 let line = line.trim_end_matches(';').trim();
1060 matches!(line, "linkage![" | "linkage! [" | "]")
1061}
1062
1063#[cfg(feature = "alloc")]
1064fn split_args(line_number: usize, args: &str) -> Result<Vec<String>, String> {
1065 let mut split_args = Vec::new();
1066 let mut current_arg = String::new();
1067 let mut parenthesis_depth = 0;
1068 let mut in_string = false;
1069
1070 for character in args.chars() {
1071 match character {
1072 '"' => {
1073 in_string = !in_string;
1074 current_arg.push(character);
1075 }
1076 '(' if !in_string => {
1077 parenthesis_depth += 1;
1078 current_arg.push(character);
1079 }
1080 ')' if !in_string => {
1081 if parenthesis_depth == 0 {
1082 return Err(format!(
1083 "line {line_number}: unexpected `)` in argument list"
1084 ));
1085 }
1086 parenthesis_depth -= 1;
1087 current_arg.push(character);
1088 }
1089 ',' if !in_string && parenthesis_depth == 0 => {
1090 let trimmed_arg = current_arg.trim();
1091 if !trimmed_arg.is_empty() {
1092 split_args.push(trimmed_arg.to_owned());
1093 }
1094 current_arg = String::new();
1095 }
1096 _ => current_arg.push(character),
1097 }
1098 }
1099
1100 if in_string {
1101 return Err(format!("line {line_number}: unterminated string literal"));
1102 }
1103 if parenthesis_depth != 0 {
1104 return Err(format!("line {line_number}: unterminated nested argument"));
1105 }
1106
1107 let current_arg = current_arg.trim();
1108 if !current_arg.is_empty() {
1109 split_args.push(current_arg.to_owned());
1110 }
1111
1112 Ok(split_args)
1113}
1114
1115#[cfg(feature = "alloc")]
1116fn apply_parsed_method<const DOF: usize, const MARKS: usize>(
1117 line_number: usize,
1118 linkage: LinkageBuf<DOF, MARKS>,
1119 method_call: &ParsedMethodCall,
1120) -> Result<LinkageBuf<DOF, MARKS>, String> {
1121 match method_call.name.as_str() {
1122 "define_param" => {
1123 expect_arg_count(line_number, method_call, 2)?;
1124 let name = parse_static_string_arg(line_number, method_call, 0)?;
1125 let default = parse_number_arg(line_number, method_call, 1)?;
1126 if !(0.0..=1.0).contains(&default) {
1127 return Err(format!(
1128 "line {line_number}: define_param default must be between 0.0 and 1.0"
1129 ));
1130 }
1131 Ok(linkage.define_param(name, default))
1132 }
1133 "forward" => {
1134 expect_arg_count(line_number, method_call, 1)?;
1135 Ok(linkage.forward(parse_number_arg(line_number, method_call, 0)?))
1136 }
1137 "forward_param" => apply_translation_param(line_number, linkage, method_call, "forward"),
1138 "left" => {
1139 expect_arg_count(line_number, method_call, 1)?;
1140 Ok(linkage.left(parse_number_arg(line_number, method_call, 0)?))
1141 }
1142 "left_param" => apply_translation_param(line_number, linkage, method_call, "left"),
1143 "up" => {
1144 expect_arg_count(line_number, method_call, 1)?;
1145 Ok(linkage.up(parse_number_arg(line_number, method_call, 0)?))
1146 }
1147 "up_param" => apply_translation_param(line_number, linkage, method_call, "up"),
1148 "yaw" => {
1149 expect_arg_count(line_number, method_call, 1)?;
1150 Ok(linkage.yaw(parse_number_arg(line_number, method_call, 0)?))
1151 }
1152 "yaw_param" => apply_rotation_param(line_number, linkage, method_call, "yaw"),
1153 "pitch" => {
1154 expect_arg_count(line_number, method_call, 1)?;
1155 Ok(linkage.pitch(parse_number_arg(line_number, method_call, 0)?))
1156 }
1157 "pitch_param" => apply_rotation_param(line_number, linkage, method_call, "pitch"),
1158 "roll" => {
1159 expect_arg_count(line_number, method_call, 1)?;
1160 Ok(linkage.roll(parse_number_arg(line_number, method_call, 0)?))
1161 }
1162 "roll_param" => apply_rotation_param(line_number, linkage, method_call, "roll"),
1163 "pen_up" => {
1164 expect_arg_count(line_number, method_call, 0)?;
1165 Ok(linkage.pen_up())
1166 }
1167 "pen_down" => {
1168 expect_arg_count(line_number, method_call, 0)?;
1169 Ok(linkage.pen_down())
1170 }
1171 "pen_color" => {
1172 expect_arg_count(line_number, method_call, 1)?;
1173 Ok(linkage.pen_color(parse_color_arg(line_number, method_call)?))
1174 }
1175 "pen_width" => {
1176 expect_arg_count(line_number, method_call, 1)?;
1177 let width = parse_number_arg(line_number, method_call, 0)?;
1178 if width < 0.0 {
1179 return Err(format!(
1180 "line {line_number}: pen_width must be non-negative"
1181 ));
1182 }
1183 Ok(linkage.pen_width(width))
1184 }
1185 "mark" => {
1186 expect_arg_count(line_number, method_call, 1)?;
1187 Ok(linkage.mark(parse_static_string_arg(line_number, method_call, 0)?))
1188 }
1189 "restore" => {
1190 expect_arg_count(line_number, method_call, 1)?;
1191 Ok(linkage.restore(parse_static_string_arg(line_number, method_call, 0)?))
1192 }
1193 "disk" => {
1194 expect_arg_count(line_number, method_call, 1)?;
1195 Ok(linkage.disk(parse_radius(line_number, method_call, 0)?))
1196 }
1197 "disk_param" => apply_radius_param(line_number, linkage, method_call, "disk"),
1198 "sphere" => {
1199 expect_arg_count(line_number, method_call, 1)?;
1200 Ok(linkage.sphere(parse_radius(line_number, method_call, 0)?))
1201 }
1202 "sphere_param" => apply_radius_param(line_number, linkage, method_call, "sphere"),
1203 _ => Err(format!(
1204 "line {line_number}: unknown method `{}`",
1205 method_call.name
1206 )),
1207 }
1208}
1209
1210#[cfg(feature = "alloc")]
1211fn apply_translation_param<const DOF: usize, const MARKS: usize>(
1212 line_number: usize,
1213 linkage: LinkageBuf<DOF, MARKS>,
1214 method_call: &ParsedMethodCall,
1215 axis: &str,
1216) -> Result<LinkageBuf<DOF, MARKS>, String> {
1217 expect_arg_count(line_number, method_call, 3)?;
1218 let name = parse_string_arg(line_number, method_call, 0)?;
1219 let low = parse_number_arg(line_number, method_call, 1)?;
1220 let high = parse_number_arg(line_number, method_call, 2)?;
1221 match axis {
1222 "forward" => Ok(linkage.forward_param(name, low, high)),
1223 "left" => Ok(linkage.left_param(name, low, high)),
1224 "up" => Ok(linkage.up_param(name, low, high)),
1225 _ => unreachable!(),
1226 }
1227}
1228
1229#[cfg(feature = "alloc")]
1230fn apply_rotation_param<const DOF: usize, const MARKS: usize>(
1231 line_number: usize,
1232 linkage: LinkageBuf<DOF, MARKS>,
1233 method_call: &ParsedMethodCall,
1234 axis: &str,
1235) -> Result<LinkageBuf<DOF, MARKS>, String> {
1236 expect_arg_count(line_number, method_call, 3)?;
1237 let name = parse_string_arg(line_number, method_call, 0)?;
1238 let low = parse_number_arg(line_number, method_call, 1)?;
1239 let high = parse_number_arg(line_number, method_call, 2)?;
1240 match axis {
1241 "yaw" => Ok(linkage.yaw_param(name, low, high)),
1242 "pitch" => Ok(linkage.pitch_param(name, low, high)),
1243 "roll" => Ok(linkage.roll_param(name, low, high)),
1244 _ => unreachable!(),
1245 }
1246}
1247
1248#[cfg(feature = "alloc")]
1249fn apply_radius_param<const DOF: usize, const MARKS: usize>(
1250 line_number: usize,
1251 linkage: LinkageBuf<DOF, MARKS>,
1252 method_call: &ParsedMethodCall,
1253 shape: &str,
1254) -> Result<LinkageBuf<DOF, MARKS>, String> {
1255 expect_arg_count(line_number, method_call, 3)?;
1256 let name = parse_string_arg(line_number, method_call, 0)?;
1257 let low = parse_number_arg(line_number, method_call, 1)?;
1258 let high = parse_number_arg(line_number, method_call, 2)?;
1259 if low < 0.0 || high < 0.0 {
1260 return Err(format!(
1261 "line {line_number}: `{}` radius range must be non-negative",
1262 method_call.name
1263 ));
1264 }
1265 match shape {
1266 "disk" => Ok(linkage.disk_param(name, low, high)),
1267 "sphere" => Ok(linkage.sphere_param(name, low, high)),
1268 _ => unreachable!(),
1269 }
1270}
1271
1272#[cfg(feature = "alloc")]
1273fn strip_rust_comment(line: &str) -> &str {
1274 line.split_once("//")
1275 .map_or(line, |(before_comment, _)| before_comment)
1276}
1277
1278#[cfg(feature = "alloc")]
1279fn expect_arg_count(
1280 line_number: usize,
1281 method_call: &ParsedMethodCall,
1282 expected: usize,
1283) -> Result<(), String> {
1284 if method_call.args.len() == expected {
1285 Ok(())
1286 } else {
1287 Err(format!(
1288 "line {line_number}: `{}` expects {expected} argument(s), got {}",
1289 method_call.name,
1290 method_call.args.len()
1291 ))
1292 }
1293}
1294
1295#[cfg(feature = "alloc")]
1296fn parse_number_arg(
1297 line_number: usize,
1298 method_call: &ParsedMethodCall,
1299 arg_index: usize,
1300) -> Result<f32, String> {
1301 let value = &method_call.args[arg_index];
1302 parse_number_or_constant(line_number, method_call.name.as_str(), value)
1303}
1304
1305#[cfg(feature = "alloc")]
1306fn parse_static_string_arg(
1307 line_number: usize,
1308 method_call: &ParsedMethodCall,
1309 arg_index: usize,
1310) -> Result<&'static str, String> {
1311 let value = parse_string_arg(line_number, method_call, arg_index)?;
1312 Ok(Box::leak(value.to_owned().into_boxed_str()))
1313}
1314
1315#[cfg(feature = "alloc")]
1316fn parse_string_arg(
1317 line_number: usize,
1318 method_call: &ParsedMethodCall,
1319 arg_index: usize,
1320) -> Result<&str, String> {
1321 let value = method_call.args[arg_index].as_str();
1322 value
1323 .strip_prefix('"')
1324 .and_then(|value| value.strip_suffix('"'))
1325 .ok_or_else(|| {
1326 format!(
1327 "line {line_number}: `{}` argument `{value}` must be a string literal",
1328 method_call.name
1329 )
1330 })
1331}
1332
1333#[cfg(feature = "alloc")]
1334fn parse_number_or_constant(
1335 line_number: usize,
1336 method_name: &str,
1337 value: &str,
1338) -> Result<f32, String> {
1339 let numeric_value: String = value
1340 .chars()
1341 .filter(|character| *character != '_')
1342 .collect();
1343
1344 if let Ok(parsed) = numeric_value.parse::<f32>() {
1345 if looks_like_integer(&numeric_value) {
1346 return Err(format!(
1347 "line {line_number}: `{method_name}` argument `{value}` is an integer; use `{value}.0`"
1348 ));
1349 }
1350 return Ok(parsed);
1351 }
1352
1353 number_constant(value).ok_or_else(|| {
1354 format!(
1355 "line {line_number}: `{method_name}` argument `{value}` is not a number or known constant"
1356 )
1357 })
1358}
1359
1360#[cfg(feature = "alloc")]
1361fn looks_like_integer(s: &str) -> bool {
1362 let digits = s
1363 .strip_prefix('-')
1364 .or_else(|| s.strip_prefix('+'))
1365 .unwrap_or(s);
1366 !digits.is_empty() && digits.chars().all(|c| c.is_ascii_digit())
1367}
1368
1369#[cfg(feature = "alloc")]
1370fn parse_radius(
1371 line_number: usize,
1372 method_call: &ParsedMethodCall,
1373 arg_index: usize,
1374) -> Result<f32, String> {
1375 let radius = parse_number_arg(line_number, method_call, arg_index)?;
1376 if radius < 0.0 {
1377 Err(format!(
1378 "line {line_number}: `{}` radius must be non-negative",
1379 method_call.name
1380 ))
1381 } else {
1382 Ok(radius)
1383 }
1384}
1385
1386#[cfg(feature = "alloc")]
1387fn number_constant(name: &str) -> Option<f32> {
1388 match name {
1389 "ARM_WIDTH" => Some(3.0),
1390 "AXIS_WIDTH" => Some(1.0),
1391 _ => None,
1392 }
1393}
1394
1395#[cfg(feature = "alloc")]
1396fn parse_color_arg(line_number: usize, method_call: &ParsedMethodCall) -> Result<Rgb888, String> {
1397 let value = method_call.args[0].as_str();
1398
1399 if let Some(color_args) = value
1400 .strip_prefix("Rgb888::new(")
1401 .and_then(|value| value.strip_suffix(')'))
1402 {
1403 return parse_rgb888_new_color(line_number, color_args);
1404 }
1405
1406 match value {
1407 "Rgb888::CSS_BLACK" => Ok(Rgb888::CSS_BLACK),
1408 "Rgb888::CSS_BLUE" => Ok(Rgb888::CSS_BLUE),
1409 "Rgb888::CSS_CRIMSON" => Ok(Rgb888::CSS_CRIMSON),
1410 "Rgb888::CSS_DARK_SLATE_GRAY" => Ok(Rgb888::CSS_DARK_SLATE_GRAY),
1411 "Rgb888::CSS_DIM_GRAY" => Ok(Rgb888::CSS_DIM_GRAY),
1412 "Rgb888::CSS_GRAY" => Ok(Rgb888::CSS_GRAY),
1413 "Rgb888::CSS_LIGHT_GRAY" => Ok(Rgb888::CSS_LIGHT_GRAY),
1414 "Rgb888::CSS_LIGHT_SLATE_GRAY" => Ok(Rgb888::CSS_LIGHT_SLATE_GRAY),
1415 "Rgb888::CSS_RED" => Ok(Rgb888::CSS_RED),
1416 "Rgb888::CSS_SLATE_GRAY" => Ok(Rgb888::CSS_SLATE_GRAY),
1417 "Rgb888::CSS_STEEL_BLUE" => Ok(Rgb888::CSS_STEEL_BLUE),
1418 "Rgb888::CSS_WHITE" => Ok(Rgb888::CSS_WHITE),
1419 _ => Err(format!(
1420 "line {line_number}: unknown color `{value}`; use `Rgb888::CSS_*` or `Rgb888::new(r, g, b)`"
1421 )),
1422 }
1423}
1424
1425#[cfg(feature = "alloc")]
1426fn parse_rgb888_new_color(line_number: usize, args: &str) -> Result<Rgb888, String> {
1427 let args = split_args(line_number, args)?;
1428 if args.len() != 3 {
1429 return Err(format!(
1430 "line {line_number}: `Rgb888::new` expects 3 argument(s), got {}",
1431 args.len()
1432 ));
1433 }
1434
1435 Ok(Rgb888::new(
1436 parse_u8_arg(line_number, "Rgb888::new", &args[0])?,
1437 parse_u8_arg(line_number, "Rgb888::new", &args[1])?,
1438 parse_u8_arg(line_number, "Rgb888::new", &args[2])?,
1439 ))
1440}
1441
1442#[cfg(feature = "alloc")]
1443fn parse_u8_arg(line_number: usize, method_name: &str, value: &str) -> Result<u8, String> {
1444 let numeric_value: String = value
1445 .chars()
1446 .filter(|character| *character != '_')
1447 .collect();
1448 numeric_value
1449 .parse::<u8>()
1450 .map_err(|_| format!("line {line_number}: `{method_name}` argument `{value}` is not a u8"))
1451}
1452
1453macro_rules! emit_fixed_step_methods {
1456 () => {
1457 pub const fn yaw(self, degrees: f32) -> Self {
1462 self.push(Step::Yaw(StepArg::Fixed(degrees_to_radians(degrees))))
1463 }
1464 pub const fn pitch(self, degrees: f32) -> Self {
1467 self.push(Step::Pitch(StepArg::Fixed(degrees_to_radians(degrees))))
1468 }
1469 pub const fn roll(self, degrees: f32) -> Self {
1472 self.push(Step::Roll(StepArg::Fixed(degrees_to_radians(degrees))))
1473 }
1474 pub const fn forward(self, distance: f32) -> Self {
1478 self.push(Step::Forward(StepArg::Fixed(distance)))
1479 }
1480 pub const fn left(self, distance: f32) -> Self {
1483 self.push(Step::Left(StepArg::Fixed(distance)))
1484 }
1485 pub const fn up(self, distance: f32) -> Self {
1488 self.push(Step::Up(StepArg::Fixed(distance)))
1489 }
1490 pub const fn pen_up(self) -> Self {
1493 self.push(Step::PenUp)
1494 }
1495 pub const fn pen_down(self) -> Self {
1498 self.push(Step::PenDown)
1499 }
1500 pub const fn pen_color(self, color: Rgb888) -> Self {
1503 self.push(Step::PenColor(color))
1504 }
1505 pub const fn pen_width(self, width: f32) -> Self {
1508 assert!(width >= 0.0, "pen width must be non-negative");
1509 self.push(Step::PenWidth(width))
1510 }
1511 pub const fn disk(self, radius: f32) -> Self {
1514 self.push(Step::Disk(radius))
1515 }
1516 pub const fn sphere(self, radius: f32) -> Self {
1519 self.push(Step::Sphere(radius))
1520 }
1521
1522 pub const fn yaw_param(self, name: &str, low: f32, high: f32) -> Self {
1527 let index = self.expect_param_index(name);
1528 self.push(Step::Yaw(StepArg::Variable(ParamArg::from_degrees(
1529 index, low, high,
1530 ))))
1531 }
1532 pub const fn pitch_param(self, name: &str, low: f32, high: f32) -> Self {
1535 let index = self.expect_param_index(name);
1536 self.push(Step::Pitch(StepArg::Variable(ParamArg::from_degrees(
1537 index, low, high,
1538 ))))
1539 }
1540 pub const fn roll_param(self, name: &str, low: f32, high: f32) -> Self {
1543 let index = self.expect_param_index(name);
1544 self.push(Step::Roll(StepArg::Variable(ParamArg::from_degrees(
1545 index, low, high,
1546 ))))
1547 }
1548 pub const fn forward_param(self, name: &str, low: f32, high: f32) -> Self {
1552 let index = self.expect_param_index(name);
1553 self.push(Step::Forward(StepArg::Variable(ParamArg::new(
1554 index, low, high,
1555 ))))
1556 }
1557 pub const fn left_param(self, name: &str, low: f32, high: f32) -> Self {
1560 let index = self.expect_param_index(name);
1561 self.push(Step::Left(StepArg::Variable(ParamArg::new(
1562 index, low, high,
1563 ))))
1564 }
1565 pub const fn up_param(self, name: &str, low: f32, high: f32) -> Self {
1568 let index = self.expect_param_index(name);
1569 self.push(Step::Up(StepArg::Variable(ParamArg::new(index, low, high))))
1570 }
1571 pub const fn disk_param(self, name: &str, low: f32, high: f32) -> Self {
1574 let index = self.expect_param_index(name);
1575 self.push(Step::DiskParam(ParamArg::new(index, low, high)))
1576 }
1577 pub const fn sphere_param(self, name: &str, low: f32, high: f32) -> Self {
1580 let index = self.expect_param_index(name);
1581 self.push(Step::SphereParam(ParamArg::new(index, low, high)))
1582 }
1583
1584 pub const fn restore(self, name: &'static str) -> Self {
1589 let index = match self.mark_index(name) {
1590 Some(i) => i,
1591 None => {
1592 panic!("restore: no mark found with name (mark must be defined before restore)")
1593 }
1594 };
1595 self.push(Step::Restore { index })
1596 }
1597 };
1598}
1599
1600#[cfg(feature = "alloc")]
1603macro_rules! emit_buf_step_methods {
1604 () => {
1605 pub fn yaw(self, degrees: f32) -> Self {
1608 self.push_step(Step::Yaw(StepArg::Fixed(degrees_to_radians(degrees))))
1609 }
1610 pub fn pitch(self, degrees: f32) -> Self {
1612 self.push_step(Step::Pitch(StepArg::Fixed(degrees_to_radians(degrees))))
1613 }
1614 pub fn roll(self, degrees: f32) -> Self {
1616 self.push_step(Step::Roll(StepArg::Fixed(degrees_to_radians(degrees))))
1617 }
1618 pub fn forward(self, distance: f32) -> Self {
1620 self.push_step(Step::Forward(StepArg::Fixed(distance)))
1621 }
1622 pub fn left(self, distance: f32) -> Self {
1624 self.push_step(Step::Left(StepArg::Fixed(distance)))
1625 }
1626 pub fn up(self, distance: f32) -> Self {
1628 self.push_step(Step::Up(StepArg::Fixed(distance)))
1629 }
1630 pub fn pen_up(self) -> Self {
1632 self.push_step(Step::PenUp)
1633 }
1634 pub fn pen_down(self) -> Self {
1636 self.push_step(Step::PenDown)
1637 }
1638 pub fn pen_color(self, color: Rgb888) -> Self {
1640 self.push_step(Step::PenColor(color))
1641 }
1642 pub fn pen_width(self, width: f32) -> Self {
1644 assert!(width >= 0.0, "pen width must be non-negative");
1645 self.push_step(Step::PenWidth(width))
1646 }
1647 pub fn disk(self, radius: f32) -> Self {
1649 self.push_step(Step::Disk(radius))
1650 }
1651 pub fn sphere(self, radius: f32) -> Self {
1653 self.push_step(Step::Sphere(radius))
1654 }
1655
1656 pub fn yaw_param(self, name: &str, low: f32, high: f32) -> Self {
1659 let index = self.expect_param_index(name);
1660 self.push_step(Step::Yaw(StepArg::Variable(ParamArg::from_degrees(
1661 index, low, high,
1662 ))))
1663 }
1664 pub fn pitch_param(self, name: &str, low: f32, high: f32) -> Self {
1666 let index = self.expect_param_index(name);
1667 self.push_step(Step::Pitch(StepArg::Variable(ParamArg::from_degrees(
1668 index, low, high,
1669 ))))
1670 }
1671 pub fn roll_param(self, name: &str, low: f32, high: f32) -> Self {
1673 let index = self.expect_param_index(name);
1674 self.push_step(Step::Roll(StepArg::Variable(ParamArg::from_degrees(
1675 index, low, high,
1676 ))))
1677 }
1678 pub fn forward_param(self, name: &str, low: f32, high: f32) -> Self {
1680 let index = self.expect_param_index(name);
1681 self.push_step(Step::Forward(StepArg::Variable(ParamArg::new(
1682 index, low, high,
1683 ))))
1684 }
1685 pub fn left_param(self, name: &str, low: f32, high: f32) -> Self {
1687 let index = self.expect_param_index(name);
1688 self.push_step(Step::Left(StepArg::Variable(ParamArg::new(
1689 index, low, high,
1690 ))))
1691 }
1692 pub fn up_param(self, name: &str, low: f32, high: f32) -> Self {
1694 let index = self.expect_param_index(name);
1695 self.push_step(Step::Up(StepArg::Variable(ParamArg::new(index, low, high))))
1696 }
1697 pub fn disk_param(self, name: &str, low: f32, high: f32) -> Self {
1699 let index = self.expect_param_index(name);
1700 self.push_step(Step::DiskParam(ParamArg::new(index, low, high)))
1701 }
1702 pub fn sphere_param(self, name: &str, low: f32, high: f32) -> Self {
1704 let index = self.expect_param_index(name);
1705 self.push_step(Step::SphereParam(ParamArg::new(index, low, high)))
1706 }
1707
1708 };
1710}
1711
1712#[doc(hidden)]
1715pub struct LinkageStepCount {
1716 step_count: usize,
1717 param_count: usize,
1718 mark_count: usize,
1719}
1720
1721impl LinkageStepCount {
1722 pub const fn start() -> Self {
1724 Self {
1725 step_count: 1,
1726 param_count: 0,
1727 mark_count: 0,
1728 }
1729 }
1730
1731 pub const fn step_count(self) -> usize {
1733 self.step_count
1734 }
1735
1736 pub const fn param_count(self) -> usize {
1738 self.param_count
1739 }
1740
1741 pub const fn mark_count(self) -> usize {
1743 self.mark_count
1744 }
1745
1746 const fn push(mut self) -> Self {
1747 self.step_count += 1;
1748 self
1749 }
1750
1751 pub const fn define_param(mut self, _name: &'static str, _default: f32) -> Self {
1752 self.param_count += 1;
1753 self
1754 }
1755
1756 pub const fn mark(mut self, _name: &'static str) -> Self {
1757 self.mark_count += 1;
1758 self.push()
1759 }
1760
1761 pub const fn restore(self, _name: &'static str) -> Self {
1762 self.push()
1763 }
1764
1765 pub const fn yaw(self, _degrees: f32) -> Self {
1766 self.push()
1767 }
1768 pub const fn pitch(self, _degrees: f32) -> Self {
1769 self.push()
1770 }
1771 pub const fn roll(self, _degrees: f32) -> Self {
1772 self.push()
1773 }
1774 pub const fn forward(self, _distance: f32) -> Self {
1775 self.push()
1776 }
1777 pub const fn left(self, _distance: f32) -> Self {
1778 self.push()
1779 }
1780 pub const fn up(self, _distance: f32) -> Self {
1781 self.push()
1782 }
1783 pub const fn pen_up(self) -> Self {
1784 self.push()
1785 }
1786 pub const fn pen_down(self) -> Self {
1787 self.push()
1788 }
1789 pub const fn pen_color(self, _color: Rgb888) -> Self {
1790 self.push()
1791 }
1792 pub const fn pen_width(self, _width: f32) -> Self {
1793 self.push()
1794 }
1795 pub const fn disk(self, _radius: f32) -> Self {
1796 self.push()
1797 }
1798 pub const fn sphere(self, _radius: f32) -> Self {
1799 self.push()
1800 }
1801 pub const fn yaw_param(self, _name: &str, _low: f32, _high: f32) -> Self {
1802 self.push()
1803 }
1804 pub const fn pitch_param(self, _name: &str, _low: f32, _high: f32) -> Self {
1805 self.push()
1806 }
1807 pub const fn roll_param(self, _name: &str, _low: f32, _high: f32) -> Self {
1808 self.push()
1809 }
1810 pub const fn forward_param(self, _name: &str, _low: f32, _high: f32) -> Self {
1811 self.push()
1812 }
1813 pub const fn left_param(self, _name: &str, _low: f32, _high: f32) -> Self {
1814 self.push()
1815 }
1816 pub const fn up_param(self, _name: &str, _low: f32, _high: f32) -> Self {
1817 self.push()
1818 }
1819 pub const fn disk_param(self, _name: &str, _low: f32, _high: f32) -> Self {
1820 self.push()
1821 }
1822 pub const fn sphere_param(self, _name: &str, _low: f32, _high: f32) -> Self {
1823 self.push()
1824 }
1825}
1826
1827pub struct LinkageFixed<const DOF: usize, const MARKS: usize, const N: usize> {
1914 steps: [Step; N],
1915 len: usize,
1916 params: [Param; DOF],
1917 param_len: usize,
1918 mark_names: [&'static str; MARKS],
1919 mark_len: usize,
1920}
1921
1922impl<const DOF: usize, const MARKS: usize, const N: usize> LinkageFixed<DOF, MARKS, N> {
1923 pub const fn start() -> Self {
1927 assert!(N > 0, "linkage must have room for the implicit start step");
1928 Self {
1929 steps: [const { Step::Start }; N],
1930 len: 1,
1931 params: [Param::EMPTY; DOF],
1932 param_len: 0,
1933 mark_names: [""; MARKS],
1934 mark_len: 0,
1935 }
1936 }
1937
1938 pub const DOF: usize = DOF;
1942
1943 pub const N: usize = N;
1947
1948 pub const MARKS: usize = MARKS;
1952
1953 #[must_use]
1961 #[inline]
1962 pub const fn view(&self) -> LinkageView<'_, DOF, MARKS> {
1963 LinkageView::new(
1964 &self.params,
1965 self.steps.split_at(self.len).0,
1966 &self.mark_names,
1967 self.mark_len,
1968 )
1969 }
1970
1971 #[must_use]
1975 pub const fn dof(&self) -> usize {
1976 DOF
1977 }
1978
1979 #[must_use]
1985 pub const fn step_count(&self) -> usize {
1986 self.len
1987 }
1988
1989 #[doc(hidden)]
1991 pub const fn len(&self) -> usize {
1992 self.len
1993 }
1994
1995 #[doc(hidden)]
1997 #[must_use]
1998 pub const fn is_empty(&self) -> bool {
1999 self.len == 0
2000 }
2001
2002 #[must_use]
2011 pub const fn param_count(&self) -> usize {
2012 self.param_len
2013 }
2014
2015 #[must_use]
2021 pub const fn mark_count(&self) -> usize {
2022 self.mark_len
2023 }
2024
2025 pub const fn define_param(mut self, name: &'static str, default: f32) -> Self {
2032 assert!(
2033 self.param_len < DOF,
2034 "DOF is too small; use a large DOF then call .param_count() to find the exact value"
2035 );
2036 assert!(default >= 0.0, "parameter default must be at least 0.0");
2037 assert!(default <= 1.0, "parameter default must be at most 1.0");
2038 self.params[self.param_len] = Param { name, default };
2039 self.param_len += 1;
2040 self
2041 }
2042
2043 emit_fixed_step_methods!();
2046
2047 pub const fn mark(mut self, name: &'static str) -> Self {
2051 let index = match self.mark_index(name) {
2052 Some(index) => index,
2053 None => {
2054 assert!(
2055 self.mark_len < MARKS,
2056 "MARKS is too small; use a large MARKS then call .mark_count() to find the exact value"
2057 );
2058 let index = self.mark_len;
2059 self.mark_names[index] = name;
2060 self.mark_len += 1;
2061 index
2062 }
2063 };
2064 self.push(Step::Mark { index })
2065 }
2066
2067 const fn mark_index(&self, name: &str) -> Option<usize> {
2068 let mut mark_index = 0;
2069 while mark_index < self.mark_len {
2070 if str_eq(self.mark_names[mark_index], name) {
2071 return Some(mark_index);
2072 }
2073 mark_index += 1;
2074 }
2075 None
2076 }
2077
2078 const fn push(mut self, step: Step) -> Self {
2079 assert!(
2080 self.len < N,
2081 "N is too small; use a large N then call .step_count() to find the exact value"
2082 );
2083 self.steps[self.len] = step;
2084 self.len += 1;
2085 self
2086 }
2087
2088 const fn last_param_index(&self, name: &str) -> Option<usize> {
2092 let mut i = self.param_len;
2093 while i > 0 {
2094 i -= 1;
2095 if str_eq(self.params[i].name, name) {
2096 return Some(i);
2097 }
2098 }
2099 None
2100 }
2101
2102 const fn expect_param_index(&self, name: &str) -> usize {
2103 match self.last_param_index(name) {
2104 Some(index) => index,
2105 None => panic!("unknown parameter name"),
2106 }
2107 }
2108
2109 pub const fn freeze_param_index<const OUT_DOF: usize>(
2130 self,
2131 param_index: usize,
2132 raw_value: f32,
2133 ) -> LinkageFixed<OUT_DOF, MARKS, N> {
2134 let mut is_frozen = [false; DOF];
2135 let frozen_at_default = [false; DOF];
2136 let mut frozen_raw = [0.0f32; DOF];
2137
2138 assert!(
2139 param_index < self.param_len,
2140 "freeze param index out of bounds"
2141 );
2142 is_frozen[param_index] = true;
2143 frozen_raw[param_index] = raw_value;
2144
2145 self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
2146 }
2147
2148 pub const fn freeze_param_name<const OUT_DOF: usize>(
2167 self,
2168 name: &'static str,
2169 raw_value: f32,
2170 ) -> LinkageFixed<OUT_DOF, MARKS, N> {
2171 let mut found_index = 0usize;
2172 let mut found_count = 0usize;
2173 let mut param_index = 0;
2174 while param_index < self.param_len {
2175 if str_eq(self.params[param_index].name, name) {
2176 found_index = param_index;
2177 found_count += 1;
2178 }
2179 param_index += 1;
2180 }
2181 assert!(found_count > 0, "freeze name not found in params");
2182 assert!(found_count == 1, "freeze name is ambiguous");
2183
2184 self.freeze_param_index(found_index, raw_value)
2185 }
2186
2187 pub const fn freeze_param_index_at_default<const OUT_DOF: usize>(
2195 self,
2196 param_index: usize,
2197 ) -> LinkageFixed<OUT_DOF, MARKS, N> {
2198 let mut is_frozen = [false; DOF];
2199 let mut frozen_at_default = [false; DOF];
2200 let frozen_raw = [0.0f32; DOF];
2201
2202 assert!(
2203 param_index < self.param_len,
2204 "freeze param index out of bounds"
2205 );
2206 is_frozen[param_index] = true;
2207 frozen_at_default[param_index] = true;
2208
2209 self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
2210 }
2211
2212 pub const fn retain_param_indexes<const OUT_DOF: usize>(
2234 self,
2235 indexes: &[usize],
2236 ) -> LinkageFixed<OUT_DOF, MARKS, N> {
2237 let mut index_index = 0;
2238 while index_index < indexes.len() {
2239 let retain_index = indexes[index_index];
2240 assert!(
2241 retain_index < self.param_len,
2242 "retain param index out of bounds"
2243 );
2244 let mut previous_index = 0;
2245 while previous_index < index_index {
2246 assert!(
2247 indexes[previous_index] != retain_index,
2248 "duplicate index in retain list"
2249 );
2250 previous_index += 1;
2251 }
2252 index_index += 1;
2253 }
2254
2255 let mut is_frozen = [false; DOF];
2256 let mut frozen_at_default = [false; DOF];
2257 let frozen_raw = [0.0f32; DOF];
2258
2259 let mut param_index = 0;
2260 while param_index < self.param_len {
2261 let mut found = false;
2262 let mut index_index = 0;
2263 while index_index < indexes.len() {
2264 if param_index == indexes[index_index] {
2265 found = true;
2266 break;
2267 }
2268 index_index += 1;
2269 }
2270 if !found {
2271 is_frozen[param_index] = true;
2272 frozen_at_default[param_index] = true;
2273 }
2274 param_index += 1;
2275 }
2276
2277 self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
2278 }
2279
2280 pub const fn retain_param_names<const OUT_DOF: usize>(
2288 self,
2289 names: &[&'static str],
2290 ) -> LinkageFixed<OUT_DOF, MARKS, N> {
2291 let mut name_index = 0;
2292 while name_index < names.len() {
2293 let retain_name = names[name_index];
2294 let mut previous_name_index = 0;
2295 while previous_name_index < name_index {
2296 assert!(
2297 !str_eq(names[previous_name_index], retain_name),
2298 "duplicate name in retain list"
2299 );
2300 previous_name_index += 1;
2301 }
2302
2303 let mut found = false;
2304 let mut param_index = 0;
2305 while param_index < self.param_len {
2306 if str_eq(self.params[param_index].name, retain_name) {
2307 found = true;
2308 break;
2309 }
2310 param_index += 1;
2311 }
2312 assert!(found, "retain name not found in params");
2313 name_index += 1;
2314 }
2315
2316 let mut is_frozen = [false; DOF];
2317 let mut frozen_at_default = [false; DOF];
2318 let frozen_raw = [0.0f32; DOF];
2319
2320 let mut param_index = 0;
2321 while param_index < self.param_len {
2322 let mut found = false;
2323 let mut name_index = 0;
2324 while name_index < names.len() {
2325 if str_eq(self.params[param_index].name, names[name_index]) {
2326 found = true;
2327 break;
2328 }
2329 name_index += 1;
2330 }
2331 if !found {
2332 is_frozen[param_index] = true;
2333 frozen_at_default[param_index] = true;
2334 }
2335 param_index += 1;
2336 }
2337
2338 self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
2339 }
2340
2341 const fn freeze_with_map<const OUT_DOF: usize>(
2342 self,
2343 is_frozen: [bool; DOF],
2344 frozen_at_default: [bool; DOF],
2345 frozen_raw: [f32; DOF],
2346 ) -> LinkageFixed<OUT_DOF, MARKS, N> {
2347 let mut new_param_index = [0usize; DOF];
2348 let mut new_param_len = 0usize;
2349
2350 let mut param_index = 0;
2351 while param_index < self.param_len {
2352 if !is_frozen[param_index] {
2353 new_param_index[param_index] = new_param_len;
2354 new_param_len += 1;
2355 }
2356 param_index += 1;
2357 }
2358
2359 assert!(
2360 new_param_len == OUT_DOF,
2361 "OUT_DOF must equal DOF minus the number of frozen parameters"
2362 );
2363
2364 let mut out = LinkageFixed {
2365 steps: [const { Step::Start }; N],
2366 len: 0,
2367 params: [Param::EMPTY; OUT_DOF],
2368 param_len: 0,
2369 mark_names: [""; MARKS],
2370 mark_len: self.mark_len,
2371 };
2372
2373 let mut mark_index = 0;
2374 while mark_index < self.mark_len {
2375 out.mark_names[mark_index] = self.mark_names[mark_index];
2376 mark_index += 1;
2377 }
2378
2379 let mut param_index = 0;
2380 while param_index < self.param_len {
2381 if !is_frozen[param_index] {
2382 out.params[new_param_index[param_index]] = self.params[param_index];
2383 out.param_len += 1;
2384 }
2385 param_index += 1;
2386 }
2387
2388 let mut step_index = 0;
2389 while step_index < self.len {
2390 out.steps[step_index] = rewrite_step_for_freeze(
2391 self.steps[step_index],
2392 &is_frozen,
2393 &frozen_at_default,
2394 &frozen_raw,
2395 &self.params,
2396 &new_param_index,
2397 );
2398 step_index += 1;
2399 }
2400 out.len = self.len;
2401
2402 out.strip_fixed_noops_same_capacity()
2403 .merge_adjacent_fixed_same_capacity()
2404 .strip_fixed_noops_same_capacity()
2405 }
2406
2407 #[doc(hidden)]
2411 #[doc(hidden)]
2413 const fn strip_fixed_noops_same_capacity(self) -> Self {
2414 let mut out_steps = [const { Step::Start }; N];
2415 let mut out_len = 0usize;
2416 let mut step_index = 0;
2417 while step_index < self.len {
2418 let step = self.steps[step_index];
2419 if !is_fixed_noop(step) {
2420 out_steps[out_len] = step;
2421 out_len += 1;
2422 }
2423 step_index += 1;
2424 }
2425 Self {
2426 steps: out_steps,
2427 len: out_len,
2428 params: self.params,
2429 param_len: self.param_len,
2430 mark_names: self.mark_names,
2431 mark_len: self.mark_len,
2432 }
2433 }
2434
2435 const fn merge_adjacent_fixed_same_capacity(self) -> Self {
2436 let mut out_steps = [const { Step::Start }; N];
2437 let mut out_len = 0usize;
2438 let mut i = 0;
2439 while i < self.len {
2440 let step = self.steps[i];
2441 i += 1;
2442 let merged = match step {
2443 Step::Yaw(StepArg::Fixed(v)) => {
2444 let mut total = v;
2445 while i < self.len {
2446 if let Step::Yaw(StepArg::Fixed(v2)) = self.steps[i] {
2447 total += v2;
2448 i += 1;
2449 } else {
2450 break;
2451 }
2452 }
2453 Step::Yaw(StepArg::Fixed(total))
2454 }
2455 Step::Pitch(StepArg::Fixed(v)) => {
2456 let mut total = v;
2457 while i < self.len {
2458 if let Step::Pitch(StepArg::Fixed(v2)) = self.steps[i] {
2459 total += v2;
2460 i += 1;
2461 } else {
2462 break;
2463 }
2464 }
2465 Step::Pitch(StepArg::Fixed(total))
2466 }
2467 Step::Roll(StepArg::Fixed(v)) => {
2468 let mut total = v;
2469 while i < self.len {
2470 if let Step::Roll(StepArg::Fixed(v2)) = self.steps[i] {
2471 total += v2;
2472 i += 1;
2473 } else {
2474 break;
2475 }
2476 }
2477 Step::Roll(StepArg::Fixed(total))
2478 }
2479 Step::Forward(StepArg::Fixed(v)) => {
2480 let mut total = v;
2481 while i < self.len {
2482 if let Step::Forward(StepArg::Fixed(v2)) = self.steps[i] {
2483 total += v2;
2484 i += 1;
2485 } else {
2486 break;
2487 }
2488 }
2489 Step::Forward(StepArg::Fixed(total))
2490 }
2491 Step::Left(StepArg::Fixed(v)) => {
2492 let mut total = v;
2493 while i < self.len {
2494 if let Step::Left(StepArg::Fixed(v2)) = self.steps[i] {
2495 total += v2;
2496 i += 1;
2497 } else {
2498 break;
2499 }
2500 }
2501 Step::Left(StepArg::Fixed(total))
2502 }
2503 Step::Up(StepArg::Fixed(v)) => {
2504 let mut total = v;
2505 while i < self.len {
2506 if let Step::Up(StepArg::Fixed(v2)) = self.steps[i] {
2507 total += v2;
2508 i += 1;
2509 } else {
2510 break;
2511 }
2512 }
2513 Step::Up(StepArg::Fixed(total))
2514 }
2515 other => other,
2516 };
2517 out_steps[out_len] = merged;
2518 out_len += 1;
2519 }
2520 Self {
2521 steps: out_steps,
2522 len: out_len,
2523 params: self.params,
2524 param_len: self.param_len,
2525 mark_names: self.mark_names,
2526 mark_len: self.mark_len,
2527 }
2528 }
2529
2530 pub const fn combine<
2556 const DOF2: usize,
2557 const MARKS2: usize,
2558 const DOF_OUT: usize,
2559 const MARKS_OUT: usize,
2560 const N_OUT: usize,
2561 >(
2562 self,
2563 other: LinkageView<'_, DOF2, MARKS2>,
2564 ) -> LinkageFixed<DOF_OUT, MARKS_OUT, N_OUT> {
2565 self.view().combine_fixed(other)
2566 }
2567}
2568
2569#[cfg(feature = "alloc")]
2570#[derive(Clone)]
2615pub struct LinkageBuf<const DOF: usize, const MARKS: usize> {
2616 params: [Param; DOF],
2617 param_len: usize,
2618 steps: alloc::vec::Vec<Step>,
2619 mark_names: [&'static str; MARKS],
2620 mark_len: usize,
2621}
2622
2623#[cfg(feature = "alloc")]
2624impl<const DOF: usize, const MARKS: usize> LinkageBuf<DOF, MARKS> {
2625 pub fn start() -> Self {
2629 Self {
2630 params: [Param::EMPTY; DOF],
2631 param_len: 0,
2632 steps: alloc::vec![Step::Start],
2633 mark_names: [""; MARKS],
2634 mark_len: 0,
2635 }
2636 }
2637
2638 pub fn from_lb_rs(source: &str) -> Result<Self, String> {
2646 parse_lb_rs(source)
2647 }
2648
2649 pub const DOF: usize = DOF;
2651
2652 pub const MARKS: usize = MARKS;
2654
2655 #[must_use]
2663 #[inline]
2664 pub fn view(&self) -> LinkageView<'_, DOF, MARKS> {
2665 LinkageView::new(&self.params, &self.steps, &self.mark_names, self.mark_len)
2666 }
2667
2668 pub fn define_param(mut self, name: &'static str, default: f32) -> Self {
2673 assert!(self.param_len < DOF, "linkage has more params than DOF");
2674 assert!(default >= 0.0, "parameter default must be at least 0.0");
2675 assert!(default <= 1.0, "parameter default must be at most 1.0");
2676 self.params[self.param_len] = Param { name, default };
2677 self.param_len += 1;
2678 self
2679 }
2680
2681 emit_buf_step_methods!();
2684
2685 pub fn mark(mut self, name: &'static str) -> Self {
2687 let index = match self.mark_index(name) {
2688 Some(index) => index,
2689 None => {
2690 assert!(self.mark_len < MARKS, "linkage has more marks than MARKS");
2691 let index = self.mark_len;
2692 self.mark_names[index] = name;
2693 self.mark_len += 1;
2694 index
2695 }
2696 };
2697 self.push_step_internal(Step::Mark { index });
2698 self
2699 }
2700
2701 pub fn restore(self, name: &'static str) -> Self {
2704 let index = match self.mark_index(name) {
2705 Some(i) => i,
2706 None => {
2707 panic!("restore: no mark found with name (mark must be defined before restore)")
2708 }
2709 };
2710 self.push_step(Step::Restore { index })
2711 }
2712
2713 fn push_step(mut self, step: Step) -> Self {
2714 self.steps.push(step);
2715 self
2716 }
2717
2718 fn push_step_internal(&mut self, step: Step) {
2719 self.steps.push(step);
2720 }
2721
2722 fn mark_index(&self, name: &str) -> Option<usize> {
2723 let mut mark_index = 0;
2724 while mark_index < self.mark_len {
2725 if str_eq(self.mark_names[mark_index], name) {
2726 return Some(mark_index);
2727 }
2728 mark_index += 1;
2729 }
2730 None
2731 }
2732
2733 fn last_param_index(&self, name: &str) -> Option<usize> {
2734 let mut i = self.param_len;
2735 while i > 0 {
2736 i -= 1;
2737 if str_eq(self.params[i].name, name) {
2738 return Some(i);
2739 }
2740 }
2741 None
2742 }
2743
2744 fn expect_param_index(&self, name: &str) -> usize {
2745 match self.last_param_index(name) {
2746 Some(index) => index,
2747 None => panic!("unknown parameter name"),
2748 }
2749 }
2750
2751 pub fn combine<
2781 const DOF2: usize,
2782 const MARKS2: usize,
2783 const DOF_OUT: usize,
2784 const MARKS_OUT: usize,
2785 >(
2786 self,
2787 other: LinkageView<'_, DOF2, MARKS2>,
2788 ) -> LinkageBuf<DOF_OUT, MARKS_OUT> {
2789 assert!(DOF_OUT == DOF + DOF2, "DOF_OUT must equal DOF + DOF2");
2790 assert!(
2791 MARKS_OUT >= self.mark_len + other.mark_len,
2792 "MARKS_OUT must fit all marks from both linkages"
2793 );
2794
2795 let mut out = LinkageBuf {
2796 params: [Param::EMPTY; DOF_OUT],
2797 param_len: 0,
2798 steps: alloc::vec::Vec::new(),
2799 mark_names: [""; MARKS_OUT],
2800 mark_len: 0,
2801 };
2802
2803 out.steps.extend_from_slice(&self.steps);
2805
2806 let mark_offset = self.mark_len;
2808 for i in 1..other.steps.len() {
2809 let step = other.steps[i].offset_params(DOF, mark_offset);
2810 out.steps.push(step);
2811 }
2812
2813 let mut i = 0;
2815 while i < self.param_len {
2816 out.params[i] = self.params[i];
2817 i += 1;
2818 }
2819
2820 let mut i = 0;
2822 while i < DOF2 {
2823 out.params[DOF + i] = other.params[i];
2824 i += 1;
2825 }
2826 out.param_len = self.param_len + DOF2;
2827
2828 let mut i = 0;
2829 while i < self.mark_len {
2830 out.mark_names[i] = self.mark_names[i];
2831 i += 1;
2832 }
2833 let mut i = 0;
2834 while i < other.mark_len {
2835 out.mark_names[self.mark_len + i] = other.mark_names[i];
2836 i += 1;
2837 }
2838 out.mark_len = self.mark_len + other.mark_len;
2839
2840 out
2841 }
2842}
2843
2844#[cfg(feature = "alloc")]
2845impl<const DOF: usize, const MARKS: usize> LinkageBuf<DOF, MARKS> {
2846 pub fn freeze_param_index<const OUT_DOF: usize>(
2856 self,
2857 param_index: usize,
2858 raw_value: f32,
2859 ) -> LinkageBuf<OUT_DOF, MARKS> {
2860 let mut is_frozen = [false; DOF];
2861 let frozen_at_default = [false; DOF];
2862 let mut frozen_raw = [0.0f32; DOF];
2863
2864 assert!(
2865 param_index < self.param_len,
2866 "freeze param index out of bounds"
2867 );
2868 is_frozen[param_index] = true;
2869 frozen_raw[param_index] = raw_value;
2870
2871 self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
2872 }
2873
2874 pub fn freeze_param_name<const OUT_DOF: usize>(
2880 self,
2881 name: &'static str,
2882 raw_value: f32,
2883 ) -> LinkageBuf<OUT_DOF, MARKS> {
2884 let mut found_index = 0usize;
2885 let mut found_count = 0usize;
2886 let mut param_index = 0;
2887 while param_index < self.param_len {
2888 if str_eq(self.params[param_index].name, name) {
2889 found_index = param_index;
2890 found_count += 1;
2891 }
2892 param_index += 1;
2893 }
2894 assert!(found_count > 0, "freeze name not found in params");
2895 assert!(found_count == 1, "freeze name is ambiguous");
2896
2897 self.freeze_param_index(found_index, raw_value)
2898 }
2899
2900 pub fn freeze_param_index_at_default<const OUT_DOF: usize>(
2902 self,
2903 param_index: usize,
2904 ) -> LinkageBuf<OUT_DOF, MARKS> {
2905 let mut is_frozen = [false; DOF];
2906 let mut frozen_at_default = [false; DOF];
2907 let frozen_raw = [0.0f32; DOF];
2908
2909 assert!(
2910 param_index < self.param_len,
2911 "freeze param index out of bounds"
2912 );
2913 is_frozen[param_index] = true;
2914 frozen_at_default[param_index] = true;
2915
2916 self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
2917 }
2918
2919 pub fn retain_param_indexes<const OUT_DOF: usize>(
2923 self,
2924 indexes: &[usize],
2925 ) -> LinkageBuf<OUT_DOF, MARKS> {
2926 let mut index_index = 0;
2927 while index_index < indexes.len() {
2928 let retain_index = indexes[index_index];
2929 assert!(
2930 retain_index < self.param_len,
2931 "retain param index out of bounds"
2932 );
2933 let mut previous_index = 0;
2934 while previous_index < index_index {
2935 assert!(
2936 indexes[previous_index] != retain_index,
2937 "duplicate index in retain list"
2938 );
2939 previous_index += 1;
2940 }
2941 index_index += 1;
2942 }
2943
2944 let mut is_frozen = [false; DOF];
2945 let mut frozen_at_default = [false; DOF];
2946 let frozen_raw = [0.0f32; DOF];
2947
2948 let mut param_index = 0;
2949 while param_index < self.param_len {
2950 let mut found = false;
2951 let mut index_index = 0;
2952 while index_index < indexes.len() {
2953 if param_index == indexes[index_index] {
2954 found = true;
2955 break;
2956 }
2957 index_index += 1;
2958 }
2959 if !found {
2960 is_frozen[param_index] = true;
2961 frozen_at_default[param_index] = true;
2962 }
2963 param_index += 1;
2964 }
2965
2966 self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
2967 }
2968
2969 pub fn retain_param_names<const OUT_DOF: usize>(
2975 self,
2976 names: &[&'static str],
2977 ) -> LinkageBuf<OUT_DOF, MARKS> {
2978 let mut ni = 0;
2979 while ni < names.len() {
2980 let retain_name = names[ni];
2981 let mut ni2 = 0;
2982 while ni2 < ni {
2983 assert!(
2984 !str_eq(names[ni2], retain_name),
2985 "duplicate name in retain list"
2986 );
2987 ni2 += 1;
2988 }
2989 let mut found = false;
2990 let mut pi = 0;
2991 while pi < self.param_len {
2992 if str_eq(self.params[pi].name, retain_name) {
2993 found = true;
2994 break;
2995 }
2996 pi += 1;
2997 }
2998 assert!(found, "retain name not found in params");
2999 ni += 1;
3000 }
3001
3002 let mut is_frozen = [false; DOF];
3003 let mut frozen_at_default = [false; DOF];
3004 let frozen_raw = [0.0f32; DOF];
3005
3006 let mut param_index = 0;
3007 while param_index < self.param_len {
3008 let mut found = false;
3009 let mut name_index = 0;
3010 while name_index < names.len() {
3011 if str_eq(self.params[param_index].name, names[name_index]) {
3012 found = true;
3013 break;
3014 }
3015 name_index += 1;
3016 }
3017 if !found {
3018 is_frozen[param_index] = true;
3019 frozen_at_default[param_index] = true;
3020 }
3021 param_index += 1;
3022 }
3023
3024 self.freeze_with_map(is_frozen, frozen_at_default, frozen_raw)
3025 }
3026
3027 fn freeze_with_map<const OUT_DOF: usize>(
3028 self,
3029 is_frozen: [bool; DOF],
3030 frozen_at_default: [bool; DOF],
3031 frozen_raw: [f32; DOF],
3032 ) -> LinkageBuf<OUT_DOF, MARKS> {
3033 let mut new_param_index = [0usize; DOF];
3034 let mut new_param_len = 0usize;
3035 let mut param_index = 0;
3036 while param_index < self.param_len {
3037 if !is_frozen[param_index] {
3038 new_param_index[param_index] = new_param_len;
3039 new_param_len += 1;
3040 }
3041 param_index += 1;
3042 }
3043
3044 assert!(
3045 new_param_len == OUT_DOF,
3046 "OUT_DOF must equal DOF minus the number of frozen parameters"
3047 );
3048
3049 let mut out = LinkageBuf {
3050 params: [Param::EMPTY; OUT_DOF],
3051 param_len: 0,
3052 steps: Vec::new(),
3053 mark_names: [""; MARKS],
3054 mark_len: self.mark_len,
3055 };
3056
3057 let mut mark_index = 0;
3058 while mark_index < self.mark_len {
3059 out.mark_names[mark_index] = self.mark_names[mark_index];
3060 mark_index += 1;
3061 }
3062
3063 let mut param_index = 0;
3064 while param_index < self.param_len {
3065 if !is_frozen[param_index] {
3066 out.params[new_param_index[param_index]] = self.params[param_index];
3067 out.param_len += 1;
3068 }
3069 param_index += 1;
3070 }
3071
3072 out.steps = self
3073 .steps
3074 .into_iter()
3075 .map(|step| {
3076 rewrite_step_for_freeze(
3077 step,
3078 &is_frozen,
3079 &frozen_at_default,
3080 &frozen_raw,
3081 &self.params,
3082 &new_param_index,
3083 )
3084 })
3085 .collect();
3086
3087 out.strip_fixed_noops()
3088 .merge_adjacent_fixed()
3089 .strip_fixed_noops()
3090 }
3091
3092 pub(crate) fn strip_fixed_noops(mut self) -> Self {
3103 self.steps.retain(|&step| !is_fixed_noop(step));
3104 self
3105 }
3106
3107 pub(crate) fn merge_adjacent_fixed(self) -> Self {
3119 let mut out = Vec::with_capacity(self.steps.len());
3120 let mut i = 0;
3121 while i < self.steps.len() {
3122 let step = self.steps[i];
3123 i += 1;
3124 let merged = match step {
3125 Step::Yaw(StepArg::Fixed(v)) => {
3126 let mut total = v;
3127 while i < self.steps.len() {
3128 if let Step::Yaw(StepArg::Fixed(v2)) = self.steps[i] {
3129 total += v2;
3130 i += 1;
3131 } else {
3132 break;
3133 }
3134 }
3135 Step::Yaw(StepArg::Fixed(total))
3136 }
3137 Step::Pitch(StepArg::Fixed(v)) => {
3138 let mut total = v;
3139 while i < self.steps.len() {
3140 if let Step::Pitch(StepArg::Fixed(v2)) = self.steps[i] {
3141 total += v2;
3142 i += 1;
3143 } else {
3144 break;
3145 }
3146 }
3147 Step::Pitch(StepArg::Fixed(total))
3148 }
3149 Step::Roll(StepArg::Fixed(v)) => {
3150 let mut total = v;
3151 while i < self.steps.len() {
3152 if let Step::Roll(StepArg::Fixed(v2)) = self.steps[i] {
3153 total += v2;
3154 i += 1;
3155 } else {
3156 break;
3157 }
3158 }
3159 Step::Roll(StepArg::Fixed(total))
3160 }
3161 Step::Forward(StepArg::Fixed(v)) => {
3162 let mut total = v;
3163 while i < self.steps.len() {
3164 if let Step::Forward(StepArg::Fixed(v2)) = self.steps[i] {
3165 total += v2;
3166 i += 1;
3167 } else {
3168 break;
3169 }
3170 }
3171 Step::Forward(StepArg::Fixed(total))
3172 }
3173 Step::Left(StepArg::Fixed(v)) => {
3174 let mut total = v;
3175 while i < self.steps.len() {
3176 if let Step::Left(StepArg::Fixed(v2)) = self.steps[i] {
3177 total += v2;
3178 i += 1;
3179 } else {
3180 break;
3181 }
3182 }
3183 Step::Left(StepArg::Fixed(total))
3184 }
3185 Step::Up(StepArg::Fixed(v)) => {
3186 let mut total = v;
3187 while i < self.steps.len() {
3188 if let Step::Up(StepArg::Fixed(v2)) = self.steps[i] {
3189 total += v2;
3190 i += 1;
3191 } else {
3192 break;
3193 }
3194 }
3195 Step::Up(StepArg::Fixed(total))
3196 }
3197 other => other,
3198 };
3199 out.push(merged);
3200 }
3201 Self {
3202 steps: out,
3203 params: self.params,
3204 param_len: self.param_len,
3205 mark_names: self.mark_names,
3206 mark_len: self.mark_len,
3207 }
3208 }
3209}
3210
3211#[cfg(feature = "alloc")]
3212impl<const DOF: usize, const MARKS: usize, const N: usize> From<&LinkageFixed<DOF, MARKS, N>>
3213 for LinkageBuf<DOF, MARKS>
3214{
3215 fn from(linkage: &LinkageFixed<DOF, MARKS, N>) -> Self {
3216 Self {
3217 params: linkage.params,
3218 param_len: linkage.param_len,
3219 steps: linkage.steps[..linkage.len].to_vec(),
3220 mark_names: linkage.mark_names,
3221 mark_len: linkage.mark_len,
3222 }
3223 }
3224}
3225
3226#[cfg(feature = "alloc")]
3227impl<'a, const DOF: usize, const MARKS: usize> From<&'a LinkageBuf<DOF, MARKS>>
3228 for LinkageView<'a, DOF, MARKS>
3229{
3230 fn from(linkage: &'a LinkageBuf<DOF, MARKS>) -> Self {
3231 linkage.view()
3232 }
3233}
3234
3235impl Step {
3236 const fn offset_params(self, param_offset: usize, remember_offset: usize) -> Self {
3237 match self {
3238 Self::Yaw(arg) => Self::Yaw(arg.offset_param(param_offset)),
3239 Self::Pitch(arg) => Self::Pitch(arg.offset_param(param_offset)),
3240 Self::Roll(arg) => Self::Roll(arg.offset_param(param_offset)),
3241 Self::Forward(arg) => Self::Forward(arg.offset_param(param_offset)),
3242 Self::Left(arg) => Self::Left(arg.offset_param(param_offset)),
3243 Self::Up(arg) => Self::Up(arg.offset_param(param_offset)),
3244 Self::DiskParam(v) => Self::DiskParam(v.offset(param_offset)),
3245 Self::SphereParam(v) => Self::SphereParam(v.offset(param_offset)),
3246 Self::Mark { index } => Self::Mark {
3247 index: index + remember_offset,
3248 },
3249 Self::Restore { index } => Self::Restore {
3250 index: index + remember_offset,
3251 },
3252 other => other,
3253 }
3254 }
3255}
3256
3257const fn str_eq(left: &str, right: &str) -> bool {
3258 let left = left.as_bytes();
3259 let right = right.as_bytes();
3260
3261 if left.len() != right.len() {
3262 return false;
3263 }
3264
3265 let mut byte_index = 0;
3266 while byte_index < left.len() {
3267 if left[byte_index] != right[byte_index] {
3268 return false;
3269 }
3270 byte_index += 1;
3271 }
3272
3273 true
3274}
3275
3276const fn is_fixed_noop(step: Step) -> bool {
3277 matches!(
3278 step,
3279 Step::Yaw(StepArg::Fixed(v))
3280 | Step::Pitch(StepArg::Fixed(v))
3281 | Step::Roll(StepArg::Fixed(v))
3282 | Step::Forward(StepArg::Fixed(v))
3283 | Step::Left(StepArg::Fixed(v))
3284 | Step::Up(StepArg::Fixed(v))
3285 if v == 0.0
3286 )
3287}
3288
3289const fn rewrite_arg_for_freeze(
3290 arg: StepArg,
3291 is_frozen: &[bool],
3292 frozen_at_default: &[bool],
3293 frozen_raw: &[f32],
3294 params: &[Param],
3295 new_param_index: &[usize],
3296 is_rotation: bool,
3297) -> StepArg {
3298 match arg {
3299 StepArg::Fixed(_) => arg,
3300 StepArg::Variable(variable_arg) => {
3301 if is_frozen[variable_arg.index] {
3302 let physical = frozen_physical_value(
3303 variable_arg,
3304 frozen_at_default,
3305 frozen_raw,
3306 params,
3307 is_rotation,
3308 );
3309 StepArg::Fixed(physical)
3310 } else {
3311 StepArg::Variable(ParamArg {
3312 index: new_param_index[variable_arg.index],
3313 low: variable_arg.low,
3314 span: variable_arg.span,
3315 })
3316 }
3317 }
3318 }
3319}
3320
3321const fn rewrite_step_for_freeze(
3322 step: Step,
3323 is_frozen: &[bool],
3324 frozen_at_default: &[bool],
3325 frozen_raw: &[f32],
3326 params: &[Param],
3327 new_param_index: &[usize],
3328) -> Step {
3329 match step {
3330 Step::Yaw(arg) => Step::Yaw(rewrite_arg_for_freeze(
3331 arg,
3332 is_frozen,
3333 frozen_at_default,
3334 frozen_raw,
3335 params,
3336 new_param_index,
3337 true,
3338 )),
3339 Step::Pitch(arg) => Step::Pitch(rewrite_arg_for_freeze(
3340 arg,
3341 is_frozen,
3342 frozen_at_default,
3343 frozen_raw,
3344 params,
3345 new_param_index,
3346 true,
3347 )),
3348 Step::Roll(arg) => Step::Roll(rewrite_arg_for_freeze(
3349 arg,
3350 is_frozen,
3351 frozen_at_default,
3352 frozen_raw,
3353 params,
3354 new_param_index,
3355 true,
3356 )),
3357 Step::Forward(arg) => Step::Forward(rewrite_arg_for_freeze(
3358 arg,
3359 is_frozen,
3360 frozen_at_default,
3361 frozen_raw,
3362 params,
3363 new_param_index,
3364 false,
3365 )),
3366 Step::Left(arg) => Step::Left(rewrite_arg_for_freeze(
3367 arg,
3368 is_frozen,
3369 frozen_at_default,
3370 frozen_raw,
3371 params,
3372 new_param_index,
3373 false,
3374 )),
3375 Step::Up(arg) => Step::Up(rewrite_arg_for_freeze(
3376 arg,
3377 is_frozen,
3378 frozen_at_default,
3379 frozen_raw,
3380 params,
3381 new_param_index,
3382 false,
3383 )),
3384 Step::DiskParam(variable_arg) => {
3385 if is_frozen[variable_arg.index] {
3386 let physical = frozen_physical_value(
3387 variable_arg,
3388 frozen_at_default,
3389 frozen_raw,
3390 params,
3391 false,
3392 );
3393 Step::Disk(physical)
3394 } else {
3395 Step::DiskParam(ParamArg {
3396 index: new_param_index[variable_arg.index],
3397 low: variable_arg.low,
3398 span: variable_arg.span,
3399 })
3400 }
3401 }
3402 Step::SphereParam(variable_arg) => {
3403 if is_frozen[variable_arg.index] {
3404 let physical = frozen_physical_value(
3405 variable_arg,
3406 frozen_at_default,
3407 frozen_raw,
3408 params,
3409 false,
3410 );
3411 Step::Sphere(physical)
3412 } else {
3413 Step::SphereParam(ParamArg {
3414 index: new_param_index[variable_arg.index],
3415 low: variable_arg.low,
3416 span: variable_arg.span,
3417 })
3418 }
3419 }
3420 other => other,
3421 }
3422}
3423
3424const fn frozen_physical_value(
3425 variable_arg: ParamArg,
3426 frozen_at_default: &[bool],
3427 frozen_raw: &[f32],
3428 params: &[Param],
3429 is_rotation: bool,
3430) -> f32 {
3431 if frozen_at_default[variable_arg.index] {
3432 variable_arg.low + params[variable_arg.index].default * variable_arg.span
3433 } else {
3434 let raw_value = frozen_raw[variable_arg.index];
3435 let physical = if is_rotation {
3436 degrees_to_radians(raw_value)
3437 } else {
3438 raw_value
3439 };
3440 assert_raw_value_in_range(variable_arg, physical);
3441 physical
3442 }
3443}
3444
3445const fn assert_raw_value_in_range(variable_arg: ParamArg, physical: f32) {
3446 let high = variable_arg.low + variable_arg.span;
3447 let (min, max) = if variable_arg.low <= high {
3448 (variable_arg.low, high)
3449 } else {
3450 (high, variable_arg.low)
3451 };
3452 assert!(
3453 physical >= min && physical <= max,
3454 "raw freeze value out of range"
3455 );
3456}
3457
3458fn validate_params<const DOF: usize>(params: &[f32; DOF]) -> Result<(), Error> {
3459 for (index, &value) in params.iter().enumerate() {
3460 if !(0.0..=1.0).contains(&value) {
3461 return Err(Error::InvalidParameter { index, value });
3462 }
3463 }
3464
3465 Ok(())
3466}
3467
3468fn rotation_matrix<const DOF: usize>(step: &Step, params: &[f32; DOF]) -> Mat3 {
3469 let radians = match step {
3470 Step::Yaw(arg) | Step::Pitch(arg) | Step::Roll(arg) => arg.resolve(params),
3471 Step::Start
3472 | Step::Forward(_)
3473 | Step::Left(_)
3474 | Step::Up(_)
3475 | Step::PenUp
3476 | Step::PenDown
3477 | Step::PenColor(_)
3478 | Step::PenWidth(_)
3479 | Step::Disk(_)
3480 | Step::DiskParam(_)
3481 | Step::Sphere(_)
3482 | Step::SphereParam(_)
3483 | Step::Mark { .. }
3484 | Step::Restore { .. } => return Mat3::IDENTITY,
3485 };
3486 match step {
3487 Step::Yaw(_) => Mat3::yaw(radians),
3488 Step::Pitch(_) => Mat3::pitch(radians),
3489 Step::Roll(_) => Mat3::roll(radians),
3490 Step::Start
3491 | Step::Forward(_)
3492 | Step::Left(_)
3493 | Step::Up(_)
3494 | Step::PenUp
3495 | Step::PenDown
3496 | Step::PenColor(_)
3497 | Step::PenWidth(_)
3498 | Step::Disk(_)
3499 | Step::DiskParam(_)
3500 | Step::Sphere(_)
3501 | Step::SphereParam(_)
3502 | Step::Mark { .. }
3503 | Step::Restore { .. } => Mat3::IDENTITY,
3504 }
3505}
3506
3507#[derive(Clone, Copy, Debug, Eq, PartialEq)]
3513pub enum PenState {
3514 Up,
3516 Down,
3518}
3519
3520#[derive(Clone, Copy, Debug)]
3522struct PenStyle {
3523 pen: PenState,
3524 color: Rgb888,
3525 width: f32,
3526}
3527
3528impl PenStyle {
3529 #[must_use]
3531 pub const fn new() -> Self {
3532 Self {
3533 pen: PenState::Down,
3534 color: Rgb888::new(255, 255, 255),
3535 width: 0.1,
3536 }
3537 }
3538
3539 fn reset(&mut self) {
3540 *self = Self::new();
3541 }
3542
3543 #[must_use]
3545 pub const fn pen(self) -> PenState {
3546 self.pen
3547 }
3548
3549 #[must_use]
3551 pub const fn color(self) -> Rgb888 {
3552 self.color
3553 }
3554
3555 #[must_use]
3557 pub const fn width(self) -> f32 {
3558 self.width
3559 }
3560
3561 fn apply(&mut self, step: &Step) {
3562 match step {
3563 Step::Start => self.reset(),
3564 Step::PenUp => self.pen = PenState::Up,
3565 Step::PenDown => self.pen = PenState::Down,
3566 Step::PenColor(color) => self.color = *color,
3567 Step::PenWidth(width) => self.width = *width,
3568 Step::Yaw(_)
3569 | Step::Pitch(_)
3570 | Step::Roll(_)
3571 | Step::Forward(_)
3572 | Step::Left(_)
3573 | Step::Up(_)
3574 | Step::Disk(_)
3575 | Step::DiskParam(_)
3576 | Step::Sphere(_)
3577 | Step::SphereParam(_)
3578 | Step::Mark { .. }
3579 | Step::Restore { .. } => {}
3580 }
3581 }
3582}
3583
3584impl Default for PenStyle {
3585 fn default() -> Self {
3586 Self::new()
3587 }
3588}
3589
3590#[derive(Clone, Copy, Debug)]
3620pub struct Pose {
3621 orientation: Mat3,
3622 position: Vec3,
3623}
3624
3625impl Pose {
3626 #[must_use]
3630 pub const fn new(orientation: Mat3, position: Vec3) -> Self {
3631 Self {
3632 orientation,
3633 position,
3634 }
3635 }
3636
3637 #[must_use]
3641 pub fn project(self, projection: &Projection) -> (f32, f32) {
3642 let c = projection.world_to_camera(self.position());
3643 let k = projection.scale * projection.depth_factor(c[0]);
3644 (
3645 projection.target_origin.x as f32 - c[1] * k,
3646 projection.target_origin.y as f32 - c[2] * k,
3647 )
3648 }
3649
3650 #[must_use]
3654 pub const fn start() -> Self {
3655 Self {
3656 orientation: Mat3::IDENTITY,
3657 position: Vec3::ZERO,
3658 }
3659 }
3660
3661 #[must_use]
3663 pub const fn orientation(self) -> Mat3 {
3664 self.orientation
3665 }
3666
3667 #[must_use]
3669 pub const fn position(self) -> Vec3 {
3670 self.position
3671 }
3672
3673 #[must_use]
3675 pub fn is_close_to(&self, other: &Self, tolerance: f32) -> bool {
3676 self.orientation.is_close_to(&other.orientation, tolerance)
3677 && self.position.is_close_to(&other.position, tolerance)
3678 }
3679
3680 fn apply<const DOF: usize>(&mut self, step: &Step, params: &[f32; DOF]) {
3681 match step {
3682 Step::Start => {
3683 *self = Self::start();
3684 }
3685 Step::Forward(arg) => {
3686 self.position += self.orientation.forward() * arg.resolve(params);
3687 }
3688 Step::Left(arg) => {
3689 self.position += self.orientation.left() * arg.resolve(params);
3690 }
3691 Step::Up(arg) => {
3692 self.position += self.orientation.up() * arg.resolve(params);
3693 }
3694 Step::Yaw(_) | Step::Pitch(_) | Step::Roll(_) => {
3695 self.orientation = self.orientation * rotation_matrix(step, params);
3696 }
3697 Step::PenUp
3698 | Step::PenDown
3699 | Step::PenColor(_)
3700 | Step::PenWidth(_)
3701 | Step::Disk(_)
3702 | Step::DiskParam(_)
3703 | Step::Sphere(_)
3704 | Step::SphereParam(_)
3705 | Step::Mark { .. }
3706 | Step::Restore { .. } => {}
3707 }
3708 }
3709}
3710
3711#[derive(Clone, Copy, Debug)]
3737pub struct StyledPose {
3738 pose: Pose,
3739 pen_style: PenStyle,
3740}
3741
3742impl StyledPose {
3743 #[must_use]
3747 pub const fn pose(self) -> Pose {
3748 self.pose
3749 }
3750
3751 #[must_use]
3755 pub const fn pen(self) -> PenState {
3756 self.pen_style.pen()
3757 }
3758
3759 #[must_use]
3761 pub const fn color(self) -> Rgb888 {
3762 self.pen_style.color()
3763 }
3764
3765 #[must_use]
3767 pub const fn width(self) -> f32 {
3768 self.pen_style.width()
3769 }
3770}
3771
3772#[derive(Clone, Copy)]
3777struct MarkedState {
3778 pose: Pose,
3779 pen_style: PenStyle,
3780}
3781
3782struct StyledPosesView<'a, const DOF: usize, const MARKS: usize> {
3784 steps: &'a [Step],
3785 params: &'a [f32; DOF],
3786 index: usize,
3787 pose: Pose,
3788 pen_style: PenStyle,
3789 marked: [MarkedState; MARKS],
3790}
3791
3792impl<'a, const DOF: usize, const MARKS: usize> StyledPosesView<'a, DOF, MARKS> {
3793 fn new(steps: &'a [Step], params_values: &'a [f32; DOF]) -> Result<Self, Error> {
3794 validate_params(params_values)?;
3795 Ok(Self {
3796 steps,
3797 params: params_values,
3798 index: 0,
3799 pose: Pose::start(),
3800 pen_style: PenStyle::new(),
3801 marked: [MarkedState {
3802 pose: Pose::start(),
3803 pen_style: PenStyle::new(),
3804 }; MARKS],
3805 })
3806 }
3807}
3808
3809impl<const DOF: usize, const MARKS: usize> Iterator for StyledPosesView<'_, DOF, MARKS> {
3810 type Item = StyledPose;
3811
3812 fn next(&mut self) -> Option<Self::Item> {
3813 loop {
3814 if self.index >= self.steps.len() {
3815 return None;
3816 }
3817 let step = &self.steps[self.index];
3818 self.index += 1;
3819
3820 match step {
3821 Step::Mark { index } => {
3822 self.marked[*index] = MarkedState {
3823 pose: self.pose,
3824 pen_style: self.pen_style,
3825 };
3826 continue;
3827 }
3828 Step::Restore { index } => {
3829 let marked_state = self.marked[*index];
3830 self.pose = marked_state.pose;
3831 self.pen_style = marked_state.pen_style;
3832 continue;
3833 }
3834 _ => {}
3835 }
3836
3837 self.pose.apply(step, self.params);
3838 self.pen_style.apply(step);
3839 return Some(StyledPose {
3840 pose: self.pose,
3841 pen_style: self.pen_style,
3842 });
3843 }
3844 }
3845}
3846
3847pub struct DrawItem3dIter<'a, const DOF: usize, const MARKS: usize> {
3877 steps: &'a [Step],
3878 mark_names: &'a [&'static str; MARKS],
3879 mark_len: usize,
3880 params: &'a [f32; DOF],
3881 index: usize,
3882 pose: Pose,
3883 pen_style: PenStyle,
3884 marked: [MarkedState; MARKS],
3885}
3886
3887impl<'a, const DOF: usize, const MARKS: usize> DrawItem3dIter<'a, DOF, MARKS> {
3888 fn new(
3889 steps: &'a [Step],
3890 mark_names: &'a [&'static str; MARKS],
3891 mark_len: usize,
3892 params_values: &'a [f32; DOF],
3893 ) -> Result<Self, Error> {
3894 validate_params(params_values)?;
3895 Ok(Self {
3896 steps,
3897 mark_names,
3898 mark_len,
3899 params: params_values,
3900 index: 0,
3901 pose: Pose::start(),
3902 pen_style: PenStyle::new(),
3903 marked: [MarkedState {
3904 pose: Pose::start(),
3905 pen_style: PenStyle::new(),
3906 }; MARKS],
3907 })
3908 }
3909
3910 pub fn pose_by_mark_name(&self, name: &str) -> Result<Pose, Error> {
3919 let mut found = None;
3920 for (index, &n) in self.mark_names[..].iter().enumerate().take(self.mark_len) {
3921 if n == name {
3922 if found.is_some() {
3923 return Err(Error::MarkAmbiguous);
3924 }
3925 found = Some(index);
3926 }
3927 }
3928 found
3929 .map(|index| self.marked[index].pose)
3930 .ok_or(Error::MarkNotFound)
3931 }
3932
3933 #[must_use]
3938 pub fn final_pose(&self) -> Pose {
3939 self.pose
3940 }
3941}
3942
3943impl<const DOF: usize, const MARKS: usize> Iterator for DrawItem3dIter<'_, DOF, MARKS> {
3944 type Item = Item3d;
3945
3946 fn next(&mut self) -> Option<Self::Item> {
3947 while self.index < self.steps.len() {
3948 let step = &self.steps[self.index];
3949 self.index += 1;
3950
3951 match step {
3952 Step::Mark { index } => {
3953 self.marked[*index] = MarkedState {
3954 pose: self.pose,
3955 pen_style: self.pen_style,
3956 };
3957 continue;
3958 }
3959 Step::Restore { index } => {
3960 let marked_state = self.marked[*index];
3961 self.pose = marked_state.pose;
3962 self.pen_style = marked_state.pen_style;
3963 continue;
3964 }
3965 _ => {}
3966 }
3967
3968 let start_pose = self.pose;
3969 let pen_style = self.pen_style;
3970 self.pose.apply(step, self.params);
3971 self.pen_style.apply(step);
3972
3973 match step {
3974 Step::Forward(_) | Step::Left(_) | Step::Up(_)
3975 if matches!(pen_style.pen(), PenState::Down) =>
3976 {
3977 return Some(Item3d::Stroke(Stroke {
3978 start: start_pose,
3979 end: self.pose,
3980 color: pen_style.color(),
3981 width: pen_style.width(),
3982 }));
3983 }
3984 Step::Disk(radius) => {
3985 return Some(Item3d::Disk(Disk {
3986 pose: start_pose,
3987 radius: *radius,
3988 color: pen_style.color(),
3989 }));
3990 }
3991 Step::DiskParam(var_arg) => {
3992 return Some(Item3d::Disk(Disk {
3993 pose: start_pose,
3994 radius: var_arg.resolve(self.params),
3995 color: pen_style.color(),
3996 }));
3997 }
3998 Step::Sphere(radius) => {
3999 return Some(Item3d::Sphere(Sphere {
4000 pose: start_pose,
4001 radius: *radius,
4002 color: pen_style.color(),
4003 }));
4004 }
4005 Step::SphereParam(var_arg) => {
4006 return Some(Item3d::Sphere(Sphere {
4007 pose: start_pose,
4008 radius: var_arg.resolve(self.params),
4009 color: pen_style.color(),
4010 }));
4011 }
4012 _ => continue,
4013 }
4014 }
4015
4016 None
4017 }
4018}
4019
4020#[macro_export]
4060macro_rules! linkage {
4061 ($($chain:tt)*) => {
4062 (__linkage_blaze_start!()) $($chain)*
4063 };
4064}
4065
4066#[cfg(feature = "alloc")]
4109#[doc(hidden)]
4110#[macro_export]
4111macro_rules! __linkage_file_buf_items {
4112 ($path:literal) => {
4113 pub type Buf = $crate::LinkageBuf<DOF, MARKS>;
4114
4115 pub fn buf() -> Buf {
4116 macro_rules! __linkage_blaze_start {
4117 () => {
4118 $crate::LinkageBuf::<DOF, MARKS>::start()
4119 };
4120 }
4121 include!($path)
4122 }
4123
4124 const _: fn() -> Buf = buf;
4125 };
4126}
4127
4128#[cfg(not(feature = "alloc"))]
4129#[doc(hidden)]
4130#[macro_export]
4131macro_rules! __linkage_file_buf_items {
4132 ($path:literal) => {};
4133}
4134
4135#[macro_export]
4136macro_rules! linkage_file {
4137 ($(#[$attribute:meta])* $visibility:vis $name:ident {
4138 file: $path:literal $(,)?
4139 }) => {
4140 $(#[$attribute])*
4141 $visibility mod $name {
4142 use $crate::{linkage, Rgb888, WebColors};
4143
4144 const _: Rgb888 = Rgb888::CSS_BLACK;
4145
4146 const __CANDIDATE: $crate::LinkageFixed<
4147 { __METADATA.param_count() },
4148 { __METADATA.mark_count() },
4149 { __METADATA.step_count() },
4150 > = {
4151 macro_rules! __linkage_blaze_start {
4152 () => {
4153 $crate::LinkageFixed::<
4154 { __METADATA.param_count() },
4155 { __METADATA.mark_count() },
4156 { __METADATA.step_count() },
4157 >::start()
4158 };
4159 }
4160 include!($path)
4161 };
4162
4163 pub const DOF: usize = __CANDIDATE.param_count();
4164 pub const MARKS: usize = __CANDIDATE.mark_count();
4165 pub const STEP_COUNT: usize = __METADATA.step_count();
4166
4167 pub type Fixed = $crate::LinkageFixed<DOF, MARKS, STEP_COUNT>;
4168 pub type View = $crate::LinkageView<'static, DOF, MARKS>;
4169
4170 const __METADATA: $crate::LinkageStepCount = {
4171 macro_rules! __linkage_blaze_start {
4172 () => { $crate::LinkageStepCount::start() };
4173 }
4174 include!($path)
4175 };
4176
4177 const __FIXED: Fixed = {
4178 macro_rules! __linkage_blaze_start {
4179 () => {
4180 $crate::LinkageFixed::<DOF, MARKS, STEP_COUNT>::start()
4181 };
4182 }
4183 let linkage = include!($path);
4184 assert!(
4185 linkage.param_count() == DOF,
4186 "DOF must equal the number of defined parameters"
4187 );
4188 assert!(
4189 linkage.mark_count() == MARKS,
4190 "MARKS must equal the number of defined marks"
4191 );
4192 linkage
4193 };
4194
4195 pub const fn fixed() -> Fixed {
4196 __FIXED
4197 }
4198
4199 const __VIEW: View = fixed().view();
4200
4201 pub const fn view() -> View {
4202 __VIEW
4203 }
4204
4205 const _: View = view();
4206
4207 $crate::__linkage_file_buf_items!($path);
4208 }
4209 };
4210 ($($tokens:tt)*) => {
4211 compile_error!("linkage_file! accepts exactly one file declaration per invocation");
4212 };
4213}
4214
4215#[cfg(test)]
4216mod test_helpers;
4217
4218#[cfg(test)]
4219mod tests {
4220 #[cfg(feature = "alloc")]
4221 use super::LinkageBuf;
4222 use super::{Error, LinkageFixed, LinkageView, Mat3, Pose, Rgb888, Step, StepArg, Vec3};
4223 use crate::render::{Item3d, Projection};
4224 use crate::test_helpers::{
4225 assert_png_matches_expected, assert_pose_approx_eq, assert_pose_trace_matches_expected,
4226 draw_linkage_xy_canvas,
4227 };
4228 use device_envoy_core::pixel_target::{
4229 rgb565_from_rgb888, rgb565_from_rgb888_components, rgb888_from_rgb565,
4230 };
4231 use embedded_graphics::{pixelcolor::Rgb565, prelude::Point};
4232 use std::{boxed::Box, error::Error as StdError};
4233
4234 const LINKAGE0: LinkageFixed<6, 0, 24> = LinkageFixed::start()
4235 .define_param("raise hand", 0.5)
4236 .define_param("bend elbow", 0.5)
4237 .define_param("close hand", 0.5)
4238 .define_param("lower arm", 0.5)
4239 .define_param("spin whole arm", 0.5)
4240 .define_param("spin hand", 0.5)
4241 .yaw(90.0)
4242 .yaw_param("spin whole arm", 180.0, -180.0)
4243 .pitch(90.0)
4244 .forward(2.5)
4245 .pitch(-90.0)
4246 .pitch_param("lower arm", 30.0, 0.0)
4247 .forward(3.0)
4248 .yaw_param("bend elbow", 90.0, -90.0)
4249 .forward(3.0)
4250 .pitch_param("raise hand", 90.0, -90.0)
4251 .forward(1.0)
4252 .roll_param("spin hand", -180.0, 180.0)
4253 .forward(0.5)
4254 .yaw(90.0)
4255 .forward_param("close hand", 0.0, 0.5)
4256 .yaw(-90.0)
4257 .forward(1.0)
4258 .yaw(180.0)
4259 .forward(1.0)
4260 .yaw(90.0)
4261 .forward_param("close hand", 0.0, 1.0)
4262 .yaw(90.0)
4263 .forward(1.0);
4264
4265 const LINKAGE1: LinkageFixed<3, 0, 16> = LinkageFixed::start()
4266 .define_param("spin whole arm", 0.5)
4267 .define_param("bend elbow", 0.5)
4268 .define_param("close hand", 0.5)
4269 .yaw(90.0)
4270 .yaw_param("spin whole arm", 180.0, -180.0)
4271 .forward(3.0)
4272 .yaw_param("bend elbow", 90.0, -90.0)
4273 .forward(3.0)
4274 .yaw(90.0)
4275 .forward_param("close hand", 0.5, 0.0)
4276 .yaw(-90.0)
4277 .forward(1.0)
4278 .yaw(-180.0)
4279 .forward(1.0)
4280 .yaw(90.0)
4281 .forward_param("close hand", 1.0, 0.0)
4282 .yaw(90.0)
4283 .forward(1.0);
4284
4285 fn assert_float_close(actual: f32, expected: f32, tolerance: f32) {
4286 assert!(
4287 (actual - expected).abs() <= tolerance,
4288 "actual {actual} was not within {tolerance} of expected {expected}",
4289 );
4290 }
4291
4292 #[test]
4293 fn rgb888_from_rgb565_is_const() {
4294 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));
4301 assert_eq!(WHITE, Rgb888::new(255, 255, 255));
4302 assert_eq!(RED, Rgb888::new(255, 0, 0));
4303 assert_eq!(GREEN, Rgb888::new(0, 255, 0));
4304 assert_eq!(BLUE, Rgb888::new(0, 0, 255));
4305 }
4306
4307 #[test]
4308 fn rgb565_from_rgb888_components_is_const() {
4309 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)));
4316 assert_eq!(WHITE, rgb565_from_rgb888(Rgb888::new(255, 255, 255)));
4317 assert_eq!(RED, rgb565_from_rgb888(Rgb888::new(255, 0, 0)));
4318 assert_eq!(GREEN, rgb565_from_rgb888(Rgb888::new(0, 255, 0)));
4319 assert_eq!(BLUE, rgb565_from_rgb888(Rgb888::new(0, 0, 255)));
4320 }
4321
4322 #[test]
4323 fn top_orthographic_projects_xy_plane() {
4324 let projection = Projection::top_orthographic(Point::new(100, 100), 2.0);
4325
4326 let forward_pose = Pose::new(Mat3::IDENTITY, Vec3::from([3.0, 0.0, 0.0]));
4327 let left_pose = Pose::new(Mat3::IDENTITY, Vec3::from([0.0, 4.0, 0.0]));
4328 let forward_projected = forward_pose.project(&projection);
4329 let left_projected = left_pose.project(&projection);
4330
4331 assert_float_close(forward_projected.0, 100.0, 1e-5);
4332 assert_float_close(forward_projected.1, 94.0, 1e-5);
4333 assert_float_close(left_projected.0, 92.0, 1e-5);
4334 assert_float_close(left_projected.1, 100.0, 1e-5);
4335
4336 let forward_axis = projection.project_dir(Pose::start(), Vec3::from([1.0, 0.0, 0.0]), 5.0);
4337 let left_axis = projection.project_dir(Pose::start(), Vec3::from([0.0, 1.0, 0.0]), 5.0);
4338
4339 assert_float_close(forward_axis.0, 0.0, 1e-5);
4340 assert_float_close(forward_axis.1, -10.0, 1e-5);
4341 assert_float_close(left_axis.0, -10.0, 1e-5);
4342 assert_float_close(left_axis.1, 0.0, 1e-5);
4343 }
4344
4345 #[test]
4346 fn zero_pen_width_still_draws() -> Result<(), Box<dyn StdError>> {
4347 const LINKAGE: LinkageFixed<0, 0, 4> = LinkageFixed::start().pen_width(0.0).forward(1.0);
4348
4349 let params = [];
4350 let draw_item_3d = LINKAGE
4351 .view()
4352 .draw_items_3d(¶ms)?
4353 .next()
4354 .expect("zero-width pen should still produce a stroke");
4355
4356 match draw_item_3d {
4357 Item3d::Stroke(stroke_segment) => {
4358 assert_eq!(stroke_segment.width(), 0.0);
4359 }
4360 _ => panic!("expected stroke from zero-width pen"),
4361 }
4362 Ok(())
4363 }
4364
4365 #[cfg(feature = "alloc")]
4366 #[test]
4367 fn serializes_linkage_view_to_lb_rs() {
4368 const LINKAGE: LinkageFixed<1, 1, 10> = LinkageFixed::start()
4369 .define_param("distance", 0.5)
4370 .pen_up()
4371 .mark("origin")
4372 .pen_color(Rgb888::new(10, 20, 30))
4373 .pen_width(2.0)
4374 .pen_down()
4375 .forward_param("distance", 1.0, 5.0)
4376 .restore("origin");
4377
4378 let source = LINKAGE.view().to_lb_rs();
4379
4380 assert!(source.starts_with("// DOF="));
4381 assert!(source.contains("linkage![\n"));
4382 assert!(source.trim_end().ends_with(']'));
4383 assert!(source.contains(".define_param(\"distance\", 0.5)"));
4384 assert!(source.contains(".pen_color(Rgb888::new(10, 20, 30))"));
4385 assert!(source.contains(".forward_param(\"distance\", 1.0, 5.0)"));
4386 assert!(source.contains(".restore(\"origin\")"));
4387 }
4388
4389 #[cfg(feature = "alloc")]
4390 #[test]
4391 fn parses_lb_rs_into_linkage_buf() -> Result<(), Box<dyn StdError>> {
4392 let source = r#"linkage![
4393 .define_param("distance", 0.5)
4394 .pen_color(Rgb888::new(10, 20, 30))
4395 .forward_param("distance", 1.0, 5.0)
4396]"#;
4397
4398 let linkage = LinkageBuf::<1, 0>::from_lb_rs(source).expect("source should parse");
4399 let pose = linkage.view().final_pose(&[0.5])?;
4400
4401 assert!(
4402 pose.position()
4403 .is_close_to(&Vec3::from([3.0, 0.0, 0.0]), 1e-5)
4404 );
4405 Ok(())
4406 }
4407
4408 #[cfg(feature = "alloc")]
4409 #[test]
4410 fn lb_rs_parser_rejects_integer_arguments() {
4411 let error = match LinkageBuf::<0, 0>::from_lb_rs("linkage![\n.forward(1)\n]") {
4412 Ok(_) => panic!("integer argument should fail"),
4413 Err(error) => error,
4414 };
4415
4416 assert!(error.contains("is an integer"));
4417 }
4418
4419 #[test]
4420 fn forward_moves_along_positive_x() -> Result<(), Box<dyn StdError>> {
4421 const LINKAGE: LinkageFixed<0, 0, 2> = LinkageFixed::start().forward(10.0);
4422
4423 let params = [];
4424 let actual = LINKAGE.view().final_pose(¶ms)?.position();
4425
4426 assert!(actual.is_close_to(&Vec3::from([10.0, 0.0, 0.0]), 1e-6));
4427 Ok(())
4428 }
4429
4430 #[test]
4431 fn link_view_finds_named_mark_pose() -> Result<(), Box<dyn StdError>> {
4432 const LINKAGE: LinkageFixed<0, 1, 4> = LinkageFixed::start().mark("tip").forward(2.0);
4433
4434 let view = LINKAGE.view();
4435 let mut items = view.draw_items_3d(&[])?;
4436 while items.next().is_some() {}
4437 let pose = items.pose_by_mark_name("tip")?;
4438 assert_eq!(pose.position(), Vec3::from([0.0, 0.0, 0.0]));
4439 Ok(())
4440 }
4441
4442 #[test]
4443 fn link_view_reports_missing_mark() {
4444 const LINKAGE: LinkageFixed<0, 1, 2> = LinkageFixed::start().mark("tip");
4445
4446 let view = LINKAGE.view();
4447 let mut items = view.draw_items_3d(&[]).expect("valid parameters");
4448 while items.next().is_some() {}
4449 assert!(matches!(
4450 items.pose_by_mark_name("missing"),
4451 Err(Error::MarkNotFound)
4452 ));
4453 }
4454
4455 #[cfg(feature = "alloc")]
4456 #[test]
4457 fn link_view_reports_ambiguous_mark() {
4458 let first = LinkageBuf::<0, 1>::start().mark("tip");
4459 let second = LinkageBuf::<0, 1>::start().mark("tip");
4460 let linkage: LinkageBuf<0, 2> = first.combine(second.view());
4461
4462 let view = linkage.view();
4463 let mut items = view.draw_items_3d(&[]).expect("valid parameters");
4464 while items.next().is_some() {}
4465 assert!(matches!(
4466 items.pose_by_mark_name("tip"),
4467 Err(Error::MarkAmbiguous)
4468 ));
4469 }
4470
4471 #[test]
4472 fn yaw_then_forward_moves_along_positive_y() -> Result<(), Box<dyn StdError>> {
4473 const LINKAGE: LinkageFixed<0, 0, 3> = LinkageFixed::start().yaw(90.0).forward(10.0);
4474
4475 let params = [];
4476 let actual = LINKAGE.view().final_pose(¶ms)?.position();
4477
4478 assert!(actual.is_close_to(&Vec3::from([0.0, 10.0, 0.0]), 1e-5));
4479 Ok(())
4480 }
4481
4482 #[test]
4483 fn left_moves_along_positive_y() -> Result<(), Box<dyn StdError>> {
4484 const LINKAGE: LinkageFixed<0, 0, 2> = LinkageFixed::start().left(10.0);
4485
4486 let params = [];
4487 let actual = LINKAGE.view().final_pose(¶ms)?.position();
4488
4489 assert!(actual.is_close_to(&Vec3::from([0.0, 10.0, 0.0]), 1e-6));
4490 Ok(())
4491 }
4492
4493 #[test]
4494 fn up_moves_along_positive_z() -> Result<(), Box<dyn StdError>> {
4495 const LINKAGE: LinkageFixed<0, 0, 2> = LinkageFixed::start().up(10.0);
4496
4497 let params = [];
4498 let actual = LINKAGE.view().final_pose(¶ms)?.position();
4499
4500 assert!(actual.is_close_to(&Vec3::from([0.0, 0.0, 10.0]), 1e-6));
4501 Ok(())
4502 }
4503
4504 #[test]
4505 fn translation_params_move_along_named_axes() -> Result<(), Box<dyn StdError>> {
4506 const LINKAGE: LinkageFixed<3, 0, 7> = LinkageFixed::start()
4507 .define_param("forward", 0.5)
4508 .define_param("left", 0.5)
4509 .define_param("up", 0.5)
4510 .forward_param("forward", 0.0, 10.0)
4511 .left_param("left", 0.0, 20.0)
4512 .up_param("up", 0.0, 30.0);
4513
4514 let params = [0.2, 0.3, 0.4];
4515 let actual = LINKAGE.view().final_pose(¶ms)?.position();
4516
4517 assert!(actual.is_close_to(&Vec3::from([2.0, 6.0, 12.0]), 1e-6));
4518 Ok(())
4519 }
4520
4521 #[test]
4522 fn planar_two_link_arm_uses_yaw_then_forward() -> Result<(), Box<dyn StdError>> {
4523 const LINKAGE: LinkageFixed<0, 0, 5> = LinkageFixed::start()
4524 .yaw(0.0)
4525 .forward(10.0)
4526 .yaw(90.0)
4527 .forward(5.0);
4528
4529 let params = [];
4530 let actual = LINKAGE.view().final_pose(¶ms)?.position();
4531
4532 assert!(actual.is_close_to(&Vec3::from([10.0, 5.0, 0.0]), 1e-5));
4533 Ok(())
4534 }
4535
4536 #[test]
4537 fn test_excel_pose_trace0_matches_expected() -> Result<(), Box<dyn StdError>> {
4538 let params = [0.751_450_1, 0.500_200_4, 0.5, 1.0, 0.625_438_7, 0.0];
4541 assert_pose_trace_matches_expected("excel_pose_trace0.csv", LINKAGE0.view().poses(¶ms)?)
4542 }
4543
4544 #[test]
4545 fn test_excel_pose_trace1_matches_expected() -> Result<(), Box<dyn StdError>> {
4546 let params = [0.30, 0.02, 0.10];
4548 assert_pose_trace_matches_expected("excel_pose_trace1.csv", LINKAGE1.view().poses(¶ms)?)
4549 }
4550
4551 #[test]
4552 fn test_setting0_matches_excel_final_pose() -> Result<(), Box<dyn StdError>> {
4553 let params = [
4554 0.751_450_1, 0.49, 0.50011957, 1.0, 0.625_438_7, 1.0, ];
4561 let pose = LINKAGE0.view().final_pose(¶ms)?;
4562 let expected = Pose::new(
4563 [
4564 [0.48325038, 0.7270788, 0.48767346],
4565 [0.5117748, -0.68655396, 0.51645917],
4566 [0.7103207, 0.0, -0.70387816],
4567 ]
4568 .into(),
4569 [5.213134, 5.747819, -0.7241982].into(),
4570 );
4571
4572 assert_pose_approx_eq(pose, expected);
4573 Ok(())
4574 }
4575
4576 #[test]
4577 fn test_setting1_matches_excel_final_pose() -> Result<(), Box<dyn StdError>> {
4578 let params = [
4579 0.30, 0.02, 0.10, ];
4583 let pose = LINKAGE1.view().final_pose(¶ms)?;
4584 let expected = Pose::new(
4585 [
4586 [-0.368_124_5, 0.929_776_43, 0.0],
4587 [-0.929_776_43, -0.368_124_5, 0.0],
4588 [0.0, 0.0, 1.0],
4589 ]
4590 .into(),
4591 [-4.744_067, -2.626_399, 0.0].into(),
4592 );
4593
4594 assert_pose_approx_eq(pose, expected);
4595 Ok(())
4596 }
4597
4598 #[test]
4599 fn test_mid_setting0_matches_excel_final_pose_and_png() -> Result<(), Box<dyn StdError>> {
4600 let params = [
4601 0.5, 0.3, 1.0, 0.5, 0.5, 0.5, ];
4608 let pose = LINKAGE0.view().final_pose(¶ms)?;
4609 let expected = Pose::new(
4610 [
4611 [-0.5877855, -0.80901694, 0.0],
4612 [0.78145033, -0.5677572, 0.25881904],
4613 [-0.20938899, 0.15213005, 0.9659258],
4614 ]
4615 .into(),
4616 [-2.828311, 7.4796333, -4.504162].into(),
4617 );
4618
4619 assert_pose_approx_eq(pose, expected);
4620
4621 let canvas = draw_linkage_xy_canvas(&LINKAGE0, ¶ms)?;
4622 assert_png_matches_expected("linkage0_xy_mid_fraction.png", &canvas)
4623 }
4624
4625 #[test]
4626 fn test_linkage0_png_matches_expected() -> Result<(), Box<dyn StdError>> {
4627 let params = [0.751_450_1, 0.500_200_4, 0.5, 1.0, 0.625_438_7, 0.0];
4630
4631 let canvas = draw_linkage_xy_canvas(&LINKAGE0, ¶ms)?;
4632 assert_png_matches_expected("linkage0_xy.png", &canvas)
4633 }
4634
4635 #[test]
4636 fn test_linkage1_png_matches_expected() -> Result<(), Box<dyn StdError>> {
4637 let params = [0.30, 0.02, 0.10];
4639
4640 let canvas = draw_linkage_xy_canvas(&LINKAGE1, ¶ms)?;
4641 assert_png_matches_expected("linkage1_xy.png", &canvas)
4642 }
4643
4644 #[test]
4645 fn test_params_are_range_checked() {
4646 let params = [
4647 0.0, 0.5, 1.1, 1.0, 0.0, 0.5, ];
4654
4655 assert!(matches!(
4656 LINKAGE0.view().final_pose(¶ms),
4657 Err(Error::InvalidParameter { index: 2, .. })
4658 ));
4659 }
4660
4661 #[test]
4669 fn duplicate_define_param_does_not_panic() {
4670 const _: LinkageFixed<2, 0, 2> = LinkageFixed::start()
4673 .define_param("angle", 0.25) .define_param("angle", 0.75); }
4676
4677 #[test]
4678 fn shadowing_builder_binds_to_most_recent_definition() -> Result<(), Box<dyn StdError>> {
4679 const LINKAGE: LinkageFixed<2, 0, 5> = LinkageFixed::start()
4686 .define_param("angle", 0.0) .define_param("angle", 1.0) .yaw_param("angle", 0.0, 90.0) .forward(10.0);
4690
4691 let params = [1.0, 0.0]; let pos = LINKAGE.view().final_pose(¶ms)?.position();
4693 assert!(pos.is_close_to(&Vec3::from([10.0, 0.0, 0.0]), 1e-5));
4695 Ok(())
4696 }
4697
4698 #[test]
4701 fn freeze_param_index_uses_raw_rotation_degrees() -> Result<(), Box<dyn StdError>> {
4702 const BASE: LinkageFixed<2, 0, 5> = LinkageFixed::start()
4703 .define_param("angle", 0.5)
4704 .define_param("len", 0.5)
4705 .yaw_param("angle", -180.0, 180.0)
4706 .forward_param("len", 0.0, 10.0);
4707
4708 const FROZEN: LinkageFixed<1, 0, 5> = BASE.freeze_param_index(0, 90.0);
4709
4710 assert_specialized_matches_original(BASE, &[0.75, 1.0], FROZEN, &[1.0])?;
4711 let pos = FROZEN.view().final_pose(&[1.0])?.position();
4712 assert!(pos.is_close_to(&Vec3::from([0.0, 10.0, 0.0]), 1e-4));
4713 Ok(())
4714 }
4715
4716 #[test]
4717 fn freeze_param_name_matches_unique_slot() -> Result<(), Box<dyn StdError>> {
4718 const BASE: LinkageFixed<2, 0, 5> = LinkageFixed::start()
4719 .define_param("yaw", 0.25)
4720 .define_param("dist", 0.5)
4721 .yaw_param("yaw", 0.0, 180.0)
4722 .forward_param("dist", 0.0, 8.0);
4723
4724 const FROZEN_BY_NAME: LinkageFixed<1, 0, 5> = BASE.freeze_param_name("yaw", 45.0);
4725 const FROZEN_BY_DEFAULT: LinkageFixed<1, 0, 5> = BASE.freeze_param_index_at_default(0);
4726
4727 let pos_by_name = FROZEN_BY_NAME.view().final_pose(&[0.5])?.position();
4728 let pos_by_default = FROZEN_BY_DEFAULT.view().final_pose(&[0.5])?.position();
4729 assert!(pos_by_name.is_close_to(&pos_by_default, 1e-6));
4730 Ok(())
4731 }
4732
4733 #[test]
4734 fn retain_param_names_freezes_unlisted_at_default() -> Result<(), Box<dyn StdError>> {
4735 const BASE: LinkageFixed<2, 0, 5> = LinkageFixed::start()
4736 .define_param("angle", 0.5)
4737 .define_param("dist", 0.5)
4738 .yaw_param("angle", -90.0, 90.0)
4739 .forward_param("dist", 0.0, 4.0);
4740
4741 const RETAINED: LinkageFixed<1, 0, 5> = BASE.retain_param_names(&["dist"]);
4742
4743 assert_specialized_matches_original(BASE, &[0.5, 1.0], RETAINED, &[1.0])?;
4744 let pos = RETAINED.view().final_pose(&[1.0])?.position();
4745 assert!(pos.is_close_to(&Vec3::from([4.0, 0.0, 0.0]), 1e-4));
4746 Ok(())
4747 }
4748
4749 #[test]
4750 fn freeze_param_index_freezes_only_that_shadowed_slot() -> Result<(), Box<dyn StdError>> {
4751 const BASE: LinkageFixed<3, 0, 5> = LinkageFixed::start()
4752 .define_param("x", 0.1)
4753 .forward_param("x", 0.0, 10.0)
4754 .define_param("y", 0.2)
4755 .left_param("y", 0.0, 10.0)
4756 .define_param("x", 0.3)
4757 .up_param("x", 0.0, 10.0);
4758
4759 const FIRST_X: LinkageFixed<2, 0, 5> = BASE.freeze_param_index(0, 4.0);
4760 const SECOND_X: LinkageFixed<2, 0, 5> = BASE.freeze_param_index(2, 6.0);
4761
4762 assert_specialized_matches_original(BASE, &[0.4, 0.8, 0.6], FIRST_X, &[0.8, 0.6])?;
4763 assert_specialized_matches_original(BASE, &[0.4, 0.8, 0.6], SECOND_X, &[0.4, 0.8])?;
4764 Ok(())
4765 }
4766
4767 #[test]
4768 fn retain_param_names_retains_every_shadowed_slot_in_original_order()
4769 -> Result<(), Box<dyn StdError>> {
4770 const BASE: LinkageFixed<3, 0, 5> = LinkageFixed::start()
4771 .define_param("x", 0.25)
4772 .forward_param("x", 0.0, 10.0)
4773 .define_param("y", 0.5)
4774 .left_param("y", 0.0, 10.0)
4775 .define_param("x", 0.75)
4776 .up_param("x", 0.0, 10.0);
4777
4778 const RETAINED: LinkageFixed<2, 0, 5> = BASE.retain_param_names(&["x"]);
4779 let params = RETAINED.view().params();
4780 assert_eq!(params[0].name(), "x");
4781 assert_eq!(params[0].default(), 0.25);
4782 assert_eq!(params[1].name(), "x");
4783 assert_eq!(params[1].default(), 0.75);
4784
4785 assert_specialized_matches_original(BASE, &[1.0, 0.5, 0.0], RETAINED, &[1.0, 0.0])?;
4786 Ok(())
4787 }
4788
4789 #[test]
4790 fn retain_param_indexes_keeps_exact_shadowed_slots() -> Result<(), Box<dyn StdError>> {
4791 const RETAINED: LinkageFixed<2, 0, 8> = BRUCE.retain_param_indexes(&[0, 2]);
4792
4793 let params = RETAINED.view().params();
4794 assert_eq!(params[0].name(), "Bruce");
4795 assert_eq!(params[0].default(), 0.10);
4796 assert_eq!(params[1].name(), "Bruce");
4797 assert_eq!(params[1].default(), 0.30);
4798
4799 assert_specialized_matches_original(
4800 BRUCE,
4801 &[0.11, 0.20, 0.33, 0.40],
4802 RETAINED,
4803 &[0.11, 0.33],
4804 )?;
4805 Ok(())
4806 }
4807
4808 #[test]
4809 fn retain_param_names_freezes_non_retained_shadowed_slots_at_their_own_defaults()
4810 -> Result<(), Box<dyn StdError>> {
4811 const BASE: LinkageFixed<3, 0, 5> = LinkageFixed::start()
4812 .define_param("x", 0.25)
4813 .forward_param("x", 0.0, 10.0)
4814 .define_param("y", 0.5)
4815 .left_param("y", 0.0, 10.0)
4816 .define_param("x", 0.75)
4817 .up_param("x", 0.0, 10.0);
4818
4819 const RETAINED: LinkageFixed<1, 0, 5> = BASE.retain_param_names(&["y"]);
4820
4821 assert_specialized_matches_original(BASE, &[0.25, 1.0, 0.75], RETAINED, &[1.0])?;
4822 Ok(())
4823 }
4824
4825 #[test]
4826 fn specialization_matches_original_for_index_and_name_selectors()
4827 -> Result<(), Box<dyn StdError>> {
4828 const BASE: LinkageFixed<3, 0, 5> = LinkageFixed::start()
4829 .define_param("x", 0.25)
4830 .forward_param("x", 0.0, 10.0)
4831 .define_param("y", 0.5)
4832 .left_param("y", 0.0, 10.0)
4833 .define_param("z", 0.75)
4834 .up_param("z", 0.0, 10.0);
4835
4836 const SPECIALIZED: LinkageFixed<1, 0, 5> = BASE
4837 .freeze_param_index::<2>(0, 4.0)
4838 .retain_param_names(&["y"]);
4839
4840 assert_specialized_matches_original(BASE, &[0.4, 0.8, 0.75], SPECIALIZED, &[0.8])?;
4841 Ok(())
4842 }
4843
4844 const BRUCE: LinkageFixed<4, 0, 8> = LinkageFixed::start()
4845 .define_param("Bruce", 0.10)
4846 .pen_down()
4847 .forward_param("Bruce", 0.0, 100.0)
4848 .define_param("Bruce", 0.20)
4849 .left_param("Bruce", 0.0, 100.0)
4850 .define_param("Bruce", 0.30)
4851 .up_param("Bruce", 0.0, 100.0)
4852 .define_param("Bruce", 0.40)
4853 .yaw_param("Bruce", -180.0, 180.0);
4854
4855 #[test]
4856 fn bruce_full_evaluation_uses_each_slot_bound_at_step_creation() -> Result<(), Box<dyn StdError>>
4857 {
4858 let pose = BRUCE.view().final_pose(&[0.11, 0.22, 0.33, 0.44])?;
4859
4860 assert_pose_close(
4861 pose,
4862 Pose::new(pose.orientation(), Vec3::from([11.0, 22.0, 33.0])),
4863 1e-4,
4864 );
4865 Ok(())
4866 }
4867
4868 #[test]
4869 fn bruce_freeze_param_index_freezes_one_slot() -> Result<(), Box<dyn StdError>> {
4870 const FROZEN: LinkageFixed<3, 0, 8> = BRUCE.freeze_param_index(2, 33.0);
4871
4872 assert_specialized_matches_original(
4873 BRUCE,
4874 &[0.11, 0.22, 0.33, 0.44],
4875 FROZEN,
4876 &[0.11, 0.22, 0.44],
4877 )?;
4878 Ok(())
4879 }
4880
4881 #[test]
4882 fn bruce_freeze_param_index_at_default_freezes_one_slot() -> Result<(), Box<dyn StdError>> {
4883 const FROZEN: LinkageFixed<3, 0, 8> = BRUCE.freeze_param_index_at_default(2);
4884
4885 assert_specialized_matches_original(
4886 BRUCE,
4887 &[0.11, 0.22, 0.30, 0.44],
4888 FROZEN,
4889 &[0.11, 0.22, 0.44],
4890 )?;
4891 Ok(())
4892 }
4893
4894 #[test]
4895 fn bruce_retain_param_names_retains_all_slots_named_bruce() -> Result<(), Box<dyn StdError>> {
4896 const RETAINED: LinkageFixed<4, 0, 8> = BRUCE.retain_param_names(&["Bruce"]);
4897
4898 let params = RETAINED.view().params();
4899 assert_eq!(params[0].name(), "Bruce");
4900 assert_eq!(params[1].name(), "Bruce");
4901 assert_eq!(params[2].name(), "Bruce");
4902 assert_eq!(params[3].name(), "Bruce");
4903
4904 assert_specialized_matches_original(
4905 BRUCE,
4906 &[0.11, 0.22, 0.33, 0.44],
4907 RETAINED,
4908 &[0.11, 0.22, 0.33, 0.44],
4909 )?;
4910 Ok(())
4911 }
4912
4913 #[test]
4914 fn bruce_retain_param_names_freezes_non_bruce_at_own_default() -> Result<(), Box<dyn StdError>>
4915 {
4916 const BASE: LinkageFixed<3, 0, 7> = LinkageFixed::start()
4917 .define_param("Bruce", 0.10)
4918 .pen_down()
4919 .forward_param("Bruce", 0.0, 100.0)
4920 .define_param("Terry", 0.77)
4921 .left_param("Terry", 0.0, 100.0)
4922 .define_param("Bruce", 0.30)
4923 .up_param("Bruce", 0.0, 100.0);
4924
4925 const RETAINED: LinkageFixed<2, 0, 7> = BASE.retain_param_names(&["Bruce"]);
4926
4927 assert_specialized_matches_original(BASE, &[0.11, 0.77, 0.33], RETAINED, &[0.11, 0.33])?;
4928 Ok(())
4929 }
4930
4931 #[cfg(feature = "alloc")]
4932 #[test]
4933 fn linkage_buf_retain_param_indexes_drops_multiple_params() -> Result<(), Box<dyn StdError>> {
4934 let base: LinkageBuf<3, 0> = LinkageBuf::start()
4936 .define_param("yaw", 0.5)
4937 .define_param("pitch", 0.5)
4938 .define_param("dist", 0.5)
4939 .yaw_param("yaw", -90.0, 90.0)
4940 .pitch_param("pitch", -90.0, 90.0)
4941 .forward_param("dist", 0.0, 6.0);
4942
4943 let frozen: LinkageBuf<1, 0> = base.retain_param_indexes(&[2]);
4944
4945 let pos = frozen.view().final_pose(&[1.0])?.position();
4946 assert!(pos.is_close_to(&Vec3::from([6.0, 0.0, 0.0]), 1e-4));
4947 Ok(())
4948 }
4949
4950 #[cfg(feature = "alloc")]
4951 #[test]
4952 fn linkage_buf_retain_params_handles_shadowed_non_retained_params_by_slot()
4953 -> Result<(), Box<dyn StdError>> {
4954 let base: LinkageBuf<3, 0> = LinkageBuf::start()
4955 .define_param("x", 0.25)
4956 .forward_param("x", 0.0, 10.0)
4957 .define_param("y", 0.5)
4958 .left_param("y", 0.0, 10.0)
4959 .define_param("x", 0.75)
4960 .up_param("x", 0.0, 10.0);
4961
4962 let retained: LinkageBuf<1, 0> = base.retain_param_names(&["y"]);
4963
4964 let pos = retained.view().final_pose(&[1.0])?.position();
4965 assert!(pos.is_close_to(&Vec3::from([2.5, 10.0, 7.5]), 1e-4));
4966 Ok(())
4967 }
4968
4969 #[cfg(feature = "alloc")]
4970 #[test]
4971 fn linkage_buf_retain_param_names_uses_each_shadowed_slots_own_default()
4972 -> Result<(), Box<dyn StdError>> {
4973 let base: LinkageBuf<3, 0> = LinkageBuf::start()
4974 .define_param("x", 0.25)
4975 .forward_param("x", 0.0, 10.0)
4976 .define_param("y", 0.5)
4977 .left_param("y", 0.0, 10.0)
4978 .define_param("x", 0.75)
4979 .up_param("x", 0.0, 10.0);
4980
4981 let frozen: LinkageBuf<1, 0> = base.retain_param_names(&["y"]);
4982
4983 let pos = frozen.view().final_pose(&[1.0])?.position();
4984 assert!(pos.is_close_to(&Vec3::from([2.5, 10.0, 7.5]), 1e-4));
4985 Ok(())
4986 }
4987
4988 #[test]
4991 #[should_panic(expected = "freeze name not found in params")]
4992 fn freeze_param_name_rejects_unknown_name() {
4993 let linkage: LinkageFixed<1, 0, 4> = LinkageFixed::start()
4994 .define_param("angle", 0.5)
4995 .yaw_param("angle", -90.0, 90.0);
4996 let _: LinkageFixed<0, 0, 4> = linkage.freeze_param_name("typo", 0.0);
4997 }
4998
4999 #[test]
5000 #[should_panic(expected = "freeze param index out of bounds")]
5001 fn freeze_param_index_rejects_out_of_bounds_index() {
5002 let linkage: LinkageFixed<1, 0, 4> = LinkageFixed::start()
5003 .define_param("angle", 0.5)
5004 .yaw_param("angle", -90.0, 90.0);
5005 let _: LinkageFixed<0, 0, 4> = linkage.freeze_param_index(9, 0.0);
5006 }
5007
5008 #[test]
5009 #[should_panic(expected = "freeze name is ambiguous")]
5010 fn freeze_param_name_rejects_duplicate_name() {
5011 let linkage: LinkageFixed<2, 0, 4> = LinkageFixed::start()
5012 .define_param("angle", 0.5)
5013 .define_param("angle", 0.5)
5014 .yaw_param("angle", -90.0, 90.0);
5015 let _: LinkageFixed<1, 0, 4> = linkage.freeze_param_name("angle", 0.0);
5016 }
5017
5018 #[test]
5019 #[should_panic(expected = "raw freeze value out of range")]
5020 fn freeze_param_index_rejects_raw_value_above_range() {
5021 let linkage: LinkageFixed<1, 0, 4> = LinkageFixed::start()
5022 .define_param("angle", 0.5)
5023 .yaw_param("angle", -180.0, 180.0);
5024 let _: LinkageFixed<0, 0, 4> = linkage.freeze_param_index(0, 999.0);
5025 }
5026
5027 #[test]
5028 #[should_panic(expected = "raw freeze value out of range")]
5029 fn freeze_param_index_rejects_raw_value_outside_one_referenced_range() {
5030 let linkage: LinkageFixed<1, 0, 4> = LinkageFixed::start()
5031 .define_param("x", 0.5)
5032 .yaw_param("x", -180.0, 180.0)
5033 .forward_param("x", 0.0, 10.0);
5034 let _: LinkageFixed<0, 0, 4> = linkage.freeze_param_index(0, 90.0);
5035 }
5036
5037 #[test]
5038 #[should_panic(expected = "retain name not found in params")]
5039 fn retain_params_rejects_unknown_name() {
5040 let linkage: LinkageFixed<2, 0, 5> = LinkageFixed::start()
5041 .define_param("angle", 0.5)
5042 .define_param("dist", 0.5)
5043 .yaw_param("angle", -90.0, 90.0)
5044 .forward_param("dist", 0.0, 10.0);
5045 let _: LinkageFixed<1, 0, 5> = linkage.retain_param_names(&["unknown_param"]);
5046 }
5047
5048 #[test]
5049 #[should_panic(expected = "duplicate name in retain list")]
5050 fn retain_params_rejects_duplicate_name() {
5051 let linkage: LinkageFixed<2, 0, 5> = LinkageFixed::start()
5052 .define_param("angle", 0.5)
5053 .define_param("dist", 0.5)
5054 .yaw_param("angle", -90.0, 90.0)
5055 .forward_param("dist", 0.0, 10.0);
5056 let _: LinkageFixed<2, 0, 5> = linkage.retain_param_names(&["angle", "angle"]);
5057 }
5058
5059 #[test]
5060 #[should_panic(expected = "duplicate index in retain list")]
5061 fn retain_param_indexes_rejects_duplicate_index() {
5062 let linkage: LinkageFixed<2, 0, 5> = LinkageFixed::start()
5063 .define_param("angle", 0.5)
5064 .define_param("dist", 0.5)
5065 .yaw_param("angle", -90.0, 90.0)
5066 .forward_param("dist", 0.0, 10.0);
5067 let _: LinkageFixed<2, 0, 5> = linkage.retain_param_indexes(&[0, 0]);
5068 }
5069
5070 #[test]
5071 fn retain_params_output_follows_original_param_order() -> Result<(), Box<dyn StdError>> {
5072 const BASE: LinkageFixed<3, 0, 6> = LinkageFixed::start()
5075 .define_param("x", 0.5)
5076 .define_param("y", 0.5)
5077 .define_param("z", 0.5)
5078 .forward_param("x", 0.0, 3.0)
5079 .left_param("z", 0.0, 7.0);
5080
5081 const RETAINED: LinkageFixed<2, 0, 6> = BASE.retain_param_names(&["z", "x"]);
5082
5083 let pos = RETAINED.view().final_pose(&[1.0, 0.0])?.position();
5086 assert!(pos.is_close_to(&Vec3::from([3.0, 0.0, 0.0]), 1e-4));
5087
5088 let pos2 = RETAINED.view().final_pose(&[1.0, 1.0])?.position();
5090 assert!(pos2.is_close_to(&Vec3::from([3.0, 7.0, 0.0]), 1e-4));
5091 Ok(())
5092 }
5093
5094 #[test]
5095 fn freeze_covers_all_translation_and_rotation_step_types() -> Result<(), Box<dyn StdError>> {
5096 const BASE: LinkageFixed<1, 0, 8> = LinkageFixed::start()
5100 .define_param("t", 0.5)
5101 .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(&[]);
5109
5110 let pos = FROZEN.view().final_pose(&[])?.position();
5112 assert!(pos.is_close_to(&Vec3::from([0.5, 1.0, 1.5]), 1e-4));
5113 Ok(())
5114 }
5115
5116 #[test]
5117 fn freeze_param_index_at_default_supports_multiple_step_ranges() -> Result<(), Box<dyn StdError>>
5118 {
5119 const BASE: LinkageFixed<1, 0, 4> = LinkageFixed::start()
5122 .define_param("t", 0.5)
5123 .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);
5127
5128 let pos = FROZEN.view().final_pose(&[])?.position();
5129 assert!(pos.is_close_to(&Vec3::from([5.0, 10.0, 0.0]), 1e-4));
5130 Ok(())
5131 }
5132
5133 #[test]
5134 fn linkage_view_materializes_active_step_count() {
5135 const BASE: LinkageFixed<0, 0, 5> = LinkageFixed::start()
5136 .yaw(0.0)
5137 .forward(2.0)
5138 .left(0.0)
5139 .up(1.0);
5140
5141 const STRIPPED: LinkageView<'static, 0, 0> = BASE.view();
5142
5143 let steps = STRIPPED.steps();
5144 assert_eq!(steps.len(), 5);
5145 assert!(matches!(steps[0], Step::Start));
5146 assert!(matches!(steps[1], Step::Yaw(StepArg::Fixed(value)) if value == 0.0));
5147 assert_fixed_move(steps[2], 2.0);
5148 assert!(matches!(steps[3], Step::Left(StepArg::Fixed(value)) if value == 0.0));
5149 assert_fixed_up(steps[4], 1.0);
5150 }
5151
5152 #[test]
5153 fn linkage_fixed_freeze_runs_cleanup_passes() -> Result<(), Box<dyn StdError>> {
5154 const BASE: LinkageFixed<1, 0, 6> = LinkageFixed::start()
5155 .define_param("t", 0.5)
5156 .yaw_param("t", -90.0, 90.0)
5157 .forward_param("t", 0.0, 4.0)
5158 .forward(6.0)
5159 .left_param("t", -2.0, 2.0)
5160 .up(1.0);
5161
5162 const FROZEN: LinkageFixed<0, 0, 6> = BASE.freeze_param_index_at_default(0);
5163
5164 let steps = FROZEN.view().steps();
5165 assert_eq!(steps.len(), 3);
5166 assert!(matches!(steps[0], Step::Start));
5167 assert_fixed_move(steps[1], 8.0);
5168 assert_fixed_up(steps[2], 1.0);
5169
5170 let original_pose = BASE.view().final_pose(&[0.5])?;
5171 let frozen_pose = FROZEN.view().final_pose(&[])?;
5172 assert_pose_close(original_pose, frozen_pose, 1e-5);
5173 Ok(())
5174 }
5175
5176 #[cfg(feature = "alloc")]
5177 #[test]
5178 fn linkage_buf_freeze_runs_cleanup_passes() -> Result<(), Box<dyn StdError>> {
5179 let base: LinkageBuf<1, 0> = LinkageBuf::start()
5180 .define_param("t", 0.5)
5181 .yaw_param("t", -90.0, 90.0)
5182 .forward_param("t", 0.0, 4.0)
5183 .forward(6.0)
5184 .left_param("t", -2.0, 2.0)
5185 .up(1.0);
5186
5187 let frozen: LinkageBuf<0, 0> = base.clone().freeze_param_index_at_default(0);
5188
5189 let steps = frozen.view().steps();
5190 assert_eq!(steps.len(), 3);
5191 assert!(matches!(steps[0], Step::Start));
5192 assert_fixed_move(steps[1], 8.0);
5193 assert_fixed_up(steps[2], 1.0);
5194
5195 let original_pose = base.view().final_pose(&[0.5])?;
5196 let frozen_pose = frozen.view().final_pose(&[])?;
5197 assert_pose_close(original_pose, frozen_pose, 1e-5);
5198 Ok(())
5199 }
5200
5201 #[cfg(feature = "alloc")]
5202 #[test]
5203 #[should_panic(expected = "freeze name not found in params")]
5204 fn linkage_buf_freeze_param_name_rejects_unknown_name() {
5205 let linkage: LinkageBuf<1, 0> = LinkageBuf::start()
5206 .define_param("angle", 0.5)
5207 .yaw_param("angle", -90.0, 90.0);
5208 let _: LinkageBuf<0, 0> = linkage.freeze_param_name("typo", 0.0);
5209 }
5210
5211 #[cfg(feature = "alloc")]
5212 #[test]
5213 #[should_panic(expected = "retain name not found in params")]
5214 fn linkage_buf_retain_params_rejects_unknown_name() {
5215 let linkage: LinkageBuf<2, 0> = LinkageBuf::start()
5216 .define_param("angle", 0.5)
5217 .define_param("dist", 0.5)
5218 .yaw_param("angle", -90.0, 90.0)
5219 .forward_param("dist", 0.0, 10.0);
5220 let _: LinkageBuf<1, 0> = linkage.retain_param_names(&["unknown_param"]);
5221 }
5222
5223 #[cfg(feature = "alloc")]
5224 #[test]
5225 #[should_panic(expected = "duplicate name in retain list")]
5226 fn linkage_buf_retain_params_rejects_duplicate_name() {
5227 let linkage: LinkageBuf<2, 0> = LinkageBuf::start()
5228 .define_param("angle", 0.5)
5229 .define_param("dist", 0.5)
5230 .yaw_param("angle", -90.0, 90.0)
5231 .forward_param("dist", 0.0, 10.0);
5232 let _: LinkageBuf<2, 0> = linkage.retain_param_names(&["angle", "angle"]);
5233 }
5234
5235 #[cfg(feature = "alloc")]
5236 #[test]
5237 fn linkage_buf_freeze_output_matches_linkage_fixed_freeze_output()
5238 -> Result<(), Box<dyn StdError>> {
5239 const FIXED_BASE: LinkageFixed<2, 0, 5> = LinkageFixed::start()
5242 .define_param("angle", 0.5)
5243 .define_param("dist", 0.5)
5244 .yaw_param("angle", -180.0, 180.0)
5245 .forward_param("dist", 0.0, 8.0);
5246
5247 const FIXED_FROZEN: LinkageFixed<1, 0, 5> = FIXED_BASE.freeze_param_name("angle", -90.0);
5248
5249 let buf_base: LinkageBuf<2, 0> = LinkageBuf::start()
5250 .define_param("angle", 0.5)
5251 .define_param("dist", 0.5)
5252 .yaw_param("angle", -180.0, 180.0)
5253 .forward_param("dist", 0.0, 8.0);
5254 let buf_frozen: LinkageBuf<1, 0> = buf_base.freeze_param_name("angle", -90.0);
5255
5256 for t in [0.0f32, 0.25, 0.5, 0.75, 1.0] {
5257 let pos_fixed = FIXED_FROZEN.view().final_pose(&[t])?.position();
5258 let pos_buf = buf_frozen.view().final_pose(&[t])?.position();
5259 assert!(pos_fixed.is_close_to(&pos_buf, 1e-5));
5260 }
5261 Ok(())
5262 }
5263
5264 fn assert_specialized_matches_original<
5265 const DOF: usize,
5266 const OUT_DOF: usize,
5267 const MARKS: usize,
5268 const N: usize,
5269 >(
5270 original: LinkageFixed<DOF, MARKS, N>,
5271 original_params: &[f32; DOF],
5272 specialized: LinkageFixed<OUT_DOF, MARKS, N>,
5273 specialized_params: &[f32; OUT_DOF],
5274 ) -> Result<(), Box<dyn StdError>> {
5275 assert_pose_close(
5276 original.view().final_pose(original_params)?,
5277 specialized.view().final_pose(specialized_params)?,
5278 1e-4,
5279 );
5280 assert_draw_items_3d_close(
5281 original.view().draw_items_3d(original_params)?,
5282 specialized.view().draw_items_3d(specialized_params)?,
5283 1e-4,
5284 );
5285 Ok(())
5286 }
5287
5288 fn assert_draw_items_3d_close(
5289 mut left: impl Iterator<Item = Item3d>,
5290 mut right: impl Iterator<Item = Item3d>,
5291 tolerance: f32,
5292 ) {
5293 loop {
5294 match (left.next(), right.next()) {
5295 (Some(left), Some(right)) => assert_draw_item_3d_close(left, right, tolerance),
5296 (None, None) => break,
5297 (Some(_), None) => panic!("specialized linkage emitted fewer draw items"),
5298 (None, Some(_)) => panic!("specialized linkage emitted more draw items"),
5299 }
5300 }
5301 }
5302
5303 fn assert_draw_item_3d_close(left: Item3d, right: Item3d, tolerance: f32) {
5304 match (left, right) {
5305 (Item3d::Stroke(left), Item3d::Stroke(right)) => {
5306 assert_pose_close(left.start(), right.start(), tolerance);
5307 assert_pose_close(left.end(), right.end(), tolerance);
5308 assert!((left.width() - right.width()).abs() <= tolerance);
5309 }
5310 (Item3d::Disk(left), Item3d::Disk(right)) => {
5311 assert_pose_close(left.pose(), right.pose(), tolerance);
5312 assert!((left.radius() - right.radius()).abs() <= tolerance);
5313 }
5314 (Item3d::Sphere(left), Item3d::Sphere(right)) => {
5315 assert_pose_close(left.pose(), right.pose(), tolerance);
5316 assert!((left.radius() - right.radius()).abs() <= tolerance);
5317 }
5318 _ => panic!("draw item variants differ"),
5319 }
5320 }
5321
5322 fn assert_pose_close(left: Pose, right: Pose, tolerance: f32) {
5323 assert!(left.position().is_close_to(&right.position(), tolerance));
5324 assert!(
5325 left.orientation()
5326 .is_close_to(&right.orientation(), tolerance)
5327 );
5328 }
5329
5330 fn assert_fixed_move(step: Step, expected: f32) {
5331 match step {
5332 Step::Forward(StepArg::Fixed(actual)) => assert!((actual - expected).abs() <= 1e-6),
5333 _ => panic!("expected fixed forward step"),
5334 }
5335 }
5336
5337 fn assert_fixed_up(step: Step, expected: f32) {
5338 match step {
5339 Step::Up(StepArg::Fixed(actual)) => assert!((actual - expected).abs() <= 1e-6),
5340 _ => panic!("expected fixed up step"),
5341 }
5342 }
5343}