glen/dyna3
1
0
Fork 0
forked from StudioInfinity/dyna3
Code Pull requests Activity

feat: Point coordinate regulators (#118)

Browse source 
Implement regulators for the Euclidean coordinates of `Point` entities, automatically creating all three of them for each added point entity. When such a regulator is set, it freezes the corresponding representation coordinate to the set point. In addition, if all three coordinates of a given `Point` are set, the coradius coordinate (which holds the norm of the point) is frozen as well.

Note that a `PointCoordinateRegulator` must be created with a `Point` as the subject. This commit modifies `HalfCurvatureRegulator` analogously, so that it can only be created with a `Sphere`.
Co-authored-by: Glen Whitney <glen@studioinfinity.org>
Co-committed-by: Glen Whitney <glen@studioinfinity.org>
This commit is contained in:
Glen Whitney 2025-10-13 22:52:02 +00:00 • committed by Vectornaut
parent 978f70aac7
commit 2c8c09d20d
6 changed files with 180 additions and 49 deletions
Download patch file Download diff file Expand all files Collapse all files

124
app-proto/src/assembly.rs
View file

@ -1,10 +1,11 @@
use enum_iterator::{all, Sequence};
use nalgebra::{DMatrix, DVector, DVectorView};
use std::{
cell::Cell,
cmp::Ordering,
collections::{BTreeMap, BTreeSet},
fmt,
fmt::{Debug, Formatter},
fmt::{Debug, Display, Formatter},
hash::{Hash, Hasher},
rc::Rc,
sync::{atomic, atomic::AtomicU64},
@ -26,6 +27,7 @@ use crate::{
ConfigSubspace,
ConstraintProblem,
DescentHistory,
MatrixEntry,
Realization,
},
specified::SpecifiedValue,
@ -84,6 +86,14 @@ impl Ord for dyn Serial {
}
}
// Small helper function to generate consistent errors when there
// are indexing issues in a ProblemPoser
fn indexing_error(item: &str, name: &str, actor: &str) -> String {
format!(
"{item} \"{name}\" must be indexed before {actor} writes problem data"
)
}
pub trait ProblemPoser {
fn pose(&self, problem: &mut ConstraintProblem);
}
@ -125,8 +135,8 @@ pub trait Element: Serial + ProblemPoser + DisplayItem {
}
impl Debug for dyn Element {
fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), fmt::Error> {
self.id().fmt(f)
fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
Debug::fmt(&self.id(), f)
}
}
@ -249,8 +259,7 @@ impl Serial for Sphere {
impl ProblemPoser for Sphere {
fn pose(&self, problem: &mut ConstraintProblem) {
let index = self.column_index().expect(
format!("Sphere \"{}\" should be indexed before writing problem data", self.id).as_str()
);
indexing_error("Sphere", &self.id, "it").as_str());
problem.gram.push_sym(index, index, 1.0);
problem.guess.set_column(index, &self.representation.get_clone_untracked());
}
@ -269,6 +278,7 @@ pub struct Point {
impl Point {
const WEIGHT_COMPONENT: usize = 3;
const NORM_COMPONENT: usize = 4;
pub fn new(
id: String,
@ -302,6 +312,15 @@ impl Element for Point {
point(0.0, 0.0, 0.0),
)
}
fn default_regulators(self: Rc<Self>) -> Vec<Rc<dyn Regulator>> {
all::<Axis>()
.map(|axis| {
Rc::new(PointCoordinateRegulator::new(self.clone(), axis))
as Rc::<dyn Regulator>
})
.collect()
}
fn id(&self) -> &String {
&self.id
@ -345,8 +364,7 @@ impl Serial for Point {
impl ProblemPoser for Point {
fn pose(&self, problem: &mut ConstraintProblem) {
let index = self.column_index().expect(
format!("Point \"{}\" should be indexed before writing problem data", self.id).as_str()
);
indexing_error("Point", &self.id, "it").as_str());
problem.gram.push_sym(index, index, 0.0);
problem.frozen.push(Self::WEIGHT_COMPONENT, index, 0.5);
problem.guess.set_column(index, &self.representation.get_clone_untracked());
@ -436,8 +454,8 @@ impl ProblemPoser for InversiveDistanceRegulator {
if let Some(val) = set_pt.value {
let [row, col] = self.subjects.each_ref().map(
|subj| subj.column_index().expect(
"Subjects should be indexed before inversive distance regulator writes problem data"
)
indexing_error("Subject", subj.id(),
"inversive distance regulator").as_str())
);
problem.gram.push_sym(row, col, val);
}
@ -446,14 +464,14 @@ impl ProblemPoser for InversiveDistanceRegulator {
}
pub struct HalfCurvatureRegulator {
pub subject: Rc<dyn Element>,
pub subject: Rc<Sphere>,
pub measurement: ReadSignal<f64>,
pub set_point: Signal<SpecifiedValue>,
serial: u64,
}
impl HalfCurvatureRegulator {
pub fn new(subject: Rc<dyn Element>) -> Self {
pub fn new(subject: Rc<Sphere>) -> Self {
let measurement = subject.representation().map(
|rep| rep[Sphere::CURVATURE_COMPONENT]
);
@ -490,14 +508,85 @@ impl ProblemPoser for HalfCurvatureRegulator {
self.set_point.with_untracked(|set_pt| {
if let Some(val) = set_pt.value {
let col = self.subject.column_index().expect(
"Subject should be indexed before half-curvature regulator writes problem data"
);
indexing_error("Subject", &self.subject.id,
"half-curvature regulator").as_str());
problem.frozen.push(Sphere::CURVATURE_COMPONENT, col, val);
}
});
}
}
#[derive(Clone, Copy, Sequence)]
pub enum Axis { X = 0, Y = 1, Z = 2 }
impl Axis {
fn name(&self) -> &'static str {
match self { Axis::X => "X", Axis::Y => "Y", Axis::Z => "Z" }
}
}
impl Display for Axis {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
f.write_str(self.name())
}
}
pub struct PointCoordinateRegulator {
pub subject: Rc<Point>,
pub axis: Axis,
pub measurement: ReadSignal<f64>,
pub set_point: Signal<SpecifiedValue>,
serial: u64
}
impl PointCoordinateRegulator {
pub fn new(subject: Rc<Point>, axis: Axis) -> Self {
let measurement = subject.representation().map(
move |rep| rep[axis as usize]
);
let set_point = create_signal(SpecifiedValue::from_empty_spec());
Self { subject, axis, measurement, set_point, serial: Self::next_serial() }
}
}
impl Serial for PointCoordinateRegulator {
fn serial(&self) -> u64 { self.serial }
}
impl Regulator for PointCoordinateRegulator {
fn subjects(&self) -> Vec<Rc<dyn Element>> { vec![self.subject.clone()] }
fn measurement(&self) -> ReadSignal<f64> { self.measurement }
fn set_point(&self) -> Signal<SpecifiedValue> { self.set_point }
}
impl ProblemPoser for PointCoordinateRegulator {
fn pose(&self, problem: &mut ConstraintProblem) {
self.set_point.with_untracked(|set_pt| {
if let Some(val) = set_pt.value {
let col = self.subject.column_index().expect(
indexing_error("Subject", &self.subject.id,
"point-coordinate regulator").as_str());
problem.frozen.push(self.axis as usize, col, val);
// If all three of the subject's spatial coordinates have been
// frozen, then freeze its norm component:
let mut coords = [0.0; Axis::CARDINALITY];
let mut nset: usize = 0;
for &MatrixEntry {index, value} in &(problem.frozen) {
if index.1 == col && index.0 < Axis::CARDINALITY {
nset += 1;
coords[index.0] = value
}
}
if nset == Axis::CARDINALITY {
let [x, y, z] = coords;
problem.frozen.push(
Point::NORM_COMPONENT, col, point(x,y,z)[Point::NORM_COMPONENT]);
}
}
});
}
}
// the velocity is expressed in uniform coordinates
pub struct ElementMotion<'a> {
pub element: Rc<dyn Element>,
@ -698,6 +787,7 @@ impl Assembly {
/* DEBUG */
// log the Gram matrix
console_log!("Gram matrix:\n{}", problem.gram);
console_log!("Frozen entries:\n{}", problem.frozen);
/* DEBUG */
// log the initial configuration matrix
@ -857,7 +947,8 @@ mod tests {
use crate::engine;
#[test]
#[should_panic(expected = "Sphere \"sphere\" should be indexed before writing problem data")]
#[should_panic(expected =
"Sphere \"sphere\" must be indexed before it writes problem data")]
fn unindexed_element_test() {
let _ = create_root(|| {
let elt = Sphere::default("sphere".to_string(), 0);
@ -866,7 +957,8 @@ mod tests {
}
#[test]
#[should_panic(expected = "Subjects should be indexed before inversive distance regulator writes problem data")]
#[should_panic(expected = "Subject \"sphere1\" must be indexed before \
inversive distance regulator writes problem data")]
fn unindexed_subject_test_inversive_distance() {
let _ = create_root(|| {
let subjects = [0, 1].map(
@ -927,4 +1019,4 @@ mod tests {
assert!((final_half_curv / INITIAL_HALF_CURV - 1.0).abs() < DRIFT_TOL);
});
}
}
}