ode_base/ode/
prim.rs

1//! prim
2//!
3
4use crate::ode::*;
5
6impl dSurfaceParameters {
7
8/// binding construct dSurfaceParameters
9pub fn new() -> dSurfaceParameters {
10  dSurfaceParameters{
11    mode: 0, // c_int
12    mu: 0.0, // dReal
13    mu2: 0.0, // dReal
14    rho: 0.0, // dReal
15    rho2: 0.0, // dReal
16    rhoN: 0.0, // dReal
17    bounce: 0.0, // dReal
18    bounce_vel: 0.0, // dReal
19    soft_erp: 0.0, // dReal
20    soft_cfm: 0.0, // dReal
21    motion1: 0.0, // dReal
22    motion2: 0.0, // dReal
23    motionN: 0.0, // dReal
24    slip1: 0.0, // dReal
25    slip2: 0.0} // dReal
26}
27
28}
29
30impl dContactGeom {
31
32/// binding construct dContactGeom
33pub fn new() -> dContactGeom {
34  dContactGeom{
35    pos: [0.0; 4], // dVector3
36    normal: [0.0; 4], // dVector3
37    depth: 0.0, // dReal
38    g1: 0 as dGeomID,
39    g2: 0 as dGeomID,
40    side1: 0, // c_int
41    side2: 0} // c_int
42}
43
44}
45
46impl dContact {
47
48/// binding construct dContact
49pub fn new() -> dContact {
50  dContact{
51    surface: dSurfaceParameters::new(),
52    geom: dContactGeom::new(),
53    fdir1: [0.0; 4]} // dVector3
54}
55
56}
57
58/// static angle 180
59pub use std::f64::consts::PI;
60/// static angle 360 dual
61pub static PId: dReal = PI * 2.0;
62/// static angle 90 half
63pub static PIh: dReal = PI / 2.0;
64/// static angle 60 regular triangle
65pub static PIt: dReal = PI / 3.0;
66/// static angle 45 quarter
67pub static PIq: dReal = PI / 4.0;
68/// static angle 30 a sixth
69pub static PIx: dReal = PI / 6.0;
70
71/// static angle 270
72pub static PIh3: dReal = PIh * 3.0;
73
74/// static angle 120
75pub static PIt2: dReal = PIt * 2.0;
76/// static angle 240
77pub static PIt4: dReal = PIt * 4.0;
78/// static angle 300
79pub static PIt5: dReal = PIt * 5.0;
80
81/// static angle 135
82pub static PIq3: dReal = PIq * 3.0;
83
84/// static angle 150
85pub static PIx5: dReal = PIx * 5.0;
86
87/// static dMatrix4 Identity
88pub static M4I: dMatrix4 = [
89  1.0, 0.0, 0.0, 0.0,
90  0.0, 1.0, 0.0, 0.0,
91  0.0, 0.0, 1.0, 0.0,
92  0.0, 0.0, 0.0, 1.0];
93
94/// static dMatrix3 Identity
95pub static M3I: dMatrix3 = [
96  1.0, 0.0, 0.0, 0.0,
97  0.0, 1.0, 0.0, 0.0,
98  0.0, 0.0, 1.0, 0.0];
99
100/// static dQuaternion Identity
101pub static QI: dQuaternion = [1.0, 0.0, 0.0, 0.0];
102
103/// constructor and converter for primitive type
104pub trait Quaternion {
105  /// ptr mut of dQuaternion
106  fn as_ptr_mut(&mut self) -> *mut dReal;
107  /// ptr of dQuaternion (use for converter)
108  fn as_ptr(&self) -> *const dReal;
109
110  /// construct as Identity
111  fn new() -> dQuaternion {
112    let mut q: dQuaternion = [0.0; 4];
113unsafe {
114    dQSetIdentity(q.as_ptr_mut());
115}
116    q
117  }
118
119  /// constructor (converter)
120  fn from_R(m: dMatrix3) -> dQuaternion {
121    let mut q: dQuaternion = [0.0; 4];
122unsafe {
123    dQfromR(q.as_ptr_mut(), m.as_ptr() as *mut dReal);
124}
125    q
126  }
127
128  /// converter (like as dMatrix3::from_Q(*self))
129  fn to_R(&self) -> dMatrix3 {
130    let mut m: dMatrix3 = [0.0; 12];
131unsafe {
132    dRfromQ(m.as_ptr_mut(), self.as_ptr() as *mut dReal);
133}
134    m
135  }
136
137  /// constructor
138  /// axis [x, y, z] should be norm == 1, angle is theta radian
139  /// Q(axis, angle) == [cos(t/2), xsin(t/2), ysin(t/2), zsin(t/2)]
140  fn from_axis_and_angle(axis: [dReal; 3], angle: dReal) -> dQuaternion {
141    let mut q: dQuaternion = [0.0; 4];
142unsafe {
143    dQFromAxisAndAngle(q.as_ptr_mut(), axis[0], axis[1], axis[2], angle);
144}
145    q
146  }
147
148  /// constructor multiply p: dQuaternion and q: dQuaternion
149  fn multiply0(p: dQuaternion, q: dQuaternion) -> dQuaternion {
150    dQuaternion::multiply0_pp(p.as_ptr(), q.as_ptr())
151  }
152
153  /// constructor multiply p: dQuaternion pointer and q: dQuaternion pointer
154  fn multiply0_pp(p: *const dReal, q: *const dReal) -> dQuaternion {
155    let mut o: dQuaternion = [0.0; 4];
156unsafe {
157    dQMultiply0(o.as_ptr_mut(), p as *mut dReal, q as *mut dReal);
158}
159    o
160  }
161
162  /// constructor multiply m: dMatrix3 and v: dVector3
163  /// dVector3::multiply0_331 is defined as dQuaternion::multiply0_331
164  fn multiply0_331(m: dMatrix3, v: dVector3) -> dVector3 {
165    dVector3::multiply0_331_pp(m.as_ptr(), v.as_ptr())
166  }
167
168  /// constructor multiply m: dMatrix3 pointer and v: dVector3 pointer
169  /// dVector3::multiply0_331_pp is defined as dQuaternion::multiply0_331_pp
170  fn multiply0_331_pp(m: *const dReal, v: *const dReal) -> dVector3 {
171    let mut o: dVector3 = [0.0; 4];
172unsafe {
173    dMULTIPLY0_331(o.as_ptr_mut(), m, v);
174}
175    o
176  }
177
178  /// constructor multiply m: dMatrix4 and v: dVector4
179  /// dVector4::multiply0_441 is defined as dQuaternion::multiply0_441
180  fn multiply0_441(m: dMatrix4, v: dVector4) -> dVector4 {
181    dVector4::multiply0_441_pp(m.as_ptr(), v.as_ptr())
182  }
183
184  /// constructor multiply m: dMatrix4 pointer and v: dVector4 pointer
185  /// dVector4::multiply0_441_pp is defined as dQuaternion::multiply0_441_pp
186  fn multiply0_441_pp(m: *const dReal, v: *const dReal) -> dVector4 {
187    let mut o: dVector4 = [0.0; 4];
188unsafe {
189    dMULTIPLY0_441(o.as_ptr_mut(), m, v);
190}
191    o
192  }
193
194  /// conjugate dQuaternion
195  fn conjugate(q: dQuaternion) -> dQuaternion {
196    dQuaternion::conjugate_ptr(q.as_ptr())
197  }
198
199  /// conjugate dQuaternion pointer
200  fn conjugate_ptr(q: *const dReal) -> dQuaternion {
201unsafe {
202    let q = std::slice::from_raw_parts(q, 4);
203    [q[0], -q[1], -q[2], -q[3]]
204}
205  }
206
207  /// check equal with precision
208  fn prec_eq(&self, e: dReal, q: dQuaternion) -> bool;
209
210  /// for Debug
211  fn as_vec(&self) -> ODEMat;
212}
213
214impl Quaternion for dQuaternion {
215  /// ptr mut of dQuaternion
216  fn as_ptr_mut(&mut self) -> *mut dReal { &mut (*self)[0] as *mut dReal }
217  /// ptr of dQuaternion (use for converter)
218  fn as_ptr(&self) -> *const dReal { &(*self)[0] as *const dReal }
219
220  /// check equal with precision
221  fn prec_eq(&self, e: dReal, q: dQuaternion) -> bool {
222    for i in 0..4 {
223      if (self[i] - q[i]).abs() >= e { return false; }
224    }
225    true
226  }
227
228  /// for Debug
229  fn as_vec(&self) -> ODEMat {
230    ODEMat::from_Q(self.as_ptr())
231  }
232}
233
234/// constructor and converter for primitive type
235pub trait Matrix3 {
236  /// ptr mut of dMatrix3
237  fn as_ptr_mut(&mut self) -> *mut dReal;
238  /// ptr of dMatrix3 (use for converter)
239  fn as_ptr(&self) -> *const dReal;
240
241  /// construct as Identity
242  fn new() -> dMatrix3 {
243    let mut m: dMatrix3 = [0.0; 12];
244unsafe {
245    dRSetIdentity(m.as_ptr_mut());
246}
247    m
248  }
249
250  /// constructor (transpose illegular order)
251  fn t(m: dMatrix3) -> dMatrix3 {
252    dMatrix3::t_ptr(m.as_ptr())
253  }
254
255  /// constructor (transpose illegular order) pointer
256  fn t_ptr(m: *const dReal) -> dMatrix3 {
257unsafe {
258    let m = std::slice::from_raw_parts(m, 12);
259    [m[0], m[4], m[8], m[3],
260     m[1], m[5], m[9], m[7],
261     m[2], m[6], m[10], m[11]]
262}
263  }
264
265  /// constructor (converter)
266  fn from_Q(q: dQuaternion) -> dMatrix3 {
267    let mut m: dMatrix3 = [0.0; 12];
268unsafe {
269    dRfromQ(m.as_ptr_mut(), q.as_ptr() as *mut dReal);
270}
271    m
272  }
273
274  /// converter (like as dQuaternion::from_R(*self))
275  fn to_Q(&self) -> dQuaternion {
276    let mut q: dQuaternion = [0.0; 4];
277unsafe {
278    dQfromR(q.as_ptr_mut(), self.as_ptr() as *mut dReal);
279}
280    q
281  }
282
283  /// constructor
284  fn from_axis_and_angle(axis: [dReal; 3], angle: dReal) -> dMatrix3 {
285    let mut m: dMatrix3 = [0.0; 12];
286unsafe {
287    dRFromAxisAndAngle(m.as_ptr_mut(), axis[0], axis[1], axis[2], angle);
288}
289    m
290  }
291
292  /// constructor
293  fn from_euler_angles(phi: dReal, theta: dReal, psi: dReal) -> dMatrix3 {
294    let mut m: dMatrix3 = [0.0; 12];
295unsafe {
296    dRFromEulerAngles(m.as_ptr_mut(), phi, theta, psi);
297}
298    m
299  }
300
301  /// constructor
302  fn from_2_axes(e0: [dReal; 3], e1: [dReal; 3]) -> dMatrix3 {
303    let mut m: dMatrix3 = [0.0; 12];
304unsafe {
305    dRFrom2Axes(m.as_ptr_mut(), e0[0], e0[1], e0[2], e1[0], e1[1], e1[2]);
306}
307    m
308  }
309
310  /// constructor
311  fn from_z_axis(e: [dReal; 3]) -> dMatrix3 {
312    let mut m: dMatrix3 = [0.0; 12];
313unsafe {
314    dRFromZAxis(m.as_ptr_mut(), e[0], e[1], e[2]);
315}
316    m
317  }
318
319  /// constructor multiply a: dMatrix3 and b: dMatrix3
320  fn multiply0_333(a: dMatrix3, b: dMatrix3) -> dMatrix3 {
321    dMatrix3::multiply0_333_pp(a.as_ptr(), b.as_ptr())
322  }
323
324  /// constructor multiply a: dMatrix3 pointer and b: dMatrix3 pointer
325  fn multiply0_333_pp(a: *const dReal, b: *const dReal) -> dMatrix3 {
326    let mut m: dMatrix3 = [0.0; 12];
327unsafe {
328    dMULTIPLY0_333(m.as_ptr_mut(), a, b);
329}
330    m
331  }
332
333  /// check equal with precision
334  fn prec_eq(&self, e: dReal, m: dMatrix3) -> bool;
335
336  /// for Debug
337  fn as_mat(&self) -> ODEMat;
338}
339
340impl Matrix3 for dMatrix3 {
341  /// ptr mut of dMatrix3
342  fn as_ptr_mut(&mut self) -> *mut dReal { &mut (*self)[0] as *mut dReal }
343  /// ptr of dMatrix3 (use for converter)
344  fn as_ptr(&self) -> *const dReal { &(*self)[0] as *const dReal }
345
346  /// check equal with precision
347  fn prec_eq(&self, e: dReal, m: dMatrix3) -> bool {
348    for i in 0..12 {
349      if (self[i] - m[i]).abs() >= e { return false; }
350    }
351    true
352  }
353
354  /// for Debug
355  fn as_mat(&self) -> ODEMat {
356    ODEMat::from_Mat3(self.as_ptr())
357  }
358}
359
360/// constructor and converter for primitive type
361pub trait Matrix4 {
362  /// ptr mut of dMatrix4
363  fn as_ptr_mut(&mut self) -> *mut dReal;
364  /// ptr of dMatrix4 (use for converter)
365  fn as_ptr(&self) -> *const dReal;
366
367  /// construct as Identity
368  fn new() -> dMatrix4 {
369    let mut m: dMatrix4 = [0.0; 16];
370    for i in 0..4 { m[i * 5] = 1.0; }
371    m
372  }
373
374  /// constructor (transpose)
375  fn t(m: dMatrix4) -> dMatrix4 {
376    dMatrix4::t_ptr(m.as_ptr())
377  }
378
379  /// constructor (transpose) pointer
380  fn t_ptr(m: *const dReal) -> dMatrix4 {
381unsafe {
382    let m = std::slice::from_raw_parts(m, 16);
383    [m[0], m[4], m[8], m[12],
384     m[1], m[5], m[9], m[13],
385     m[2], m[6], m[10], m[14],
386     m[3], m[7], m[11], m[15]]
387}
388  }
389
390  /// constructor (Quaternion conjugate)
391  fn q_conjugate(m: dMatrix4) -> dMatrix4 {
392    dMatrix4::q_conjugate_ptr(m.as_ptr())
393  }
394
395  /// constructor (Quaternion conjugate) pointer
396  fn q_conjugate_ptr(m: *const dReal) -> dMatrix4 {
397unsafe {
398    let m = std::slice::from_raw_parts(m, 16);
399    [m[0], -m[1], -m[2], -m[3],
400     -m[4], m[5], -m[6], -m[7],
401     -m[8], -m[9], m[10], -m[11],
402     -m[12], -m[13], -m[14], m[15]]
403}
404  }
405
406  /// constructor (converter) q: dQuaternion
407  fn from_Conjugate_Q(q: dQuaternion) -> dMatrix4 {
408    dMatrix4::from_Q(dQuaternion::conjugate(q))
409  }
410
411  /// constructor (converter) q: dQuaternion pointer
412  fn from_Conjugate_Q_ptr(q: *const dReal) -> dMatrix4 {
413    dMatrix4::from_Q(dQuaternion::conjugate_ptr(q))
414  }
415
416  /// constructor (converter) p: dQuaternion
417  fn from_P(p: dQuaternion) -> dMatrix4 {
418    dMatrix4::from_P_ptr(p.as_ptr())
419  }
420
421  /// constructor (converter) p: dQuaternion pointer
422  fn from_P_ptr(p: *const dReal) -> dMatrix4 {
423unsafe {
424    let p = std::slice::from_raw_parts(p, 4);
425    [ p[0], -p[1], -p[2], -p[3],
426      p[1],  p[0],  p[3], -p[2],
427      p[2], -p[3],  p[0],  p[1],
428      p[3],  p[2], -p[1],  p[0]]
429}
430  }
431
432  /// constructor (converter) q: dQuaternion
433  fn from_Q(q: dQuaternion) -> dMatrix4 {
434    dMatrix4::from_Q_ptr(q.as_ptr())
435  }
436
437  /// constructor (converter) q: dQuaternion pointer
438  fn from_Q_ptr(q: *const dReal) -> dMatrix4 {
439unsafe {
440    let q = std::slice::from_raw_parts(q, 4);
441    [ q[0], -q[1], -q[2], -q[3],
442      q[1],  q[0], -q[3],  q[2],
443      q[2],  q[3],  q[0], -q[1],
444      q[3], -q[2],  q[1],  q[0]]
445}
446  }
447
448  /// constructor multiply a: dMatrix4 and b: dMatrix4
449  fn multiply0_444(a: dMatrix4, b: dMatrix4) -> dMatrix4 {
450    dMatrix4::multiply0_444_pp(a.as_ptr(), b.as_ptr())
451  }
452
453  /// constructor multiply a: dMatrix4 pointer and b: dMatrix4 pointer
454  fn multiply0_444_pp(a: *const dReal, b: *const dReal) -> dMatrix4 {
455    let mut m: dMatrix4 = [0.0; 16];
456unsafe {
457    dMULTIPLY0_444(m.as_ptr_mut(), a, b);
458}
459    m
460  }
461
462  /// check dMatrix4 is dQuaternion
463  fn is_quaternion(&self) -> bool;
464
465  /// to dQuatrenion (check is_quaternion() before)
466  fn to_Q(&self) -> dQuaternion;
467
468  /// check equal with precision
469  fn prec_eq(&self, e: dReal, m: dMatrix4) -> bool;
470
471  /// for Debug
472  fn as_mat(&self) -> ODEMat;
473}
474
475impl Matrix4 for dMatrix4 {
476  /// ptr mut of dMatrix4
477  fn as_ptr_mut(&mut self) -> *mut dReal { &mut (*self)[0] as *mut dReal }
478  /// ptr of dMatrix4 (use for converter)
479  fn as_ptr(&self) -> *const dReal { &(*self)[0] as *const dReal }
480
481  /// check dMatrix4 is dQuaternion
482  fn is_quaternion(&self) -> bool {
483    if self[0] != self[5] || self[0] != self[10] || self[0] != self[15] {
484      return false;
485    }
486    if (self[4] != self[14] || self[8] != self[7] || self[12] != self[9])
487    && (self[4] != self[11] || self[8] != self[13] || self[12] != self[6]) {
488      return false;
489    }
490    dMatrix4::t(dMatrix4::q_conjugate(*self)) == *self
491  }
492
493  /// to dQuatrenion (check is_quaternion() before)
494  fn to_Q(&self) -> dQuaternion {
495    if !self.is_quaternion() { panic!("not quaternion"); }
496    [self[0], self[4], self[8], self[12]]
497  }
498
499  /// check equal with precision
500  fn prec_eq(&self, e: dReal, m: dMatrix4) -> bool {
501    for i in 0..16 {
502      if (self[i] - m[i]).abs() >= e { return false; }
503    }
504    true
505  }
506
507  /// for Debug
508  fn as_mat(&self) -> ODEMat {
509    ODEMat::from_Mat4(self.as_ptr())
510  }
511}
512
513impl dMass {
514
515/// binding construct dMass (as dMassSetZero)
516pub fn new() -> dMass {
517  let mut mass: dMass = dMass{
518    mass: 0.0,
519    c: [0.0; 4], // dVector3
520    I: [0.0; 12]}; // dMatrix3
521  unsafe { dMassSetZero(&mut mass); } // may be needless
522  mass
523}
524
525}
ode_base/ode/prim.rs

ode_base/ode/
prim.rs
