forked from 1b-agent/fieldcraft-upstream
fem: mortar segmentation of face pairs with forward-mode automatic differentiation
The overlap of a secondary face with a main face (projection onto the secondary plane, Sutherland-Hodgman clipping of their triangles, Radon's seven-point rule on the overlap's fan) and the inverse linear and bilinear maps of both faces, written over a Real scalar so that f64, Dual (gradient) and Hyper (gradient and packed Hessian, 24 variables) give the values and the exact linearisation of the segmentation, the normal and the projection. Co-Authored-By: Claude Opus 5.5 <[email protected]> Claude-Session: https://claude.ai/code/session_01UhhZMBTviDQxZvwnyP89fo
This commit is contained in:
1 parent
e8b0831884
commit
37f338c9e8
3 files changed
+828
No files matched your search
@@ -0,0 +1,341 @@
|
||||
//! Forward-mode automatic differentiation in [`VARS`] variables, for kernels whose exact first
|
||||
//! and second derivatives are too long to write by hand (the mortar contact integrals of
|
||||
//! [`crate::mortar`], whose segmentation moves with the nodes).
|
||||
//!
|
||||
//! A kernel is written once over a [`Real`] scalar and run on `f64` for its value, on [`Dual`]
|
||||
//! for its gradient, or on [`Hyper`] for its gradient and Hessian. Arithmetic propagates the
|
||||
//! derivatives by the chain rule (truncated Taylor arithmetic: A. Griewank, A. Walther,
|
||||
//! *Evaluating Derivatives*, 2nd ed., SIAM 2008, ch. 3 and 13), so the derivatives are those of
|
||||
//! the code as it runs, to round-off: the branches a kernel takes (which polygon edges clip,
|
||||
//! which root of a quadratic) are decided on the values and differentiated where they are
|
||||
//! taken, which is the derivative of the piecewise smooth function wherever it is smooth. The
|
||||
//! automated linearisation of contact kernels this way is that of J. Lengiewicz, J. Korelc, S.
|
||||
//! Stupkiewicz, *Automation of finite element formulations for large deformation contact
|
||||
//! problems*, IJNME 85:1252–1279, 2011.
|
||||
//!
|
||||
//! [`jet::Jet`](crate::jet::Jet) is the three-variable sibling the shape-function library uses.
|
||||
|
||||
use std::ops::{Add, Div, Mul, Neg, Sub};
|
||||
|
||||
/// Independent variables of [`Dual`] and [`Hyper`]: the corners of two faces of up to four
|
||||
/// nodes, three coordinates each.
|
||||
pub const VARS: usize = 24;
|
||||
|
||||
/// Entries of a packed symmetric [`VARS`] × [`VARS`] Hessian (its upper triangle by rows).
|
||||
pub const PACKED: usize = VARS * (VARS + 1) / 2;
|
||||
|
||||
/// The position of (i, j) in a packed upper triangle (either order).
|
||||
pub fn packed(i: usize, j: usize) -> usize {
|
||||
let (a, b) = if i <= j { (i, j) } else { (j, i) };
|
||||
// Row a starts after rows 0..a, of VARS, VARS − 1, … entries.
|
||||
a * VARS - a * a.saturating_sub(1) / 2 + (b - a)
|
||||
}
|
||||
|
||||
/// A scalar a kernel can be written over: `f64`, [`Dual`] or [`Hyper`].
|
||||
pub trait Real:
|
||||
Copy
|
||||
+ Add<Output = Self>
|
||||
+ Sub<Output = Self>
|
||||
+ Mul<Output = Self>
|
||||
+ Div<Output = Self>
|
||||
+ Neg<Output = Self>
|
||||
+ Add<f64, Output = Self>
|
||||
+ Sub<f64, Output = Self>
|
||||
+ Mul<f64, Output = Self>
|
||||
{
|
||||
/// A constant.
|
||||
fn cst(v: f64) -> Self;
|
||||
/// The value.
|
||||
fn val(&self) -> f64;
|
||||
fn sqrt(self) -> Self;
|
||||
}
|
||||
|
||||
impl Real for f64 {
|
||||
fn cst(v: f64) -> f64 {
|
||||
v
|
||||
}
|
||||
fn val(&self) -> f64 {
|
||||
*self
|
||||
}
|
||||
fn sqrt(self) -> f64 {
|
||||
f64::sqrt(self)
|
||||
}
|
||||
}
|
||||
|
||||
/// A value and its gradient.
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Dual {
|
||||
pub v: f64,
|
||||
pub g: [f64; VARS],
|
||||
}
|
||||
|
||||
impl Dual {
|
||||
/// Variable `k` at value `v` (a constant when `k` is out of range).
|
||||
pub fn var(v: f64, k: usize) -> Dual {
|
||||
let mut d = Dual::cst(v);
|
||||
if let Some(g) = d.g.get_mut(k) {
|
||||
*g = 1.0;
|
||||
}
|
||||
d
|
||||
}
|
||||
|
||||
fn chain(self, v: f64, d: f64) -> Dual {
|
||||
Dual { v, g: self.g.map(|x| d * x) }
|
||||
}
|
||||
}
|
||||
|
||||
impl Real for Dual {
|
||||
fn cst(v: f64) -> Dual {
|
||||
Dual { v, g: [0.0; VARS] }
|
||||
}
|
||||
fn val(&self) -> f64 {
|
||||
self.v
|
||||
}
|
||||
fn sqrt(self) -> Dual {
|
||||
let s = self.v.sqrt();
|
||||
self.chain(s, 0.5 / s)
|
||||
}
|
||||
}
|
||||
|
||||
impl Add for Dual {
|
||||
type Output = Dual;
|
||||
fn add(mut self, o: Dual) -> Dual {
|
||||
self.v += o.v;
|
||||
self.g.iter_mut().zip(o.g).for_each(|(a, b)| *a += b);
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
impl Sub for Dual {
|
||||
type Output = Dual;
|
||||
fn sub(mut self, o: Dual) -> Dual {
|
||||
self.v -= o.v;
|
||||
self.g.iter_mut().zip(o.g).for_each(|(a, b)| *a -= b);
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
impl Mul for Dual {
|
||||
type Output = Dual;
|
||||
fn mul(self, o: Dual) -> Dual {
|
||||
let mut g = [0.0; VARS];
|
||||
for k in 0..VARS {
|
||||
g[k] = self.v * o.g[k] + o.v * self.g[k];
|
||||
}
|
||||
Dual { v: self.v * o.v, g }
|
||||
}
|
||||
}
|
||||
|
||||
impl Div for Dual {
|
||||
type Output = Dual;
|
||||
fn div(self, o: Dual) -> Dual {
|
||||
let r = 1.0 / o.v;
|
||||
let v = self.v * r;
|
||||
let mut g = [0.0; VARS];
|
||||
for k in 0..VARS {
|
||||
g[k] = (self.g[k] - v * o.g[k]) * r;
|
||||
}
|
||||
Dual { v, g }
|
||||
}
|
||||
}
|
||||
|
||||
impl Neg for Dual {
|
||||
type Output = Dual;
|
||||
fn neg(self) -> Dual {
|
||||
Dual { v: -self.v, g: self.g.map(|x| -x) }
|
||||
}
|
||||
}
|
||||
|
||||
impl Add<f64> for Dual {
|
||||
type Output = Dual;
|
||||
fn add(mut self, o: f64) -> Dual {
|
||||
self.v += o;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
impl Sub<f64> for Dual {
|
||||
type Output = Dual;
|
||||
fn sub(mut self, o: f64) -> Dual {
|
||||
self.v -= o;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
impl Mul<f64> for Dual {
|
||||
type Output = Dual;
|
||||
fn mul(self, o: f64) -> Dual {
|
||||
Dual { v: self.v * o, g: self.g.map(|x| x * o) }
|
||||
}
|
||||
}
|
||||
|
||||
/// A value, its gradient and its Hessian (packed upper triangle, [`packed`]).
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Hyper {
|
||||
pub v: f64,
|
||||
pub g: [f64; VARS],
|
||||
pub h: [f64; PACKED],
|
||||
}
|
||||
|
||||
impl Hyper {
|
||||
/// Variable `k` at value `v` (a constant when `k` is out of range).
|
||||
pub fn var(v: f64, k: usize) -> Hyper {
|
||||
let mut d = Hyper::cst(v);
|
||||
if let Some(g) = d.g.get_mut(k) {
|
||||
*g = 1.0;
|
||||
}
|
||||
d
|
||||
}
|
||||
|
||||
/// ∂²/∂x_i∂x_j.
|
||||
pub fn hessian(&self, i: usize, j: usize) -> f64 {
|
||||
self.h.get(packed(i, j)).copied().unwrap_or(0.0)
|
||||
}
|
||||
|
||||
/// f(self) from f's value `f0`, first derivative `f1` and second derivative `f2` at the
|
||||
/// value: g = f1 g, H = f1 H + f2 g gᵀ.
|
||||
fn chain(&self, f0: f64, f1: f64, f2: f64) -> Hyper {
|
||||
let mut out = Hyper { v: f0, g: self.g.map(|x| f1 * x), h: [0.0; PACKED] };
|
||||
let mut at = 0;
|
||||
for i in 0..VARS {
|
||||
let gi = f2 * self.g[i];
|
||||
for j in i..VARS {
|
||||
out.h[at] = f1 * self.h[at] + gi * self.g[j];
|
||||
at += 1;
|
||||
}
|
||||
}
|
||||
out
|
||||
}
|
||||
}
|
||||
|
||||
impl Real for Hyper {
|
||||
fn cst(v: f64) -> Hyper {
|
||||
Hyper { v, g: [0.0; VARS], h: [0.0; PACKED] }
|
||||
}
|
||||
fn val(&self) -> f64 {
|
||||
self.v
|
||||
}
|
||||
fn sqrt(self) -> Hyper {
|
||||
let s = self.v.sqrt();
|
||||
self.chain(s, 0.5 / s, -0.25 / (s * s * s))
|
||||
}
|
||||
}
|
||||
|
||||
impl Add for Hyper {
|
||||
type Output = Hyper;
|
||||
fn add(mut self, o: Hyper) -> Hyper {
|
||||
self.v += o.v;
|
||||
self.g.iter_mut().zip(&o.g).for_each(|(a, b)| *a += b);
|
||||
self.h.iter_mut().zip(&o.h).for_each(|(a, b)| *a += b);
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
impl Sub for Hyper {
|
||||
type Output = Hyper;
|
||||
fn sub(mut self, o: Hyper) -> Hyper {
|
||||
self.v -= o.v;
|
||||
self.g.iter_mut().zip(&o.g).for_each(|(a, b)| *a -= b);
|
||||
self.h.iter_mut().zip(&o.h).for_each(|(a, b)| *a -= b);
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
impl Mul for Hyper {
|
||||
type Output = Hyper;
|
||||
fn mul(self, o: Hyper) -> Hyper {
|
||||
let mut out = Hyper { v: self.v * o.v, g: [0.0; VARS], h: [0.0; PACKED] };
|
||||
for k in 0..VARS {
|
||||
out.g[k] = self.v * o.g[k] + o.v * self.g[k];
|
||||
}
|
||||
let mut at = 0;
|
||||
for i in 0..VARS {
|
||||
let (a, b) = (self.g[i], o.g[i]);
|
||||
for j in i..VARS {
|
||||
out.h[at] = self.v * o.h[at] + o.v * self.h[at] + a * o.g[j] + b * self.g[j];
|
||||
at += 1;
|
||||
}
|
||||
}
|
||||
out
|
||||
}
|
||||
}
|
||||
|
||||
impl Div for Hyper {
|
||||
type Output = Hyper;
|
||||
fn div(self, o: Hyper) -> Hyper {
|
||||
let r = 1.0 / o.v;
|
||||
self * o.chain(r, -r * r, 2.0 * r * r * r)
|
||||
}
|
||||
}
|
||||
|
||||
impl Neg for Hyper {
|
||||
type Output = Hyper;
|
||||
fn neg(self) -> Hyper {
|
||||
Hyper { v: -self.v, g: self.g.map(|x| -x), h: self.h.map(|x| -x) }
|
||||
}
|
||||
}
|
||||
|
||||
impl Add<f64> for Hyper {
|
||||
type Output = Hyper;
|
||||
fn add(mut self, o: f64) -> Hyper {
|
||||
self.v += o;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
impl Sub<f64> for Hyper {
|
||||
type Output = Hyper;
|
||||
fn sub(mut self, o: f64) -> Hyper {
|
||||
self.v -= o;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
impl Mul<f64> for Hyper {
|
||||
type Output = Hyper;
|
||||
fn mul(self, o: f64) -> Hyper {
|
||||
Hyper { v: self.v * o, g: self.g.map(|x| x * o), h: self.h.map(|x| x * o) }
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
fn f<T: Real>(x: T, y: T) -> T {
|
||||
(x * x * y + T::cst(1.0)).sqrt() / (y - x * 0.5) + x * 3.0
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn packed_indices_cover_the_upper_triangle_once() {
|
||||
let mut seen = vec![false; PACKED];
|
||||
let mut at = 0;
|
||||
for i in 0..VARS {
|
||||
for j in i..VARS {
|
||||
assert_eq!(packed(i, j), at);
|
||||
assert_eq!(packed(j, i), at);
|
||||
seen[at] = true;
|
||||
at += 1;
|
||||
}
|
||||
}
|
||||
assert!(seen.iter().all(|s| *s));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn derivatives_match_finite_differences() {
|
||||
let (x, y) = (0.7, 2.3);
|
||||
let h = 1e-5;
|
||||
let fx = |x: f64, y: f64| f(x, y);
|
||||
let gx = (fx(x + h, y) - fx(x - h, y)) / (2.0 * h);
|
||||
let gy = (fx(x, y + h) - fx(x, y - h)) / (2.0 * h);
|
||||
let hxy = (fx(x + h, y + h) - fx(x + h, y - h) - fx(x - h, y + h) + fx(x - h, y - h)) / (4.0 * h * h);
|
||||
let hxx = (fx(x + h, y) - 2.0 * fx(x, y) + fx(x - h, y)) / (h * h);
|
||||
let d = f(Dual::var(x, 3), Dual::var(y, 17));
|
||||
assert!((d.v - fx(x, y)).abs() < 1e-14 && (d.g[3] - gx).abs() < 1e-8 && (d.g[17] - gy).abs() < 1e-8);
|
||||
let e = f(Hyper::var(x, 3), Hyper::var(y, 17));
|
||||
assert!((e.g[3] - gx).abs() < 1e-8 && (e.g[17] - gy).abs() < 1e-8);
|
||||
assert!((e.hessian(3, 17) - hxy).abs() < 1e-4 && (e.hessian(17, 3) - hxy).abs() < 1e-4, "{} {hxy}", e.hessian(3, 17));
|
||||
assert!((e.hessian(3, 3) - hxx).abs() < 1e-3);
|
||||
assert_eq!(e.hessian(0, 5), 0.0);
|
||||
}
|
||||
}
|
||||
@@ -30,6 +30,8 @@
|
||||
//! - [`face`]: consistent nodal forces of pressure on element faces;
|
||||
//! - [`contact`]: contact geometry: the closest point of an element face to a point and the
|
||||
//! bucket grid of the global search;
|
||||
//! - [`mortar`]: mortar segmentation of a secondary face against a main face and the integration
|
||||
//! points on the overlap, over [`dual`]'s automatic differentiation for the exact tangent;
|
||||
//! - [`heat`]: heat conduction (conductivity, capacity, sources, surface flux and convection,
|
||||
//! heat flux); thermal strain loads and stresses are in [`element`];
|
||||
//! - [`uniform`]: uniform-strain (one-point) hexahedron and tetrahedron kinematics, hourglass
|
||||
@@ -59,6 +61,7 @@ pub mod constitutive;
|
||||
pub mod contact;
|
||||
pub mod dense;
|
||||
pub mod dof;
|
||||
pub mod dual;
|
||||
pub mod elastoplastic;
|
||||
pub mod element;
|
||||
pub mod face;
|
||||
@@ -68,6 +71,7 @@ pub mod heat;
|
||||
pub mod hybrid;
|
||||
pub mod jet;
|
||||
pub mod material;
|
||||
pub mod mortar;
|
||||
pub mod plane;
|
||||
pub mod plasticity;
|
||||
pub mod quadrature;
|
||||
|
||||
@@ -0,0 +1,483 @@
|
||||
//! Mortar segmentation and integration: the overlap of a secondary (non-mortar) face with a main
|
||||
//! (mortar) face, and the integration points on it where both faces' shape functions and
|
||||
//! positions are known.
|
||||
//!
|
||||
//! **Segmentation** (M. A. Puso, T. A. Laursen, *A mortar segment-to-segment contact method for
|
||||
//! large deformation solid mechanics*, CMAME 193:601–629, 2004, §3; A. Popp, M. W. Gee, W. A.
|
||||
//! Wall, *A finite deformation mortar contact formulation using a primal–dual active set
|
||||
//! strategy*, IJNME 79:1354–1391, 2009, §3.3). Both faces are projected along the secondary
|
||||
//! face's unit normal n onto its plane (through its centroid), and the main polygon is clipped by
|
||||
//! the secondary one (Sutherland–Hodgman; a quadrilateral whose projection is not convex is
|
||||
//! split into its two triangles first). Each overlap polygon is split into a fan of triangles
|
||||
//! integrated with the seven-point degree-5 rule (J. Radon 1948; A. H. Stroud, *Approximate
|
||||
//! Calculation of Multiple Integrals*, Prentice-Hall 1971, T2:5-1), which integrates the products
|
||||
//! of two shape functions that the tie and the patch test need exactly on triangles and
|
||||
//! parallelograms (on a general quadrilateral the inverse of its map is not a polynomial, and the
|
||||
//! rule is accurate to its degree).
|
||||
//!
|
||||
//! **Points.** At an integration point y of the plane, each face's coordinates are found by
|
||||
//! inverting its projected map: the linear one of a triangle, or the bilinear one of a
|
||||
//! quadrilateral, x(u, v) = a + b u + c v + d u v, whose inverse is the root in [0, 1] of the
|
||||
//! quadratic (b × d) u² + (b × c − h × d) u − h × c = 0 (h = y − a, × the scalar cross product of
|
||||
//! the plane). The projection of a bilinear patch onto a plane is the bilinear patch of its
|
||||
//! projected corners, so the main face's point found this way is the point of the main surface
|
||||
//! along n through y, warped or not. The [`Sample`] then holds both faces' shape functions, both
|
||||
//! points in space, the normal and the weight (the plane's area element times the rule's weight).
|
||||
//!
|
||||
//! The kernel is written over [`Real`], so the same code gives the values (`f64`), their
|
||||
//! gradient ([`crate::dual::Dual`]) or their gradient and Hessian ([`crate::dual::Hyper`]) with
|
||||
//! respect to the corner coordinates, variables `3 i + k` for secondary corner i and
|
||||
//! `12 + 3 j + k` for main corner j: the exact linearisation of the segmentation, the normal and
|
||||
//! the projection, which the consistent tangent of mortar contact needs (Popp et al. 2009, §4).
|
||||
|
||||
use crate::dual::Real;
|
||||
|
||||
/// A point in space over a scalar.
|
||||
pub type Point<T> = [T; 3];
|
||||
|
||||
/// A point of the secondary face's plane.
|
||||
type Flat<T> = [T; 2];
|
||||
|
||||
/// Radon's seven-point rule on a triangle, exact for degree 5: barycentric coordinates of the
|
||||
/// second and third corners and weights summing to one.
|
||||
fn rule() -> [([f64; 2], f64); 7] {
|
||||
let s = 15f64.sqrt();
|
||||
let (a, b) = ((6.0 - s) / 21.0, (9.0 + 2.0 * s) / 21.0);
|
||||
let (c, d) = ((6.0 + s) / 21.0, (9.0 - 2.0 * s) / 21.0);
|
||||
let (wa, wc) = ((155.0 - s) / 1200.0, (155.0 + s) / 1200.0);
|
||||
[([1.0 / 3.0, 1.0 / 3.0], 0.225), ([a, a], wa), ([b, a], wa), ([a, b], wa), ([c, c], wc), ([d, c], wc), ([c, d], wc)]
|
||||
}
|
||||
|
||||
/// One integration point of a segment.
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct Sample<T> {
|
||||
/// The rule's weight times the plane's area element (the secondary face's projected area).
|
||||
pub weight: T,
|
||||
/// The secondary face's shape functions at the point (0 past its corners).
|
||||
pub secondary: [T; 4],
|
||||
/// The main face's shape functions at the point.
|
||||
pub main: [T; 4],
|
||||
/// The point on the secondary face, and on the main face along the normal.
|
||||
pub xs: Point<T>,
|
||||
pub xm: Point<T>,
|
||||
/// The secondary face's unit normal (outward from its element).
|
||||
pub normal: Point<T>,
|
||||
}
|
||||
|
||||
impl<T: Real> Sample<T> {
|
||||
/// The gap along the secondary normal, (x_m − x_s) · n: positive where the faces are apart.
|
||||
pub fn gap(&self) -> T {
|
||||
dot(sub(self.xm, self.xs), self.normal)
|
||||
}
|
||||
}
|
||||
|
||||
fn sub<T: Real>(a: Point<T>, b: Point<T>) -> Point<T> {
|
||||
[a[0] - b[0], a[1] - b[1], a[2] - b[2]]
|
||||
}
|
||||
|
||||
fn dot<T: Real>(a: Point<T>, b: Point<T>) -> T {
|
||||
a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
|
||||
}
|
||||
|
||||
fn cross<T: Real>(a: Point<T>, b: Point<T>) -> Point<T> {
|
||||
[a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
|
||||
}
|
||||
|
||||
fn scale<T: Real>(a: Point<T>, s: T) -> Point<T> {
|
||||
[a[0] * s, a[1] * s, a[2] * s]
|
||||
}
|
||||
|
||||
fn cross2<T: Real>(a: Flat<T>, b: Flat<T>) -> T {
|
||||
a[0] * b[1] - a[1] * b[0]
|
||||
}
|
||||
|
||||
fn sub2<T: Real>(a: Flat<T>, b: Flat<T>) -> Flat<T> {
|
||||
[a[0] - b[0], a[1] - b[1]]
|
||||
}
|
||||
|
||||
/// A face's area vector: its outward normal times its area (flat triangle; for a bilinear
|
||||
/// quadrilateral half the cross product of its diagonals, the area of its projection on the
|
||||
/// plane normal to it).
|
||||
pub fn area_vector<T: Real>(x: &[Point<T>]) -> Option<Point<T>> {
|
||||
let half = |v: Point<T>| scale(v, T::cst(0.5));
|
||||
match x {
|
||||
[a, b, c] => Some(half(cross(sub(*b, *a), sub(*c, *a)))),
|
||||
[a, b, c, d] => Some(half(cross(sub(*c, *a), sub(*d, *b)))),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// A projected face as convex counter-clockwise pieces for the clipping: itself when it is
|
||||
/// convex (no artificial diagonal, whose crossings with the other face's edges would change the
|
||||
/// overlap's polygons, and the integration's round-off, as the faces move), else its two
|
||||
/// triangles.
|
||||
fn pieces<T: Real>(points: &[Flat<T>]) -> Vec<Vec<Flat<T>>> {
|
||||
let mut whole = points.to_vec();
|
||||
if signed_area(&whole).val() < 0.0 {
|
||||
whole.reverse();
|
||||
}
|
||||
let n = whole.len();
|
||||
let convex = (0..n).all(|i| {
|
||||
let (a, b, c) = (whole[i], whole[(i + 1) % n], whole[(i + 2) % n]);
|
||||
cross2(sub2(b, a), sub2(c, b)).val() > 0.0
|
||||
});
|
||||
if convex || n == 3 {
|
||||
return vec![whole];
|
||||
}
|
||||
[[0, 1, 2], [0, 2, 3]].iter().map(|t| counter_clockwise(t, points)).collect()
|
||||
}
|
||||
|
||||
fn signed_area<T: Real>(p: &[Flat<T>]) -> T {
|
||||
let mut s = T::cst(0.0);
|
||||
for (i, a) in p.iter().enumerate() {
|
||||
let b = p.get((i + 1) % p.len()).copied().unwrap_or(*a);
|
||||
s = s + cross2(*a, b);
|
||||
}
|
||||
s * 0.5
|
||||
}
|
||||
|
||||
/// The corners `t` of `points`, counter-clockwise.
|
||||
fn counter_clockwise<T: Real>(t: &[usize; 3], points: &[Flat<T>]) -> Vec<Flat<T>> {
|
||||
let mut out: Vec<Flat<T>> = t.iter().filter_map(|i| points.get(*i).copied()).collect();
|
||||
if signed_area(&out).val() < 0.0 {
|
||||
out.reverse();
|
||||
}
|
||||
out
|
||||
}
|
||||
|
||||
/// A vertex this close to a clip edge, relative to the edge's length, is on it.
|
||||
const ON_EDGE: f64 = 1e-8;
|
||||
|
||||
/// `subject` clipped by the convex counter-clockwise polygon `clip` (Sutherland–Hodgman), the
|
||||
/// sides decided on the values. A vertex within [`ON_EDGE`] of a clip edge's line counts as on
|
||||
/// it (kept, and never cut): the intersection of two nearly coincident edges is ill-conditioned,
|
||||
/// its position and above all its derivatives swamped by round-off, while the vertex itself is
|
||||
/// as good a corner of the overlap to that precision. Edges are cut only between a vertex
|
||||
/// clearly inside and one clearly outside, where the intersection is well conditioned.
|
||||
fn clip<T: Real>(subject: &[Flat<T>], clip: &[Flat<T>]) -> Vec<Flat<T>> {
|
||||
let mut out: Vec<Flat<T>> = subject.to_vec();
|
||||
for (i, a) in clip.iter().enumerate() {
|
||||
let Some(b) = clip.get((i + 1) % clip.len()).copied() else { break };
|
||||
let input = std::mem::take(&mut out);
|
||||
let edge = sub2(b, *a);
|
||||
let eps = ON_EDGE * (edge[0].val() * edge[0].val() + edge[1].val() * edge[1].val());
|
||||
let side = |p: Flat<T>| cross2(edge, sub2(p, *a));
|
||||
for (j, p) in input.iter().enumerate() {
|
||||
let Some(q) = input.get((j + 1) % input.len()).copied() else { break };
|
||||
let (sp, sq) = (side(*p), side(q));
|
||||
if sp.val() >= -eps {
|
||||
out.push(*p);
|
||||
}
|
||||
if (sp.val() > eps && sq.val() < -eps) || (sp.val() < -eps && sq.val() > eps) {
|
||||
let t = sp / (sp - sq);
|
||||
out.push([p[0] + t * (q[0] - p[0]), p[1] + t * (q[1] - p[1])]);
|
||||
}
|
||||
}
|
||||
if out.is_empty() {
|
||||
break;
|
||||
}
|
||||
}
|
||||
out
|
||||
}
|
||||
|
||||
/// The shape functions of a face (of 3 or 4 projected corners `p`) at plane point `y`: the
|
||||
/// inverse of its linear or bilinear map. `None` when it has no inverse there.
|
||||
fn shape_at<T: Real>(p: &[Flat<T>], y: Flat<T>) -> Option<[T; 4]> {
|
||||
let zero = T::cst(0.0);
|
||||
match p {
|
||||
[p0, p1, p2] => {
|
||||
let (e1, e2, h) = (sub2(*p1, *p0), sub2(*p2, *p0), sub2(y, *p0));
|
||||
let det = cross2(e1, e2);
|
||||
if !(det.val().abs() > 0.0) {
|
||||
return None;
|
||||
}
|
||||
let v = cross2(h, e2) / det;
|
||||
let w = cross2(e1, h) / det;
|
||||
Some([T::cst(1.0) - v - w, v, w, zero])
|
||||
}
|
||||
[p0, p1, p2, p3] => {
|
||||
let b = sub2(*p1, *p0);
|
||||
let c = sub2(*p3, *p0);
|
||||
let d = [p0[0] - p1[0] + p2[0] - p3[0], p0[1] - p1[1] + p2[1] - p3[1]];
|
||||
let h = sub2(y, *p0);
|
||||
let k2 = cross2(b, d);
|
||||
let k1 = cross2(b, c) - cross2(h, d);
|
||||
let k0 = -cross2(h, c);
|
||||
let u = if k2.val().abs() <= 1e-12 * k1.val().abs() {
|
||||
if !(k1.val().abs() > 0.0) {
|
||||
return None;
|
||||
}
|
||||
-k0 / k1
|
||||
} else {
|
||||
// The roots of k2 u² + k1 u + k0, computed without cancellation; the one in [0, 1]
|
||||
// (the nearer when round-off puts both outside).
|
||||
let disc = k1 * k1 - k2 * k0 * 4.0;
|
||||
let root = if disc.val() > 0.0 { disc.sqrt() } else { T::cst(0.0) };
|
||||
let q = if k1.val() >= 0.0 { -(k1 + root) * 0.5 } else { (root - k1) * 0.5 };
|
||||
let mut candidates = [None, None];
|
||||
if q.val().abs() > 0.0 {
|
||||
candidates[0] = Some(q / k2);
|
||||
candidates[1] = Some(k0 / q);
|
||||
} else {
|
||||
candidates[0] = Some(-k1 / (k2 * 2.0));
|
||||
}
|
||||
let distance = |u: f64| {
|
||||
if u < 0.0 {
|
||||
-u
|
||||
} else if u > 1.0 {
|
||||
u - 1.0
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
};
|
||||
candidates.into_iter().flatten().filter(|u| u.val().is_finite()).min_by(|a, b| distance(a.val()).total_cmp(&distance(b.val())))?
|
||||
};
|
||||
// v from the better conditioned component of h − u b = v (c + d u).
|
||||
let m = [c[0] + d[0] * u, c[1] + d[1] * u];
|
||||
let k = if m[0].val().abs() >= m[1].val().abs() { 0 } else { 1 };
|
||||
if !(m[k].val().abs() > 0.0) {
|
||||
return None;
|
||||
}
|
||||
let v = (h[k] - b[k] * u) / m[k];
|
||||
let (s, t) = (u * 2.0 - 1.0, v * 2.0 - 1.0);
|
||||
let one = T::cst(1.0);
|
||||
Some([(one - s) * (one - t) * 0.25, (one + s) * (one - t) * 0.25, (one + s) * (one + t) * 0.25, (one - s) * (one + t) * 0.25])
|
||||
}
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
fn interpolate<T: Real>(x: &[Point<T>], n: &[T; 4]) -> Point<T> {
|
||||
let mut out = [T::cst(0.0); 3];
|
||||
for (xi, ni) in x.iter().zip(n) {
|
||||
for k in 0..3 {
|
||||
out[k] = out[k] + xi[k] * *ni;
|
||||
}
|
||||
}
|
||||
out
|
||||
}
|
||||
|
||||
/// Integrate over the overlap of secondary face `secondary` with main face `main` (3 or 4
|
||||
/// corners each, in their faces' node order): `f` is called at every integration point. `None`
|
||||
/// when a face has another number of corners or the secondary face has no normal (collapsed);
|
||||
/// faces that do not overlap give no point.
|
||||
pub fn integrate<T: Real>(secondary: &[Point<T>], main: &[Point<T>], mut f: impl FnMut(&Sample<T>)) -> Option<()> {
|
||||
if !matches!(main.len(), 3 | 4) {
|
||||
return None;
|
||||
}
|
||||
let area = area_vector(secondary)?;
|
||||
let length = dot(area, area).sqrt();
|
||||
if !(length.val() > 0.0 && length.val().is_finite()) {
|
||||
return None;
|
||||
}
|
||||
let normal = scale(area, T::cst(1.0) / length);
|
||||
let count = T::cst(secondary.len() as f64);
|
||||
let mut center = [T::cst(0.0); 3];
|
||||
for x in secondary {
|
||||
for k in 0..3 {
|
||||
center[k] = center[k] + x[k] / count;
|
||||
}
|
||||
}
|
||||
// A basis of the plane, (e1, e2, n) right-handed, e1 towards the first corner.
|
||||
let r = sub(*secondary.first()?, center);
|
||||
let r = sub(r, scale(normal, dot(r, normal)));
|
||||
let rl = dot(r, r).sqrt();
|
||||
if !(rl.val() > 0.0) {
|
||||
return None;
|
||||
}
|
||||
let e1 = scale(r, T::cst(1.0) / rl);
|
||||
let e2 = cross(normal, e1);
|
||||
let flat = |x: &Point<T>| -> Flat<T> {
|
||||
let d = sub(*x, center);
|
||||
[dot(d, e1), dot(d, e2)]
|
||||
};
|
||||
let own: Vec<Flat<T>> = secondary.iter().map(flat).collect();
|
||||
let theirs: Vec<Flat<T>> = main.iter().map(flat).collect();
|
||||
let rule = rule();
|
||||
let (own_pieces, their_pieces) = (pieces(&own), pieces(&theirs));
|
||||
for s in &own_pieces {
|
||||
for m in &their_pieces {
|
||||
let overlap = clip(m, s);
|
||||
let Some(o) = overlap.first().copied() else { continue };
|
||||
for w in overlap.windows(2).skip(1) {
|
||||
let (p, q) = (w[0], w[1]);
|
||||
let doubled = cross2(sub2(p, o), sub2(q, o));
|
||||
if !(doubled.val() > 0.0) {
|
||||
continue;
|
||||
}
|
||||
for (bary, weight) in rule {
|
||||
let y = [0, 1].map(|k| o[k] + (p[k] - o[k]) * bary[0] + (q[k] - o[k]) * bary[1]);
|
||||
let (Some(ns), Some(nm)) = (shape_at(&own, y), shape_at(&theirs, y)) else { continue };
|
||||
f(&Sample {
|
||||
weight: doubled * (0.5 * weight),
|
||||
secondary: ns,
|
||||
main: nm,
|
||||
xs: interpolate(secondary, &ns),
|
||||
xm: interpolate(main, &nm),
|
||||
normal,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Some(())
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::dual::{Dual, Hyper, VARS};
|
||||
|
||||
fn square(x0: f64, y0: f64, side: f64, z: f64, up: bool) -> Vec<[f64; 3]> {
|
||||
let c = [[x0, y0, z], [x0 + side, y0, z], [x0 + side, y0 + side, z], [x0, y0 + side, z]];
|
||||
if up { c.to_vec() } else { vec![c[0], c[3], c[2], c[1]] }
|
||||
}
|
||||
|
||||
/// ∫ N_a over the overlap and ∫ N_a g, per secondary corner.
|
||||
fn integrals(s: &[[f64; 3]], m: &[[f64; 3]]) -> ([f64; 4], [f64; 4], f64) {
|
||||
let (mut d, mut g, mut area) = ([0.0; 4], [0.0; 4], 0.0);
|
||||
integrate(s, m, |p| {
|
||||
area += p.weight;
|
||||
for a in 0..4 {
|
||||
d[a] += p.weight * p.secondary[a];
|
||||
g[a] += p.weight * p.secondary[a] * p.gap();
|
||||
}
|
||||
})
|
||||
.unwrap();
|
||||
(d, g, area)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_covered_face_integrates_its_shape_functions_and_gap() {
|
||||
// Secondary face up (normal +z), main face facing it 0.1 above.
|
||||
let s = square(0.0, 0.0, 1.0, 0.0, true);
|
||||
let m = square(-0.5, -0.5, 2.0, 0.1, false);
|
||||
let (d, g, area) = integrals(&s, &m);
|
||||
assert!((area - 1.0).abs() < 1e-14);
|
||||
for a in 0..4 {
|
||||
assert!((d[a] - 0.25).abs() < 1e-14 && (g[a] - 0.025).abs() < 1e-14, "{d:?} {g:?}");
|
||||
}
|
||||
// Half covered: the overlap is the strip x > 0.5.
|
||||
let (d, _, area) = integrals(&s, &square(0.5, -0.5, 2.0, 0.1, false));
|
||||
assert!((area - 0.5).abs() < 1e-14);
|
||||
assert!((d[0] - 0.0625).abs() < 1e-14 && (d[1] - 0.1875).abs() < 1e-14, "{d:?}");
|
||||
// Apart: nothing.
|
||||
let (_, _, area) = integrals(&s, &square(3.0, 0.0, 1.0, 0.1, false));
|
||||
assert_eq!(area, 0.0);
|
||||
}
|
||||
|
||||
/// The sums over a 3 × 3 grid of main faces (triangles in one corner) under secondary face
|
||||
/// `s`, against a tensor-product Gauss rule on the face itself.
|
||||
fn covered(s: &[[f64; 3]; 4]) -> ([f64; 4], [f64; 4], f64, f64) {
|
||||
let mut d = [0.0; 4];
|
||||
let mut area = 0.0;
|
||||
for i in 0..3 {
|
||||
for j in 0..3 {
|
||||
let quad = square(-0.3 + 0.6 * i as f64, -0.3 + 0.6 * j as f64, 0.6, 0.2, false);
|
||||
let faces: Vec<Vec<[f64; 3]>> =
|
||||
if i == 0 && j == 0 { vec![vec![quad[0], quad[1], quad[2]], vec![quad[0], quad[2], quad[3]]] } else { vec![quad] };
|
||||
for f in faces {
|
||||
let (da, _, a) = integrals(s, &f);
|
||||
area += a;
|
||||
for k in 0..4 {
|
||||
d[k] += da[k];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
let mut reference = [0.0; 4];
|
||||
let mut exact = 0.0;
|
||||
let g = [-(0.6f64).sqrt(), 0.0, 0.6f64.sqrt()];
|
||||
let w = [5.0 / 9.0, 8.0 / 9.0, 5.0 / 9.0];
|
||||
for (a, wa) in g.iter().zip(w) {
|
||||
for (b, wb) in g.iter().zip(w) {
|
||||
let n = [(1.0 - a) * (1.0 - b), (1.0 + a) * (1.0 - b), (1.0 + a) * (1.0 + b), (1.0 - a) * (1.0 + b)].map(|v| v / 4.0);
|
||||
let dxa: [f64; 3] =
|
||||
std::array::from_fn(|k| 0.25 * (-(1.0 - b) * s[0][k] + (1.0 - b) * s[1][k] + (1.0 + b) * s[2][k] - (1.0 + b) * s[3][k]));
|
||||
let dxb: [f64; 3] =
|
||||
std::array::from_fn(|k| 0.25 * (-(1.0 - a) * s[0][k] - (1.0 + a) * s[1][k] + (1.0 + a) * s[2][k] + (1.0 - a) * s[3][k]));
|
||||
let j = crate::frame::norm(crate::frame::cross(dxa, dxb));
|
||||
exact += wa * wb * j;
|
||||
for k in 0..4 {
|
||||
reference[k] += wa * wb * j * n[k];
|
||||
}
|
||||
}
|
||||
}
|
||||
(d, reference, area, exact)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn non_matching_main_faces_add_up_to_the_covered_integrals() {
|
||||
// A parallelogram: its shape functions are polynomials of the plane, integrated exactly.
|
||||
let (d, reference, area, exact) = covered(&[[0.0, 0.0, 0.0], [1.0, 0.2, 0.0], [1.1, 1.1, 0.0], [0.1, 0.9, 0.0]]);
|
||||
assert!((area - exact).abs() < 1e-13, "{area} {exact}");
|
||||
for k in 0..4 {
|
||||
assert!((d[k] - reference[k]).abs() < 1e-13, "{d:?} {reference:?}");
|
||||
}
|
||||
// A general quadrilateral: its inverse map is not polynomial; the rule is accurate to
|
||||
// its degree.
|
||||
let (d, reference, area, exact) = covered(&[[0.0, 0.0, 0.0], [1.1, 0.1, 0.0], [1.0, 0.9, 0.0], [0.05, 1.2, 0.0]]);
|
||||
assert!((area - exact).abs() < 1e-13, "{area} {exact}");
|
||||
for k in 0..4 {
|
||||
assert!((d[k] - reference[k]).abs() < 1e-5 * reference[k], "{d:?} {reference:?}");
|
||||
}
|
||||
}
|
||||
|
||||
/// ∫ N_a g over a warped, tilted pair, as a function of all 24 coordinates.
|
||||
fn weighted_gap<T: Real>(x: &[T; VARS], a: usize) -> T {
|
||||
let s: Vec<Point<T>> = (0..4).map(|i| [x[3 * i], x[3 * i + 1], x[3 * i + 2]]).collect();
|
||||
let m: Vec<Point<T>> = (0..4).map(|j| [x[12 + 3 * j], x[13 + 3 * j], x[14 + 3 * j]]).collect();
|
||||
let mut g = T::cst(0.0);
|
||||
integrate(&s, &m, |p| g = g + p.weight * p.secondary[a] * p.gap()).unwrap();
|
||||
g
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn the_automatic_derivatives_are_those_of_the_integral() {
|
||||
let base: [f64; VARS] = [
|
||||
0.0, 0.0, 0.0, 1.0, 0.05, 0.02, 1.1, 0.9, -0.03, -0.05, 1.0, 0.01, // secondary
|
||||
-0.2, -0.3, 0.12, -0.25, 1.15, 0.08, 1.2, 1.1, 0.1, 0.9, -0.2, 0.15, // main, facing down
|
||||
];
|
||||
let a = 2;
|
||||
let duals: [Dual; VARS] = std::array::from_fn(|k| Dual::var(base[k], k));
|
||||
let hypers: [Hyper; VARS] = std::array::from_fn(|k| Hyper::var(base[k], k));
|
||||
let d = weighted_gap(&duals, a);
|
||||
let e = weighted_gap(&hypers, a);
|
||||
let v = weighted_gap(&base, a);
|
||||
assert!((d.v - v).abs() < 1e-14 && (e.v - v).abs() < 1e-14);
|
||||
let h = 1e-6;
|
||||
let shifted = |k: usize, by: f64| {
|
||||
let mut x = base;
|
||||
x[k] += by;
|
||||
x
|
||||
};
|
||||
for k in 0..VARS {
|
||||
let fd = (weighted_gap(&shifted(k, h), a) - weighted_gap(&shifted(k, -h), a)) / (2.0 * h);
|
||||
assert!((d.g[k] - fd).abs() < 1e-7 && (e.g[k] - d.g[k]).abs() < 1e-13, "gradient {k}: {} {fd}", d.g[k]);
|
||||
// The Hessian's row k against differences of the gradient.
|
||||
let (plus, minus) = (shifted(k, h), shifted(k, -h));
|
||||
let gp = weighted_gap(&std::array::from_fn::<Dual, VARS, _>(|i| Dual::var(plus[i], i)), a);
|
||||
let gm = weighted_gap(&std::array::from_fn::<Dual, VARS, _>(|i| Dual::var(minus[i], i)), a);
|
||||
for j in 0..VARS {
|
||||
let fd = (gp.g[j] - gm.g[j]) / (2.0 * h);
|
||||
assert!((e.hessian(k, j) - fd).abs() < 1e-6, "hessian ({k}, {j}): {} {fd}", e.hessian(k, j));
|
||||
}
|
||||
}
|
||||
// A rigid translation changes nothing: the gradient sums to zero per direction.
|
||||
for c in 0..3 {
|
||||
let sum: f64 = (0..8).map(|n| d.g[3 * n + c]).sum();
|
||||
assert!(sum.abs() < 1e-13, "{sum}");
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn degenerate_faces_give_nothing() {
|
||||
let s = square(0.0, 0.0, 1.0, 0.0, true);
|
||||
let collapsed = [[0.0; 3], [1.0, 0.0, 0.0], [2.0, 0.0, 0.0]];
|
||||
assert!(integrate(&collapsed, &s, |_| {}).is_none());
|
||||
assert!(integrate(&s, &s[..2], |_| {}).is_none());
|
||||
let mut count = 0;
|
||||
integrate(&s, &[[f64::NAN, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]], |_| count += 1);
|
||||
assert_eq!(count, 0);
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user