k/
link.rs

Help
1/*
2  Copyright 2017 Takashi Ogura
3
4  Licensed under the Apache License, Version 2.0 (the "License");
5  you may not use this file except in compliance with the License.
6  You may obtain a copy of the License at
7
8      http://www.apache.org/licenses/LICENSE-2.0
9
10  Unless required by applicable law or agreed to in writing, software
11  distributed under the License is distributed on an "AS IS" BASIS,
12  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  See the License for the specific language governing permissions and
14  limitations under the License.
15*/
16//! `link` can be used to show the shape of the robot, or collision checking libraries.
17//!
18//! `link` module is optional for `k`.
19//!
20use na::{Isometry3, Matrix3, RealField, Vector3};
21use nalgebra as na;
22
23#[derive(Debug, Clone)]
24pub enum Geometry<T: RealField> {
25    Box { depth: T, width: T, height: T },
26    Cylinder { radius: T, length: T },
27    Capsule { radius: T, length: T },
28    Sphere { radius: T },
29    Mesh { filename: String, scale: Vector3<T> },
30}
31
32#[derive(Debug, Default, Clone)]
33pub struct Color<T: RealField> {
34    pub r: T,
35    pub g: T,
36    pub b: T,
37    pub a: T,
38}
39
40#[derive(Debug, Clone)]
41pub struct Texture {
42    pub filename: String,
43}
44
45impl Default for Texture {
46    fn default() -> Texture {
47        Texture {
48            filename: "".to_string(),
49        }
50    }
51}
52
53#[derive(Debug, Default, Clone)]
54pub struct Material<T: RealField> {
55    pub name: String,
56    pub color: Color<T>,
57    pub texture: Texture,
58}
59
60#[derive(Debug, Clone)]
61pub struct Inertial<T: RealField> {
62    origin: Isometry3<T>,
63    pub mass: T,
64    pub inertia: Matrix3<T>,
65    world_transform_cache: Option<Isometry3<T>>,
66}
67
68// TODO
69impl<T> Inertial<T>
70where
71    T: RealField,
72{
73    pub fn from_mass(mass: T) -> Self {
74        Self {
75            origin: Isometry3::identity(),
76            mass,
77            inertia: Matrix3::identity(),
78            world_transform_cache: None,
79        }
80    }
81    pub fn new(origin: Isometry3<T>, mass: T, inertia: Matrix3<T>) -> Self {
82        Self {
83            origin,
84            mass,
85            inertia,
86            world_transform_cache: None,
87        }
88    }
89    pub fn set_origin(&mut self, origin: Isometry3<T>) {
90        self.origin = origin;
91        self.world_transform_cache = None;
92    }
93    pub fn origin(&self) -> &Isometry3<T> {
94        &self.origin
95    }
96    pub fn clear_world_transform(&mut self) {
97        self.world_transform_cache = None
98    }
99    pub fn set_world_transform(&mut self, trans: Isometry3<T>) {
100        self.world_transform_cache = Some(trans);
101    }
102    pub fn world_transform(&self) -> &Option<Isometry3<T>> {
103        &self.world_transform_cache
104    }
105}
106
107#[derive(Debug, Clone)]
108pub struct Visual<T: RealField> {
109    pub name: String,
110    origin: Isometry3<T>,
111    pub geometry: Geometry<T>,
112    pub material: Material<T>,
113    world_transform_cache: Option<Isometry3<T>>,
114}
115
116impl<T> Visual<T>
117where
118    T: RealField,
119{
120    pub fn new(
121        name: String,
122        origin: Isometry3<T>,
123        geometry: Geometry<T>,
124        material: Material<T>,
125    ) -> Self {
126        Self {
127            name,
128            origin,
129            geometry,
130            material,
131            world_transform_cache: None,
132        }
133    }
134    pub fn set_origin(&mut self, origin: Isometry3<T>) {
135        self.origin = origin;
136        self.world_transform_cache = None;
137    }
138    pub fn origin(&self) -> &Isometry3<T> {
139        &self.origin
140    }
141    pub fn clear_world_transform(&mut self) {
142        self.world_transform_cache = None
143    }
144    pub fn set_world_transform(&mut self, trans: Isometry3<T>) {
145        self.world_transform_cache = Some(trans);
146    }
147    pub fn world_transform(&self) -> &Option<Isometry3<T>> {
148        &self.world_transform_cache
149    }
150}
151
152#[derive(Debug, Clone)]
153pub struct Collision<T: RealField> {
154    pub name: String,
155    origin: Isometry3<T>,
156    pub geometry: Geometry<T>,
157    world_transform_cache: Option<Isometry3<T>>,
158}
159
160impl<T> Collision<T>
161where
162    T: RealField,
163{
164    pub fn new(name: String, origin: Isometry3<T>, geometry: Geometry<T>) -> Self {
165        Self {
166            name,
167            origin,
168            geometry,
169            world_transform_cache: None,
170        }
171    }
172    pub fn set_origin(&mut self, origin: Isometry3<T>) {
173        self.origin = origin;
174        self.world_transform_cache = None;
175    }
176    pub fn origin(&self) -> &Isometry3<T> {
177        &self.origin
178    }
179    pub fn clear_world_transform(&mut self) {
180        self.world_transform_cache = None
181    }
182    pub fn set_world_transform(&mut self, trans: Isometry3<T>) {
183        self.world_transform_cache = Some(trans);
184    }
185    pub fn world_transform(&self) -> &Option<Isometry3<T>> {
186        &self.world_transform_cache
187    }
188}
189
190#[derive(Debug, Clone)]
191pub struct Link<T: RealField> {
192    pub name: String,
193    pub inertial: Inertial<T>,
194    pub visuals: Vec<Visual<T>>,
195    pub collisions: Vec<Collision<T>>,
196}
197
198impl<T> Default for Link<T>
199where
200    T: RealField,
201{
202    fn default() -> Self {
203        Self {
204            name: "".to_owned(),
205            inertial: Inertial::new(Isometry3::identity(), T::zero(), Matrix3::identity()),
206            visuals: Vec::new(),
207            collisions: Vec::new(),
208        }
209    }
210}
211
212#[derive(Debug)]
213pub struct LinkBuilder<T>
214where
215    T: RealField,
216{
217    name: String,
218    inertial: Inertial<T>,
219    visuals: Vec<Visual<T>>,
220    collisions: Vec<Collision<T>>,
221}
222
223impl<T> LinkBuilder<T>
224where
225    T: RealField,
226{
227    pub fn new() -> Self {
228        Self {
229            name: "".to_owned(),
230            inertial: Inertial::new(Isometry3::identity(), T::zero(), Matrix3::identity()),
231            visuals: Vec::new(),
232            collisions: Vec::new(),
233        }
234    }
235    pub fn name(mut self, name: &str) -> Self {
236        name.clone_into(&mut self.name);
237        self
238    }
239    pub fn inertial(mut self, inertial: Inertial<T>) -> Self {
240        self.inertial = inertial;
241        self
242    }
243    pub fn add_visual(mut self, visual: Visual<T>) -> Self {
244        self.visuals.push(visual);
245        self
246    }
247    pub fn add_collision(mut self, collision: Collision<T>) -> Self {
248        self.collisions.push(collision);
249        self
250    }
251    pub fn finalize(self) -> Link<T> {
252        Link {
253            name: self.name,
254            inertial: self.inertial,
255            visuals: self.visuals,
256            collisions: self.collisions,
257        }
258    }
259}
260
261impl<T> Default for LinkBuilder<T>
262where
263    T: RealField,
264{
265    fn default() -> Self {
266        Self::new()
267    }
268}
k/link.rs

k/
link.rs
