bbd4ee08b6
In the process, make points round, since the highlighting works better visually that way.
772 lines
No EOL
26 KiB
Rust
772 lines
No EOL
26 KiB
Rust
use nalgebra::{DMatrix, DVector, DVectorView};
|
|
use rustc_hash::FxHashMap;
|
|
use slab::Slab;
|
|
use std::{
|
|
any::{Any, TypeId},
|
|
cell::Cell,
|
|
collections::BTreeSet,
|
|
rc::Rc,
|
|
sync::atomic::{AtomicU64, Ordering}
|
|
};
|
|
use sycamore::prelude::*;
|
|
use web_sys::{console, wasm_bindgen::JsValue}; /* DEBUG */
|
|
|
|
use crate::{
|
|
display::DisplayItem,
|
|
engine::{
|
|
Q,
|
|
change_half_curvature,
|
|
local_unif_to_std,
|
|
point,
|
|
realize_gram,
|
|
sphere,
|
|
ConfigSubspace,
|
|
ConstraintProblem
|
|
},
|
|
outline::OutlineItem,
|
|
specified::SpecifiedValue
|
|
};
|
|
|
|
// the types of the keys we use to access an assembly's elements and regulators
|
|
pub type ElementKey = usize;
|
|
pub type RegulatorKey = usize;
|
|
|
|
pub type ElementColor = [f32; 3];
|
|
|
|
/* KLUDGE */
|
|
// we should reconsider this design when we build a system for switching between
|
|
// assemblies. at that point, we might want to switch to hierarchical keys,
|
|
// where each each element has a key that identifies it within its assembly and
|
|
// each assembly has a key that identifies it within the sesssion
|
|
static NEXT_ELEMENT_SERIAL: AtomicU64 = AtomicU64::new(0);
|
|
|
|
pub trait ProblemPoser {
|
|
fn pose(&self, problem: &mut ConstraintProblem, elts: &Slab<Rc<dyn Element>>);
|
|
}
|
|
|
|
pub trait Element: ProblemPoser + DisplayItem {
|
|
// the default identifier for an element of this type
|
|
fn default_id() -> String where Self: Sized;
|
|
|
|
// create the default example of an element of this type
|
|
fn default(id: String, id_num: u64) -> Self where Self: Sized;
|
|
|
|
// the regulators that should be created when an element of this type is
|
|
// inserted into the given assembly with the given storage key
|
|
/* KLUDGE */
|
|
// right now, this organization makes sense because regulators identify
|
|
// their subjects by storage key, so the element has to be inserted before
|
|
// its regulators can be created. if we change the way regulators identify
|
|
// their subjects, we should consider refactoring
|
|
fn default_regulators(_key: ElementKey, _assembly: &Assembly) -> Vec<Rc<dyn Regulator>> where Self: Sized {
|
|
Vec::new()
|
|
}
|
|
|
|
fn id(&self) -> &String;
|
|
fn label(&self) -> &String;
|
|
fn representation(&self) -> Signal<DVector<f64>>;
|
|
|
|
// the regulators the element is subject to. the assembly that owns the
|
|
// element is responsible for keeping this set up to date
|
|
fn regulators(&self) -> Signal<BTreeSet<RegulatorKey>>;
|
|
|
|
// a serial number that uniquely identifies this element
|
|
fn serial(&self) -> u64;
|
|
|
|
// take the next serial number, panicking if that was the last one left
|
|
fn next_serial() -> u64 where Self: Sized {
|
|
// the technique we use to panic on overflow is taken from _Rust Atomics
|
|
// and Locks_, by Mara Bos
|
|
//
|
|
// https://marabos.nl/atomics/atomics.html#example-handle-overflow
|
|
//
|
|
NEXT_ELEMENT_SERIAL.fetch_update(
|
|
Ordering::SeqCst, Ordering::SeqCst,
|
|
|serial| serial.checked_add(1)
|
|
).expect("Out of serial numbers for elements")
|
|
}
|
|
|
|
// the configuration matrix column index that was assigned to the element
|
|
// last time the assembly was realized, or `None` if the element has never
|
|
// been through a realization
|
|
fn column_index(&self) -> Option<usize>;
|
|
|
|
// assign the element a configuration matrix column index. this method must
|
|
// be used carefully to preserve invariant (1), described in the comment on
|
|
// the `tangent` field of the `Assembly` structure
|
|
fn set_column_index(&self, index: usize);
|
|
}
|
|
|
|
// the `Element` trait needs to be dyn-compatible, so its method signatures can
|
|
// only use `Self` in the type of the receiver. that means `Element` can't
|
|
// implement `PartialEq`. if you need partial equivalence for `Element` trait
|
|
// objects, use this wrapper
|
|
#[derive(Clone)]
|
|
pub struct ElementRc(pub Rc<dyn Element>);
|
|
|
|
impl PartialEq for ElementRc {
|
|
fn eq(&self, ElementRc(other): &Self) -> bool {
|
|
let ElementRc(rc) = self;
|
|
Rc::ptr_eq(rc, &other)
|
|
}
|
|
}
|
|
|
|
pub struct Sphere {
|
|
pub id: String,
|
|
pub label: String,
|
|
pub color: ElementColor,
|
|
pub representation: Signal<DVector<f64>>,
|
|
pub regulators: Signal<BTreeSet<RegulatorKey>>,
|
|
pub serial: u64,
|
|
column_index: Cell<Option<usize>>
|
|
}
|
|
|
|
impl Sphere {
|
|
const CURVATURE_COMPONENT: usize = 3;
|
|
|
|
pub fn new(
|
|
id: String,
|
|
label: String,
|
|
color: ElementColor,
|
|
representation: DVector<f64>
|
|
) -> Sphere {
|
|
Sphere {
|
|
id: id,
|
|
label: label,
|
|
color: color,
|
|
representation: create_signal(representation),
|
|
regulators: create_signal(BTreeSet::default()),
|
|
serial: Self::next_serial(),
|
|
column_index: None.into()
|
|
}
|
|
}
|
|
}
|
|
|
|
impl Element for Sphere {
|
|
fn default_id() -> String {
|
|
"sphere".to_string()
|
|
}
|
|
|
|
fn default(id: String, id_num: u64) -> Sphere {
|
|
Sphere::new(
|
|
id,
|
|
format!("Sphere {id_num}"),
|
|
[0.75_f32, 0.75_f32, 0.75_f32],
|
|
sphere(0.0, 0.0, 0.0, 1.0)
|
|
)
|
|
}
|
|
|
|
fn default_regulators(key: ElementKey, assembly: &Assembly) -> Vec<Rc<dyn Regulator>> {
|
|
vec![Rc::new(HalfCurvatureRegulator::new(key, assembly))]
|
|
}
|
|
|
|
fn id(&self) -> &String {
|
|
&self.id
|
|
}
|
|
|
|
fn label(&self) -> &String {
|
|
&self.label
|
|
}
|
|
|
|
fn representation(&self) -> Signal<DVector<f64>> {
|
|
self.representation
|
|
}
|
|
|
|
fn regulators(&self) -> Signal<BTreeSet<RegulatorKey>> {
|
|
self.regulators
|
|
}
|
|
|
|
fn serial(&self) -> u64 {
|
|
self.serial
|
|
}
|
|
|
|
fn column_index(&self) -> Option<usize> {
|
|
self.column_index.get()
|
|
}
|
|
|
|
fn set_column_index(&self, index: usize) {
|
|
self.column_index.set(Some(index));
|
|
}
|
|
}
|
|
|
|
impl ProblemPoser for Sphere {
|
|
fn pose(&self, problem: &mut ConstraintProblem, _elts: &Slab<Rc<dyn Element>>) {
|
|
let index = self.column_index().expect(
|
|
format!("Sphere \"{}\" should be indexed before writing problem data", self.id).as_str()
|
|
);
|
|
problem.gram.push_sym(index, index, 1.0);
|
|
problem.guess.set_column(index, &self.representation.get_clone_untracked());
|
|
}
|
|
}
|
|
|
|
pub struct Point {
|
|
pub id: String,
|
|
pub label: String,
|
|
pub color: ElementColor,
|
|
pub representation: Signal<DVector<f64>>,
|
|
pub regulators: Signal<BTreeSet<RegulatorKey>>,
|
|
pub serial: u64,
|
|
column_index: Cell<Option<usize>>
|
|
}
|
|
|
|
impl Point {
|
|
const WEIGHT_COMPONENT: usize = 3;
|
|
|
|
pub fn new(
|
|
id: String,
|
|
label: String,
|
|
color: ElementColor,
|
|
representation: DVector<f64>
|
|
) -> Point {
|
|
Point {
|
|
id,
|
|
label,
|
|
color,
|
|
representation: create_signal(representation),
|
|
regulators: create_signal(BTreeSet::default()),
|
|
serial: Self::next_serial(),
|
|
column_index: None.into()
|
|
}
|
|
}
|
|
}
|
|
|
|
impl Element for Point {
|
|
fn default_id() -> String {
|
|
"point".to_string()
|
|
}
|
|
|
|
fn default(id: String, id_num: u64) -> Point {
|
|
Point::new(
|
|
id,
|
|
format!("Point {id_num}"),
|
|
[0.75_f32, 0.75_f32, 0.75_f32],
|
|
point(0.0, 0.0, 0.0)
|
|
)
|
|
}
|
|
|
|
fn id(&self) -> &String {
|
|
&self.id
|
|
}
|
|
|
|
fn label(&self) -> &String {
|
|
&self.label
|
|
}
|
|
|
|
fn representation(&self) -> Signal<DVector<f64>> {
|
|
self.representation
|
|
}
|
|
|
|
fn regulators(&self) -> Signal<BTreeSet<RegulatorKey>> {
|
|
self.regulators
|
|
}
|
|
|
|
fn serial(&self) -> u64 {
|
|
self.serial
|
|
}
|
|
|
|
fn column_index(&self) -> Option<usize> {
|
|
self.column_index.get()
|
|
}
|
|
|
|
fn set_column_index(&self, index: usize) {
|
|
self.column_index.set(Some(index));
|
|
}
|
|
}
|
|
|
|
impl ProblemPoser for Point {
|
|
fn pose(&self, problem: &mut ConstraintProblem, _elts: &Slab<Rc<dyn Element>>) {
|
|
let index = self.column_index().expect(
|
|
format!("Point \"{}\" should be indexed before writing problem data", self.id).as_str()
|
|
);
|
|
problem.gram.push_sym(index, index, 0.0);
|
|
problem.frozen.push(Point::WEIGHT_COMPONENT, index, 0.5);
|
|
problem.guess.set_column(index, &self.representation.get_clone_untracked());
|
|
}
|
|
}
|
|
|
|
pub trait Regulator: ProblemPoser + OutlineItem {
|
|
fn subjects(&self) -> Vec<ElementKey>;
|
|
fn measurement(&self) -> ReadSignal<f64>;
|
|
fn set_point(&self) -> Signal<SpecifiedValue>;
|
|
|
|
// this method is used to responsively precondition the assembly for
|
|
// realization when the regulator becomes a constraint, or is edited while
|
|
// acting as a constraint. it should track the set point, do any desired
|
|
// preconditioning when the set point is present, and use its return value
|
|
// to report whether the set is present. the default implementation does no
|
|
// preconditioning
|
|
fn try_activate(&self, _assembly: &Assembly) -> bool {
|
|
self.set_point().with(|set_pt| set_pt.is_present())
|
|
}
|
|
}
|
|
|
|
pub struct InversiveDistanceRegulator {
|
|
pub subjects: [ElementKey; 2],
|
|
pub measurement: ReadSignal<f64>,
|
|
pub set_point: Signal<SpecifiedValue>
|
|
}
|
|
|
|
impl InversiveDistanceRegulator {
|
|
pub fn new(subjects: [ElementKey; 2], assembly: &Assembly) -> InversiveDistanceRegulator {
|
|
let measurement = assembly.elements.map(
|
|
move |elts| {
|
|
let representations = subjects.map(|subj| elts[subj].representation());
|
|
representations[0].with(|rep_0|
|
|
representations[1].with(|rep_1|
|
|
rep_0.dot(&(&*Q * rep_1))
|
|
)
|
|
)
|
|
}
|
|
);
|
|
|
|
let set_point = create_signal(SpecifiedValue::from_empty_spec());
|
|
|
|
InversiveDistanceRegulator { subjects, measurement, set_point }
|
|
}
|
|
}
|
|
|
|
impl Regulator for InversiveDistanceRegulator {
|
|
fn subjects(&self) -> Vec<ElementKey> {
|
|
self.subjects.into()
|
|
}
|
|
|
|
fn measurement(&self) -> ReadSignal<f64> {
|
|
self.measurement
|
|
}
|
|
|
|
fn set_point(&self) -> Signal<SpecifiedValue> {
|
|
self.set_point
|
|
}
|
|
}
|
|
|
|
impl ProblemPoser for InversiveDistanceRegulator {
|
|
fn pose(&self, problem: &mut ConstraintProblem, elts: &Slab<Rc<dyn Element>>) {
|
|
self.set_point.with_untracked(|set_pt| {
|
|
if let Some(val) = set_pt.value {
|
|
let [row, col] = self.subjects.map(
|
|
|subj| elts[subj].column_index().expect(
|
|
"Subjects should be indexed before inversive distance regulator writes problem data"
|
|
)
|
|
);
|
|
problem.gram.push_sym(row, col, val);
|
|
}
|
|
});
|
|
}
|
|
}
|
|
|
|
pub struct HalfCurvatureRegulator {
|
|
pub subject: ElementKey,
|
|
pub measurement: ReadSignal<f64>,
|
|
pub set_point: Signal<SpecifiedValue>
|
|
}
|
|
|
|
impl HalfCurvatureRegulator {
|
|
pub fn new(subject: ElementKey, assembly: &Assembly) -> HalfCurvatureRegulator {
|
|
let measurement = assembly.elements.map(
|
|
move |elts| elts[subject].representation().with(
|
|
|rep| rep[Sphere::CURVATURE_COMPONENT]
|
|
)
|
|
);
|
|
|
|
let set_point = create_signal(SpecifiedValue::from_empty_spec());
|
|
|
|
HalfCurvatureRegulator { subject, measurement, set_point }
|
|
}
|
|
}
|
|
|
|
impl Regulator for HalfCurvatureRegulator {
|
|
fn subjects(&self) -> Vec<ElementKey> {
|
|
vec![self.subject]
|
|
}
|
|
|
|
fn measurement(&self) -> ReadSignal<f64> {
|
|
self.measurement
|
|
}
|
|
|
|
fn set_point(&self) -> Signal<SpecifiedValue> {
|
|
self.set_point
|
|
}
|
|
|
|
fn try_activate(&self, assembly: &Assembly) -> bool {
|
|
match self.set_point.with(|set_pt| set_pt.value) {
|
|
Some(half_curv) => {
|
|
let representation = assembly.elements.with_untracked(
|
|
|elts| elts[self.subject].representation()
|
|
);
|
|
representation.update(
|
|
|rep| change_half_curvature(rep, half_curv)
|
|
);
|
|
true
|
|
}
|
|
None => false
|
|
}
|
|
}
|
|
}
|
|
|
|
impl ProblemPoser for HalfCurvatureRegulator {
|
|
fn pose(&self, problem: &mut ConstraintProblem, elts: &Slab<Rc<dyn Element>>) {
|
|
self.set_point.with_untracked(|set_pt| {
|
|
if let Some(val) = set_pt.value {
|
|
let col = elts[self.subject].column_index().expect(
|
|
"Subject should be indexed before half-curvature regulator writes problem data"
|
|
);
|
|
problem.frozen.push(Sphere::CURVATURE_COMPONENT, col, val);
|
|
}
|
|
});
|
|
}
|
|
}
|
|
|
|
// the velocity is expressed in uniform coordinates
|
|
pub struct ElementMotion<'a> {
|
|
pub key: ElementKey,
|
|
pub velocity: DVectorView<'a, f64>
|
|
}
|
|
|
|
type AssemblyMotion<'a> = Vec<ElementMotion<'a>>;
|
|
|
|
// a complete, view-independent description of an assembly
|
|
#[derive(Clone)]
|
|
pub struct Assembly {
|
|
// elements and regulators
|
|
pub elements: Signal<Slab<Rc<dyn Element>>>,
|
|
pub regulators: Signal<Slab<Rc<dyn Regulator>>>,
|
|
|
|
// solution variety tangent space. the basis vectors are stored in
|
|
// configuration matrix format, ordered according to the elements' column
|
|
// indices. when you realize the assembly, every element that's present
|
|
// during realization gets a column index and is reflected in the tangent
|
|
// space. since the methods in this module never assign column indices
|
|
// without later realizing the assembly, we get the following invariant:
|
|
//
|
|
// (1) if an element has a column index, its tangent motions can be found
|
|
// in that column of the tangent space basis matrices
|
|
//
|
|
pub tangent: Signal<ConfigSubspace>,
|
|
|
|
// indexing
|
|
pub elements_by_id: Signal<FxHashMap<String, ElementKey>>
|
|
}
|
|
|
|
impl Assembly {
|
|
pub fn new() -> Assembly {
|
|
Assembly {
|
|
elements: create_signal(Slab::new()),
|
|
regulators: create_signal(Slab::new()),
|
|
tangent: create_signal(ConfigSubspace::zero(0)),
|
|
elements_by_id: create_signal(FxHashMap::default())
|
|
}
|
|
}
|
|
|
|
// --- inserting elements and regulators ---
|
|
|
|
// insert an element into the assembly without checking whether we already
|
|
// have an element with the same identifier. any element that does have the
|
|
// same identifier will get kicked out of the `elements_by_id` index
|
|
fn insert_element_unchecked<T: Element + 'static>(&self, elt: T) -> ElementKey {
|
|
// insert the element
|
|
let id = elt.id().clone();
|
|
let key = self.elements.update(|elts| elts.insert(Rc::new(elt)));
|
|
self.elements_by_id.update(|elts_by_id| elts_by_id.insert(id, key));
|
|
|
|
// create and insert the element's default regulators
|
|
for reg in T::default_regulators(key, &self) {
|
|
self.insert_regulator(reg);
|
|
}
|
|
|
|
key
|
|
}
|
|
|
|
pub fn try_insert_element(&self, elt: impl Element + 'static) -> Option<ElementKey> {
|
|
let can_insert = self.elements_by_id.with_untracked(
|
|
|elts_by_id| !elts_by_id.contains_key(elt.id())
|
|
);
|
|
if can_insert {
|
|
Some(self.insert_element_unchecked(elt))
|
|
} else {
|
|
None
|
|
}
|
|
}
|
|
|
|
pub fn insert_element_default<T: Element + 'static>(&self) {
|
|
// find the next unused identifier in the default sequence
|
|
let default_id = T::default_id();
|
|
let mut id_num = 1;
|
|
let mut id = format!("{default_id}{id_num}");
|
|
while self.elements_by_id.with_untracked(
|
|
|elts_by_id| elts_by_id.contains_key(&id)
|
|
) {
|
|
id_num += 1;
|
|
id = format!("{default_id}{id_num}");
|
|
}
|
|
|
|
// create and insert the default example of `T`
|
|
let _ = self.insert_element_unchecked(T::default(id, id_num));
|
|
}
|
|
|
|
pub fn insert_regulator(&self, regulator: Rc<dyn Regulator>) {
|
|
// add the regulator to the assembly's regulator list
|
|
let key = self.regulators.update(
|
|
|regs| regs.insert(regulator.clone())
|
|
);
|
|
|
|
// add the regulator to each subject's regulator list
|
|
let subjects = regulator.subjects();
|
|
let subject_regulators: Vec<_> = self.elements.with_untracked(
|
|
|elts| subjects.into_iter().map(
|
|
|subj| elts[subj].regulators()
|
|
).collect()
|
|
);
|
|
for regulators in subject_regulators {
|
|
regulators.update(|regs| regs.insert(key));
|
|
}
|
|
|
|
// update the realization when the regulator becomes a constraint, or is
|
|
// edited while acting as a constraint
|
|
let self_for_effect = self.clone();
|
|
create_effect(move || {
|
|
/* DEBUG */
|
|
// log the regulator update
|
|
console::log_1(&JsValue::from(
|
|
format!("Updated regulator with subjects {:?}", regulator.subjects())
|
|
));
|
|
|
|
if regulator.try_activate(&self_for_effect) {
|
|
self_for_effect.realize();
|
|
}
|
|
});
|
|
|
|
/* DEBUG */
|
|
// print an updated list of regulators
|
|
console::log_1(&JsValue::from("Regulators:"));
|
|
self.regulators.with_untracked(|regs| {
|
|
for (_, reg) in regs.into_iter() {
|
|
console::log_1(&JsValue::from(format!(
|
|
" {:?}: {}",
|
|
reg.subjects(),
|
|
reg.set_point().with_untracked(
|
|
|set_pt| {
|
|
let spec = &set_pt.spec;
|
|
if spec.is_empty() {
|
|
"__".to_string()
|
|
} else {
|
|
spec.clone()
|
|
}
|
|
}
|
|
)
|
|
)));
|
|
}
|
|
});
|
|
}
|
|
|
|
// --- realization ---
|
|
|
|
pub fn realize(&self) {
|
|
// index the elements
|
|
self.elements.update_silent(|elts| {
|
|
for (index, (_, elt)) in elts.into_iter().enumerate() {
|
|
elt.set_column_index(index);
|
|
}
|
|
});
|
|
|
|
// set up the constraint problem
|
|
let problem = self.elements.with_untracked(|elts| {
|
|
let mut problem = ConstraintProblem::new(elts.len());
|
|
for (_, elt) in elts {
|
|
elt.pose(&mut problem, elts);
|
|
}
|
|
self.regulators.with_untracked(|regs| {
|
|
for (_, reg) in regs {
|
|
reg.pose(&mut problem, elts);
|
|
}
|
|
});
|
|
problem
|
|
});
|
|
|
|
/* DEBUG */
|
|
// log the Gram matrix
|
|
console::log_1(&JsValue::from("Gram matrix:"));
|
|
problem.gram.log_to_console();
|
|
|
|
/* DEBUG */
|
|
// log the initial configuration matrix
|
|
console::log_1(&JsValue::from("Old configuration:"));
|
|
for j in 0..problem.guess.nrows() {
|
|
let mut row_str = String::new();
|
|
for k in 0..problem.guess.ncols() {
|
|
row_str.push_str(format!(" {:>8.3}", problem.guess[(j, k)]).as_str());
|
|
}
|
|
console::log_1(&JsValue::from(row_str));
|
|
}
|
|
|
|
// look for a configuration with the given Gram matrix
|
|
let (config, tangent, success, history) = realize_gram(
|
|
&problem, 1.0e-12, 0.5, 0.9, 1.1, 200, 110
|
|
);
|
|
|
|
/* DEBUG */
|
|
// report the outcome of the search
|
|
console::log_1(&JsValue::from(
|
|
if success {
|
|
"Target accuracy achieved!"
|
|
} else {
|
|
"Failed to reach target accuracy"
|
|
}
|
|
));
|
|
console::log_2(&JsValue::from("Steps:"), &JsValue::from(history.scaled_loss.len() - 1));
|
|
console::log_2(&JsValue::from("Loss:"), &JsValue::from(*history.scaled_loss.last().unwrap()));
|
|
console::log_2(&JsValue::from("Tangent dimension:"), &JsValue::from(tangent.dim()));
|
|
|
|
if success {
|
|
// read out the solution
|
|
for (_, elt) in self.elements.get_clone_untracked() {
|
|
elt.representation().update(
|
|
|rep| rep.set_column(0, &config.column(elt.column_index().unwrap()))
|
|
);
|
|
}
|
|
|
|
// save the tangent space
|
|
self.tangent.set_silent(tangent);
|
|
}
|
|
}
|
|
|
|
// --- deformation ---
|
|
|
|
// project the given motion to the tangent space of the solution variety and
|
|
// move the assembly along it. the implementation is based on invariant (1)
|
|
// from above and the following additional invariant:
|
|
//
|
|
// (2) if an element is affected by a constraint, it has a column index
|
|
//
|
|
// we have this invariant because the assembly gets realized each time you
|
|
// add a constraint
|
|
pub fn deform(&self, motion: AssemblyMotion) {
|
|
/* KLUDGE */
|
|
// when the tangent space is zero, deformation won't do anything, but
|
|
// the attempt to deform should be registered in the UI. this console
|
|
// message will do for now
|
|
if self.tangent.with(|tan| tan.dim() <= 0 && tan.assembly_dim() > 0) {
|
|
console::log_1(&JsValue::from("The assembly is rigid"));
|
|
}
|
|
|
|
// give a column index to each moving element that doesn't have one yet.
|
|
// this temporarily breaks invariant (1), but the invariant will be
|
|
// restored when we realize the assembly at the end of the deformation.
|
|
// in the process, we find out how many matrix columns we'll need to
|
|
// hold the deformation
|
|
let realized_dim = self.tangent.with(|tan| tan.assembly_dim());
|
|
let motion_dim = self.elements.update_silent(|elts| {
|
|
let mut next_column_index = realized_dim;
|
|
for elt_motion in motion.iter() {
|
|
let moving_elt = &mut elts[elt_motion.key];
|
|
if moving_elt.column_index().is_none() {
|
|
moving_elt.set_column_index(next_column_index);
|
|
next_column_index += 1;
|
|
}
|
|
}
|
|
next_column_index
|
|
});
|
|
|
|
// project the element motions onto the tangent space of the solution
|
|
// variety and sum them to get a deformation of the whole assembly. the
|
|
// matrix `motion_proj` that holds the deformation has extra columns for
|
|
// any moving elements that aren't reflected in the saved tangent space
|
|
const ELEMENT_DIM: usize = 5;
|
|
let mut motion_proj = DMatrix::zeros(ELEMENT_DIM, motion_dim);
|
|
for elt_motion in motion {
|
|
// we can unwrap the column index because we know that every moving
|
|
// element has one at this point
|
|
let column_index = self.elements.with_untracked(
|
|
|elts| elts[elt_motion.key].column_index().unwrap()
|
|
);
|
|
|
|
if column_index < realized_dim {
|
|
// this element had a column index when we started, so by
|
|
// invariant (1), it's reflected in the tangent space
|
|
let mut target_columns = motion_proj.columns_mut(0, realized_dim);
|
|
target_columns += self.tangent.with(
|
|
|tan| tan.proj(&elt_motion.velocity, column_index)
|
|
);
|
|
} else {
|
|
// this element didn't have a column index when we started, so
|
|
// by invariant (2), it's unconstrained
|
|
let mut target_column = motion_proj.column_mut(column_index);
|
|
let unif_to_std = self.elements.with_untracked(
|
|
|elts| {
|
|
elts[elt_motion.key].representation().with_untracked(
|
|
|rep| local_unif_to_std(rep.as_view())
|
|
)
|
|
}
|
|
);
|
|
target_column += unif_to_std * elt_motion.velocity;
|
|
}
|
|
}
|
|
|
|
// step the assembly along the deformation. this changes the elements'
|
|
// normalizations, so we restore those afterward
|
|
/* KLUDGE */
|
|
// for now, we only restore the normalizations of spheres
|
|
for (_, elt) in self.elements.get_clone_untracked() {
|
|
elt.representation().update_silent(|rep| {
|
|
match elt.column_index() {
|
|
Some(column_index) => {
|
|
// step the assembly along the deformation
|
|
*rep += motion_proj.column(column_index);
|
|
|
|
if elt.type_id() == TypeId::of::<Sphere>() {
|
|
// restore normalization by contracting toward the
|
|
// last coordinate axis
|
|
let q_sp = rep.fixed_rows::<3>(0).norm_squared();
|
|
let half_q_lt = -2.0 * rep[3] * rep[4];
|
|
let half_q_lt_sq = half_q_lt * half_q_lt;
|
|
let scaling = half_q_lt + (q_sp + half_q_lt_sq).sqrt();
|
|
rep.fixed_rows_mut::<4>(0).scale_mut(1.0 / scaling);
|
|
}
|
|
},
|
|
None => {
|
|
console::log_1(&JsValue::from(
|
|
format!("No velocity to unpack for fresh element \"{}\"", elt.id())
|
|
))
|
|
}
|
|
};
|
|
});
|
|
}
|
|
|
|
// bring the configuration back onto the solution variety. this also
|
|
// gets the elements' column indices and the saved tangent space back in
|
|
// sync
|
|
self.realize();
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
#[should_panic(expected = "Sphere \"sphere\" should be indexed before writing problem data")]
|
|
fn unindexed_element_test() {
|
|
let _ = create_root(|| {
|
|
let elt = Sphere::default("sphere".to_string(), 0);
|
|
elt.pose(&mut ConstraintProblem::new(1), &Slab::new());
|
|
});
|
|
}
|
|
|
|
#[test]
|
|
#[should_panic(expected = "Subjects should be indexed before inversive distance regulator writes problem data")]
|
|
fn unindexed_subject_test_inversive_distance() {
|
|
let _ = create_root(|| {
|
|
let mut elts = Slab::<Rc<dyn Element>>::new();
|
|
let subjects = [0, 1].map(|k| {
|
|
elts.insert(
|
|
Rc::new(Sphere::default(format!("sphere{k}"), k))
|
|
)
|
|
});
|
|
elts[subjects[0]].set_column_index(0);
|
|
InversiveDistanceRegulator {
|
|
subjects: subjects,
|
|
measurement: create_memo(|| 0.0),
|
|
set_point: create_signal(SpecifiedValue::try_from("0.0".to_string()).unwrap())
|
|
}.pose(&mut ConstraintProblem::new(2), &elts);
|
|
});
|
|
}
|
|
} |