engeom 0.2.10__tar.gz → 0.2.11__tar.gz

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

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

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

@@ -1,6 +1,6 @@
 [package]
 name = "py-engeom"
-version = "0.2.10"
+version = "0.2.11"
 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.11}/PKG-INFO

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

{engeom-0.2.10 → engeom-0.2.11}/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.11/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.11}/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.11}/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.11}/python/engeom/geom3.pyi

@@ -461,6 +461,29 @@ class Iso3:
         """
         ...
 
+    @staticmethod
+    def from_xyzwpr(x: float, y: float, z: float, w: float, p: float, r: float) -> Iso3:
+        """
+        Create an isometry from the specified translation and rotation angles in yaw, pitch, and roll format, following
+        the convention typically used in robotics. The angles are specified in degrees.
+        :param x:
+        :param y:
+        :param z:
+        :param w:
+        :param p:
+        :param r:
+        :return:
+        """
+        ...
+
+    def to_xyzwpr(self) -> List[float]:
+        """
+        Convert the isometry to a list of translation and rotation angles in yaw, pitch, and roll format, following the
+        convention typically used in robotics. The angles are returned in degrees.
+        :return: a list of 6 floats representing the translation and rotation angles.
+        """
+        ...
+
     def __matmul__(self, other: Transformable3) -> Transformable3:
         """
         Multiply another object by the isometry, transforming it and returning a new object of the same type.
@@ -537,6 +560,14 @@ class Iso3:
         """
         ...
 
+    @property
+    def origin(self) -> Point3:
+        """
+        Get the origin of the isometry as a Point3 object.
+        :return: a Point3 object representing the origin of the isometry.
+        """
+        ...
+
     def translation(self) -> Iso3:
         """
         Return the translation component of the isometry as a separate isometry.
@@ -1157,14 +1188,14 @@ class Mesh:
         ...
 
     @staticmethod
-    def create_box(width: float, height: float, depth: float) -> Mesh:
+    def create_box(length: float, width: float, height: float) -> Mesh:
         """
-        Creates a box with the corner at the origin and the specified width, height, and depth all positive.
+        Creates a box with the center at the origin and the specified length, width, and height
 
-        :param width:
-        :param height:
-        :param depth:
-        :return:
+        :param length: the size of the box along the X-axis
+        :param width: the size of the box along the Y-axis
+        :param height: the size of the box along the Z-axis
+        :return: a new `Mesh` object representing the box
         """
         ...
 
@@ -1172,14 +1203,91 @@ class Mesh:
     def create_cylinder(radius: float, height: float, steps: int) -> Mesh:
         """
         Creates a cylinder with a radius and height. The cylinder will be centered at the origin and oriented along the
-        Z-axis. The bottom of the cylinder will be at Z = 0 and the top will be at Z = height. The cylinder will be
-        created with a number of steps around the circumference, which will determine the number of vertices used to
-        create the cylinder. The number of steps should be at least 3. The first set of vertices will be at X = radius,
-        Y = 0.
-        :param radius:
-        :param height:
-        :param steps:
-        :return:
+        Y-axis.
+
+        :param radius: the radius of the cylinder
+        :param height: the size of the cylinder along the Y-axis
+        :param steps: the number of subdivisions to create vertices around the cylinder. The more steps the smoother the
+        cylinder will be.
+        :return: a new `Mesh` object representing the cylinder
+        """
+        ...
+
+    @staticmethod
+    def create_sphere(radius: float, n_theta: int, n_phi: int) -> Mesh:
+        """
+        Creates a sphere with a radius. The sphere will be centered at the origin. The step counts `n_theta` and `n_phi`
+        will determine the smoothness of the sphere in the radial (n_theta) and polar (n_phi) directions. The poles
+        will be located at Y=+radius and Y=-radius, and the equator will lie in the XZ plane.
+
+        :param radius: the radius of the sphere
+        :param n_theta: the number of subdivisions to create vertices around the sphere in the theta direction
+        :param n_phi: the number of subdivisions to create vertices around the sphere in the phi direction
+        :return: a new `Mesh` object representing the sphere
+        """
+        ...
+
+    @staticmethod
+    def create_cone(radius: float, height: float, steps: int) -> Mesh:
+        """
+        Creates a cone with a radius and height. The cone will be centered at the origin and oriented so that the
+        point of the cone is located at Y=height/2 and the base is located at Y=-height/2.
+
+        :param radius: the radius of the base of the cone
+        :param height: the size of the cone along the Y-axis
+        :param steps: the number of subdivisions to create vertices around the cone. The more steps the smoother the
+        cone will be.
+        :return: a new `Mesh` object representing the cone
+        """
+        ...
+
+    @staticmethod
+    def create_capsule(p0: Point3, p1: Point3, radius: float, n_theta: int, n_phi: int) -> Mesh:
+        """
+        Creates a capsule shape between two points with a specified radius. The capsule will be centered between the two
+        points and oriented along the line connecting them. The step counts `n_theta` and `n_phi` will determine the
+        smoothness of the sphere in the radial (n_theta) and polar (n_phi) directions.
+
+        :param p0: the first point of the capsule
+        :param p1: the second point of the capsule
+        :param radius: the radius of the capsule
+        :param n_theta: the number of subdivisions to create vertices around the sphere in the theta direction
+        :param n_phi: the number of subdivisions to create vertices around the sphere in the phi direction
+        :return: a new `Mesh` object representing the capsule
+        """
+        ...
+
+    @staticmethod
+    def create_cylinder_between(p0: Point3, p1: Point3, radius: float, steps: int) -> Mesh:
+        """
+        Creates a cylinder between two points with a specified radius. The cylinder will be centered between the two
+        points and oriented along the line connecting them.
+
+        :param p0: the first point of the cylinder
+        :param p1: the second point of the cylinder
+        :param radius: the radius of the cylinder
+        :param steps: the number of subdivisions to create vertices around the cylinder. The more steps the smoother the
+        cylinder will be.
+        :return: a new `Mesh` object representing the cylinder
+        """
+        ...
+
+    @staticmethod
+    def create_rect_beam_between(p0: Point3, p1: Point3, width: float, height: float, up: Vector3 | None = None) -> Mesh:
+        """
+        Create a rectangular cross-sectioned prism between two points with a specified width and height. The prism will
+        be centered between the two points and oriented along the line connecting them. The up vector's projection onto
+        the line connecting the two end points will determine the direction of the height of the prism. If None, the
+        height will be aligned with the projection of the Z-axis.
+
+        If the up vector is parallel to the line connecting the two points, an error will be thrown.
+
+        :param p0: the first point of the prism
+        :param p1: the second point of the prism
+        :param width: the width of the prism
+        :param height: the height of the prism
+        :param up: the up vector to use for the height direction. If None, the Z-axis will be used
+        :return: a new `Mesh` object representing the prism
         """
         ...
 
