engeom 0.2.10__tar.gz → 0.2.12__tar.gz

{engeom-0.2.10 → engeom-0.2.12}/Cargo.lock

@@ -1025,7 +1025,7 @@ dependencies = [
 
 [[package]]
 name = "py-engeom"
-version = "0.2.10"
+version = "0.2.12"
 dependencies = [
  "engeom",
  "numpy",

{engeom-0.2.10 → engeom-0.2.12}/Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "py-engeom"
-version = "0.2.10"
+version = "0.2.12"
 edition = "2021"
 
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html

{engeom-0.2.10 → engeom-0.2.12}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: engeom
-Version: 0.2.10
+Version: 0.2.12
 Classifier: Programming Language :: Rust
 Classifier: Programming Language :: Python :: Implementation :: CPython
 Classifier: Programming Language :: Python :: Implementation :: PyPy

{engeom-0.2.10 → engeom-0.2.12}/engeom/src/geom3/mesh.rs

@@ -2,6 +2,7 @@
 //! indices into the vertex list.  This abstraction is built around the `TriMesh` type from the
 //! `parry3d` crate.
 
+mod collisions;
 mod conformal;
 mod edges;
 mod faces;
@@ -12,15 +13,14 @@ mod patches;
 mod queries;
 mod sampling;
 mod uv_mapping;
-mod collisions;
 
-pub use self::uv_mapping::UvMapping;
 pub use self::collisions::MeshCollisionSet;
-use crate::geom3::Aabb3;
+pub use self::uv_mapping::UvMapping;
+use crate::geom3::{Aabb3, IsoExtensions3};
 use crate::{Iso3, Point2, Point3, Result, SurfacePoint3, UnitVec3, Vector3};
 pub use edges::MeshEdges;
 use parry3d_f64::shape::{TriMesh, TriMeshFlags};
-use parry3d_f64::transformation;
+use parry3d_f64::{shape, transformation};
 
 #[derive(Clone)]
 pub struct Mesh {
@@ -204,32 +204,79 @@ impl Mesh {
         }
     }
 
-    pub fn create_box(width: f64, height: f64, depth: f64, is_solid: bool) -> Self {
-        let (vertices, triangles) = box_geom(width, height, depth);
+    pub fn create_cone(half_height: f64, radius: f64, steps: usize) -> Self {
+        let cone = shape::Cone::new(half_height, radius);
+        let (vertices, faces) = cone.to_trimesh(steps as u32);
+
+        Self::new(vertices, faces, true)
+    }
+
+    pub fn create_capsule(
+        p0: &Point3,
+        p1: &Point3,
+        radius: f64,
+        n_theta: usize,
+        n_phi: usize,
+    ) -> Self {
+        let capsule = shape::Capsule::new(*p0, *p1, radius);
+        let (vertices, faces) = capsule.to_trimesh(n_theta as u32, n_phi as u32);
+
+        Self::new(vertices, faces, true)
+    }
+
+    pub fn create_sphere(radius: f64, n_theta: usize, n_phi: usize) -> Self {
+        let sphere = shape::Ball::new(radius);
+        let (vertices, faces) = sphere.to_trimesh(n_theta as u32, n_phi as u32);
+
+        Self::new(vertices, faces, true)
+    }
+
+    pub fn create_box(length: f64, width: f64, height: f64, is_solid: bool) -> Self {
+        let bx = shape::Cuboid::new(Vector3::new(length / 2.0, width / 2.0, height / 2.0));
+        let (vertices, triangles) = bx.to_trimesh();
         Self::new(vertices, triangles, is_solid)
     }
 
     pub fn create_cylinder(radius: f64, height: f64, steps: usize) -> Self {
-        let mut vertices = Vec::new();
-        let mut faces: Vec<[u32; 3]> = Vec::new();
+        let cyl = shape::Cylinder::new(height / 2.0, radius);
+        let (vertices, faces) = cyl.to_trimesh(steps as u32);
+
+        Self::new(vertices, faces, true)
+    }
+
+    pub fn create_rect_beam_between(
+        p0: &Point3,
+        p1: &Point3,
+        width: f64,
+        height: f64,
+        up: &Vector3,
+    ) -> Result<Self> {
+        let v = *p1 - *p0;
+        let pc = *p0 + v / 2.0;
+        let box_geom = shape::Cuboid::new(Vector3::new(width / 2.0, height / 2.0, v.norm() / 2.0));
 
-        for i in 0..steps {
-            let angle = i as f64 * 2.0 * std::f64::consts::PI / (steps as f64);
-            let x = radius * angle.cos();
-            let y = radius * angle.sin();
+        // I think this is OK?
+        let transform = Iso3::try_from_basis_zy(&v, up, Some(pc))?;
 
-            // First vertex is i * 2
-            vertices.push(Point3::new(x, y, 0.0));
-            // Second vertex is i * 2 + 1
-            vertices.push(Point3::new(x, y, height));
+        let (vertices, faces) = box_geom.to_trimesh();
+        let mut mesh = Self::new(vertices, faces, true);
+        mesh.transform(&transform);
+        Ok(mesh)
+    }
 
-            let k = (i + 1) % steps;
+    pub fn create_cylinder_between(p0: &Point3, p1: &Point3, radius: f64, steps: usize) -> Self {
+        let v = *p1 - *p0;
+        let pc = *p0 + v / 2.0;
+        let cyl = shape::Cylinder::new(v.norm() / 2.0, radius);
 
-            faces.push([(i * 2) as u32, (i * 2 + 1) as u32, (k * 2 + 1) as u32]);
-            faces.push([(i * 2) as u32, (k * 2) as u32, (k * 2 + 1) as u32]);
-        }
+        // I think this is OK?
+        let transform = Iso3::try_from_basis_yz(&v, &Vector3::z(), Some(pc))
+            .unwrap_or(Iso3::try_from_basis_yx(&v, &Vector3::x(), Some(pc)).unwrap());
 
-        Mesh::new(vertices, faces, false)
+        let (vertices, faces) = cyl.to_trimesh(steps as u32);
+        let mut mesh = Self::new(vertices, faces, true);
+        mesh.transform(&transform);
+        mesh
     }
 
     pub fn get_patches(&self) -> Vec<Vec<usize>> {

engeom-0.2.12/engeom/src/geom3/xyzwpr.rs

@@ -0,0 +1,141 @@
+use serde::{Deserialize, Serialize};
+use crate::{Iso3, Vector3};
+use crate::na::{Translation3, UnitQuaternion};
+
+/// A struct representing a 6D pose in XYZ and WPR (Yaw, Pitch, Roll) format, commonly used by
+/// FANUC robots. Angles are represented in degrees.
+#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
+pub struct XyzWpr {
+    pub x: f64,
+    pub y: f64,
+    pub z: f64,
+    pub w: f64,
+    pub p: f64,
+    pub r: f64,
+}
+
+impl XyzWpr {
+    pub fn new(x: f64, y: f64, z: f64, w: f64, p: f64, r: f64) -> Self {
+        XyzWpr { x, y, z, w, p, r }
+    }
+
+    pub fn from_isometry(isometry: &Iso3) -> Self {
+        let translation = isometry.translation.vector;
+        let rotation = isometry.rotation;
+        let (r, p, w) = rotation.euler_angles();
+        XyzWpr::new(
+            translation.x,
+            translation.y,
+            translation.z,
+            w.to_degrees(),
+            p.to_degrees(),
+            r.to_degrees(),
+        )
+    }
+
+    pub fn to_isometry(&self) -> Iso3 {
+        let translation = Vector3::new(self.x, self.y, self.z);
+        let rotation = UnitQuaternion::from_euler_angles(
+            self.r.to_radians(),
+            self.p.to_radians(),
+            self.w.to_radians(),
+        );
+        Iso3::from_parts(Translation3::from(translation), rotation)
+    }
+
+    pub fn approx_eq(&self, other: &XyzWpr, epsilon: f64) -> bool {
+        let m0 = self.to_isometry().to_matrix();
+        let m1 = other.to_isometry().to_matrix();
+        let diff = m0 - m1;
+        diff.amax() < epsilon
+    }
+}
+
+impl std::fmt::Display for XyzWpr {
+    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
+        write!(
+            f,
+            "XyzWpr {{ x: {}, y: {}, z: {}, w: {}, p: {}, r: {} }}",
+            self.x, self.y, self.z, self.w, self.p, self.r
+        )
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::na::{try_convert, Matrix4};
+    use approx::assert_relative_eq;
+    use test_case::test_case;
+
+    #[test_case((-0.8156824504, -0.5743236360, -0.0693866068, -413.8635232282, 0.0004524620, 0.1193088953, -0.9928570807, 263.1863811434, 0.5784997281, -0.8098874913, -0.0970583124, 291.9820746748, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-413.8635232282, 263.1863811434, 291.9820746748, -96.8338333769, -35.3450905433, 179.9682178248))]
+    #[test_case((-0.1500516217, 0.5455385155, -0.8245436550, -149.1668440821, 0.9459952737, -0.1631791693, -0.2801169415, 59.3543506644, -0.2873629291, -0.8220464019, -0.4915915582, 543.2838251943, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-149.1668440821, 59.3543506644, 543.2838251943, -120.8798246395, 16.7001441168, 99.0130388728))]
+    #[test_case((0.5009445358, 0.6825401810, 0.5321592557, 31.0642288750, -0.8639774486, 0.3581642754, 0.3539227601, -26.7743169615, 0.0509660705, -0.6370692687, 0.7691197609, 37.9006405971, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (31.0642288750, -26.7743169615, 37.9006405971, -39.6352782596, -2.9214064154, -59.8943255222))]
+    #[test_case((0.9089103503, -0.4069146210, 0.0911178708, 33.5138781088, -0.0854907806, 0.0320310501, 0.9958239494, 96.1164814542, -0.4081339260, -0.9129044326, -0.0056740956, -300.9032245106, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (33.5138781088, 96.1164814542, -300.9032245106, -90.3561134473, 24.0876647389, -5.3733485621))]
+    #[test_case((-0.1653518888, 0.9687851465, -0.1847000075, 77.8658952620, 0.9532759246, 0.2050088803, 0.2218927005, 100.2923343848, 0.2528314941, -0.1393796933, -0.9574181619, -127.6541936118, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (77.8658952620, 100.2923343848, -127.6541936118, -171.7171410357, -14.6451288996, 99.8404139441))]
+    #[test_case((-0.4118951499, 0.3279601759, -0.8501673415, -70.6389144769, -0.3403719856, -0.9208299694, -0.1903131075, 22.3217390074, -0.8452746873, 0.2109841002, 0.4909138545, 18.1798498130, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-70.6389144769, 22.3217390074, 18.1798498130, 23.2569815201, 57.7013782986, -140.4311743532))]
+    #[test_case((-0.7479917678, -0.6191725989, -0.2390263753, 604.2872268462, -0.4057545090, 0.1415967662, 0.9029471935, -20.9333741686, -0.5252347986, 0.7723830970, -0.3571452893, 30.4479677368, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (604.2872268462, -20.9333741686, 30.4479677368, 114.8155367373, 31.6840511268, -151.5220050073))]
+    #[test_case((0.4873440333, 0.6172398073, 0.6176656163, 77.4016768659, 0.3323406191, 0.5230063131, -0.7848656633, -28.4720503689, -0.8074933474, 0.5877749712, 0.0497501464, -19.7908480100, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (77.4016768659, -28.4720503689, -19.7908480100, 85.1619317507, 53.8517426985, 34.2917400153))]
+    #[test_case((0.0833272687, 0.6836449300, 0.7250421891, 471.9320306822, -0.8519144952, -0.3286045615, 0.4077508246, -102.6726730677, 0.5170089546, -0.6516507130, 0.5550253049, 588.5120154719, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (471.9320306822, -102.6726730677, 588.5120154719, -49.5782300627, -31.1318310910, -84.4135682965))]
+    #[test_case((-0.2510504585, 0.9157805028, -0.3135597837, -577.6445766741, -0.0345374583, -0.3322023607, -0.9425755967, -216.2517731052, -0.9673576543, -0.2258044778, 0.1150282859, 240.9460890915, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-577.6445766741, -216.2517731052, 240.9460890915, -63.0050045001, 75.3203125033, -172.1668871906))]
+    #[test_case((0.1791056335, -0.5890186234, -0.7880217213, 411.6803610302, 0.6839325663, -0.5012295385, 0.5300992308, 331.0434986878, -0.7072180829, -0.6338974767, 0.3130759848, -361.9951518329, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (411.6803610302, 331.0434986878, -361.9951518329, -63.7156683382, 45.0090193165, 75.3251405039))]
+    #[test_case((-0.7389487926, -0.6540675080, 0.1617107816, 5.4890201453, -0.6393453686, 0.7564348576, 0.1379992968, -26.5744088848, -0.2125845282, -0.0014146255, -0.9771416567, -99.6628429743, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (5.4890201453, -26.5744088848, -99.6628429743, -179.9170519309, 12.2738553488, -139.1333357015))]
+    #[test_case((-0.2037395422, 0.6790363200, -0.7052658187, -1.7434467919, -0.8508250650, -0.4791894336, -0.2155787453, -14.7633944063, -0.4843417261, 0.5561359212, 0.6753709570, -79.9344894886, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-1.7434467919, -14.7633944063, -79.9344894886, 39.4698348110, 28.9693530939, -103.4665369616))]
+    #[test_case((0.7230637290, 0.1432361537, -0.6757678952, -49.7118498703, 0.5101470443, 0.5488670300, 0.6621895322, 20.3765636776, 0.4657561992, -0.8235462268, 0.3237943410, 8.4422995188, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-49.7118498703, 20.3765636776, 8.4422995188, -68.5366961539, -27.7591729450, 35.2043221840))]
+    #[test_case((0.9572590453, -0.2498925755, 0.1456324858, 59.5794643289, -0.2886479285, -0.7934076632, 0.5358979878, 6.7548483372, -0.0183709982, -0.5550297115, -0.8316276365, -88.8341095469, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (59.5794643289, 6.7548483372, -88.8341095469, -146.2807856754, 1.0526398756, -16.7799423973))]
+    fn xyzwpr_to_isometry(
+        mf: (
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+            f64,
+        ),
+        pf: (f64, f64, f64, f64, f64, f64),
+    ) {
+        let m = Matrix4::new(
+            mf.0, mf.1, mf.2, mf.3, mf.4, mf.5, mf.6, mf.7, mf.8, mf.9, mf.10, mf.11, mf.12, mf.13,
+            mf.14, mf.15,
+        );
+        let p = XyzWpr::new(pf.0, pf.1, pf.2, pf.5, pf.4, pf.3);
+        let t = p.to_isometry();
+        let m_ = t.to_matrix();
+        assert_relative_eq!(m, m_, epsilon = 1e-5);
+    }
+
+    #[test_case((213.3455874397, -541.8828362001, -95.1324602951, -164.2348619634, 24.0489557219, -170.7519141710))]
+    #[test_case((70.6318465112, 113.6560228546, 346.7855689786, -26.3445451403, -30.6825741682, -92.3381779921))]
+    #[test_case((-18.5208462508, 554.3972482202, 531.8505273397, 24.9359848528, -20.6580074641, -118.2163130322))]
+    #[test_case((-279.4908707038, 107.0379921066, 347.6153730196, 141.6846980303, 42.9511504078, 21.8352677126))]
+    #[test_case((61.7823486174, 39.0960520469, 7.1731831514, 63.6707920489, 28.5547935126, 65.2651276746))]
+    #[test_case((17.8583707934, -103.5277608846, -41.9211021563, -129.0763419754, -64.3075695674, -119.6216242866))]
+    #[test_case((381.5649351646, -118.4073378481, 86.8318954547, -69.6420826640, 50.3520931676, -113.9445091815))]
+    #[test_case((-46.1766927642, -55.8019771470, -84.1930750980, -148.4720043329, 11.1136514360, 139.1677460882))]
+    #[test_case((222.2302724845, -372.5432843996, 266.9542861389, 88.4476440066, 35.3943355124, -166.3788546777))]
+    #[test_case((459.8055541953, 401.5061123248, 180.9052529976, 121.5295678361, -30.3457576176, 66.0945383908))]
+    #[test_case((-68.9503102461, -79.4547936268, -62.0503091459, 121.1860503955, 5.5438249089, -33.3572843285))]
+    fn xyzwpr_from_isometry(pf: (f64, f64, f64, f64, f64, f64)) {
+        let p = XyzWpr::new(pf.0, pf.1, pf.2, pf.5, pf.4, pf.3);
+        let i = p.to_isometry();
+
+        let p_ = XyzWpr::from_isometry(&i);
+
+        assert_relative_eq!(p.x, p_.x, epsilon = 1e-5);
+        assert_relative_eq!(p.y, p_.y, epsilon = 1e-5);
+        assert_relative_eq!(p.z, p_.z, epsilon = 1e-5);
+        assert_relative_eq!(p.w, p_.w, epsilon = 1e-5);
+        assert_relative_eq!(p.p, p_.p, epsilon = 1e-5);
+        assert_relative_eq!(p.r, p_.r, epsilon = 1e-5);
+    }
+}

{engeom-0.2.10 → engeom-0.2.12}/engeom/src/geom3.rs

@@ -4,6 +4,7 @@ pub mod mesh;
 mod plane3;
 mod point_cloud;
 mod iso3;
+mod xyzwpr;
 
 use parry3d_f64::na::UnitQuaternion;
 
@@ -17,6 +18,7 @@ pub use mesh::{Mesh, UvMapping, MeshCollisionSet};
 pub use plane3::Plane3;
 pub use point_cloud::{PointCloud, PointCloudFeatures, PointCloudKdTree};
 pub use iso3::IsoExtensions3;
+pub use xyzwpr::XyzWpr;
 
 pub type Point3 = parry3d_f64::na::Point3<f64>;
 pub type Vector3 = parry3d_f64::na::Vector3<f64>;

{engeom-0.2.10 → engeom-0.2.12}/engeom/src/lib.rs

@@ -15,6 +15,10 @@ pub mod sensor;
 
 pub type Result<T> = std::result::Result<T, Box<dyn Error>>;
 
+// Re-export parry and nalgebra
+pub use parry3d_f64::na as na;
+pub use parry3d_f64 as parry3d;
+
 // Common one dimensional functions
 pub use func1::{Func1, Gaussian1, Line1, Polynomial, Series1};
 

{engeom-0.2.10 → engeom-0.2.12}/python/engeom/geom2.pyi

@@ -138,6 +138,22 @@ class Vector2(Iterable[float]):
         """
         ...
 
+    def with_x(self, x: float) -> Vector2:
+        """
+        Return a new vector with the same y component as this vector, but with the x component set to the given value.
+        :param x: the new x component of the vector.
+        :return: a new vector with the same y component as this vector, but with the x component set to the given value.
+        """
+        ...
+
+    def with_y(self, y: float) -> Vector2:
+        """
+        Return a new vector with the same x component as this vector, but with the y component set to the given value.
+        :param y: the new y component of the vector.
+        :return: a new vector with the same x component as this vector, but with the y component set to the given value.
+        """
+        ...
+
 
 class Point2(Iterable[float]):
     """
@@ -243,6 +259,22 @@ class Point2(Iterable[float]):
         """
         ...
 
+    def with_x(self, x: float) -> Point2:
+        """
+        Return a new point with the same y component as this point, but with the x component set to the given value.
+        :param x: the new x component of the point.
+        :return: a new point with the same y component as this point, but with the x component set to the given value.
+        """
+        ...
+
+    def with_y(self, y: float) -> Point2:
+        """
+        Return a new point with the same x component as this point, but with the y component set to the given value.
+        :param y: the new y component of the point.
+        :return: a new point with the same x component as this point, but with the y component set to the given value.
+        """
+        ...
+
 
 class SurfacePoint2:
     """
@@ -950,6 +982,43 @@ class Circle2:
         """
         ...
 
+    @staticmethod
+    def fitting(points: NDArray[float], guess: Circle2 | None = None, sigma: float | None = None) -> Circle2:
+        """
+        Fit a circle to a set of points using an unconstrained Levenberg-Marquardt minimization of the sum of
+        squared errors between the points and the boundary of the circle.
+
+        The initial guess is used to provide a starting point for the optimization. If no guess is provided, the
+        unit circle will be used.
+
+        The sigma parameter is used to weight the points in the optimization. If no sigma is provided, all points
+        will be weighted equally, otherwise points beyond `sigma` standard deviations from the mean will be
+        assigned a weight of 0.0.
+        :param points: the points to fit the circle to.
+        :param guess: an optional initial guess for the circle. If None, the unit circle will be used.
+        :param sigma: an optional standard deviation to use for weighting the points. If None, all points will be
+        weighted equally.
+        :return: a new `Circle2` object representing the fitted circle.
+        """
+        ...
+
+    @staticmethod
+    def ransac(points: NDArray[float], tol: float, iterations: int | None = None, min_r: float | None = None,
+               max_r: float | None = None) -> Circle2:
+        """
+        Fit a circle to a set of points using the RANSAC algorithm. The algorithm will randomly sample points from the
+        input set and fit a circle to them, then check how many points are within the given tolerance of the fitted
+        circle. The best fitting circle will be returned.
+
+        :param points: the points to fit the circle to.
+        :param tol: the tolerance for the RANSAC algorithm.
+        :param iterations: the number of iterations to run. If None, a default value of 500 will be used.
+        :param min_r: the minimum radius of the circle. If None, no minimum will be enforced.
+        :param max_r: the maximum radius of the circle. If None, no maximum will be enforced.
+        :return: a new `Circle2` object representing the fitted circle.
+        """
+        ...
+
 
 class Arc2:
     """
@@ -1148,3 +1217,11 @@ class Aabb2:
         :return: a new AABB object with the shrunk bounds.
         """
         ...
+
+    def merged(self, other: Aabb2) -> Aabb2:
+        """
+        Merge this AABB with another AABB and return a new AABB.
+        :param other: the other AABB to merge with.
+        :return: a new AABB object that is the result of merging this AABB with the other AABB.
+        """
+        ...