@@ -1304,6 +1412,7 @@ class MeshCollisionSet:
         """
         ...
 
+
 class CurveStation3:
     """
     A class representing a station along a curve in 3D space. The station is represented by a point on the curve, a

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

@@ -614,6 +614,74 @@ impl From<engeom::CurveStation3<'_>> for CurveStation3 {
 // Transformations
 // ================================================================================================
 
+#[pyclass]
+#[derive(Clone, Debug)]
+pub struct XyzWpr {
+    inner: engeom::geom3::XyzWpr,
+}
+
+impl XyzWpr {
+    pub fn get_inner(&self) -> &engeom::geom3::XyzWpr {
+        &self.inner
+    }
+
+    pub fn from_inner(inner: engeom::geom3::XyzWpr) -> Self {
+        Self { inner }
+    }
+}
+
+// #[pymethods]
+// impl XyzWpr {
+//     #[new]
+//     fn new(x: f64, y: f64, z: f64, w: f64, p: f64, r: f64) -> Self {
+//         Self {
+//             inner: engeom::geom3::XyzWpr::new(x, y, z, w, p, r),
+//         }
+//     }
+//
+//     #[getter]
+//     fn values(&self) -> Vec<f64> {
+//         vec![
+//             self.inner.x,
+//             self.inner.y,
+//             self.inner.z,
+//             self.inner.w,
+//             self.inner.p,
+//             self.inner.r,
+//         ]
+//     }
+//
+//     #[getter]
+//     fn x(&self) -> f64 {
+//         self.inner.x
+//     }
+//
+//     #[getter]
+//     fn y(&self) -> f64 {
+//         self.inner.y
+//     }
+//
+//     #[getter]
+//     fn z(&self) -> f64 {
+//         self.inner.z
+//     }
+//
+//     #[getter]
+//     fn w(&self) -> f64 {
+//         self.inner.w
+//     }
+//
+//     #[getter]
+//     fn p(&self) -> f64 {
+//         self.inner.p
+//     }
+//
+//     #[getter]
+//     fn r(&self) -> f64 {
+//         self.inner.r
+//     }
+// }
+
 #[derive(FromPyObject)]
 enum Transformable3 {
     Iso(Iso3),
@@ -654,6 +722,27 @@ impl Iso3 {
         )
     }
 
+    #[getter]
+    fn origin(&self) -> Point3 {
+        Point3::from_inner(engeom::Point3::new(
+            self.inner.translation.x,
+            self.inner.translation.y,
+            self.inner.translation.z,
+        ))
+    }
+
+    #[staticmethod]
+    fn from_xyzwpr(x: f64, y: f64, z: f64, w: f64, p: f64, r: f64) -> Self {
+        Self {
+            inner: engeom::geom3::XyzWpr::new(x, y, z, w, p, r).to_isometry(),
+        }
+    }
+
+    fn to_xyzwpr(&self) -> Vec<f64> {
+        let v = engeom::geom3::XyzWpr::from_isometry(&self.inner);
+        vec![v.x, v.y, v.z, v.w, v.p, v.r]
+    }
+
     #[new]
     fn new(matrix: PyReadonlyArrayDyn<'_, f64>) -> PyResult<Self> {
         if matrix.shape().len() != 2 || matrix.shape()[0] != 4 || matrix.shape()[1] != 4 {
@@ -769,13 +858,13 @@ impl Iso3 {
 
     fn translation(&self) -> Iso3 {
         Self {
-            inner: engeom::Iso3::from_parts(self.inner.translation, UnitQuaternion::identity())
+            inner: engeom::Iso3::from_parts(self.inner.translation, UnitQuaternion::identity()),
         }
     }
 
     fn rotation(&self) -> Iso3 {
         Self {
-            inner: engeom::Iso3::from_parts(Translation3::identity(), self.inner.rotation)
+            inner: engeom::Iso3::from_parts(Translation3::identity(), self.inner.rotation),
         }
     }